dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btTypedConstraint.h"
  17 #include "BulletDynamics/Dynamics/btRigidBody.h"
  18 #include "LinearMath/btSerializer.h"
  19
  20 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
  21
  22 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
  23         : btTypedObject(type),
  24           m_userConstraintType(-1),
  25           m_userConstraintPtr((void*)-1),
  26           m_breakingImpulseThreshold(SIMD_INFINITY),
  27           m_isEnabled(true),
  28           m_needsFeedback(false),
  29           m_overrideNumSolverIterations(-1),
  30           m_rbA(rbA),
  31           m_rbB(getFixedBody()),
  32           m_appliedImpulse(btScalar(0.)),
  33           m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
  34           m_jointFeedback(0)
  35 {
  36 }
  37
  38 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB)
  39         : btTypedObject(type),
  40           m_userConstraintType(-1),
  41           m_userConstraintPtr((void*)-1),
  42           m_breakingImpulseThreshold(SIMD_INFINITY),
  43           m_isEnabled(true),
  44           m_needsFeedback(false),
  45           m_overrideNumSolverIterations(-1),
  46           m_rbA(rbA),
  47           m_rbB(rbB),
  48           m_appliedImpulse(btScalar(0.)),
  49           m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
  50           m_jointFeedback(0)
  51 {
  52 }
  53
  54 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
  55 {
  56         if (lowLim > uppLim)
  57         {
  58                 return btScalar(1.0f);
  59         }
  60         else if (lowLim == uppLim)
  61         {
  62                 return btScalar(0.0f);
  63         }
  64         btScalar lim_fact = btScalar(1.0f);
  65         btScalar delta_max = vel / timeFact;
  66         if (delta_max < btScalar(0.0f))
  67         {
  68                 if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
  69                 {
  70                         lim_fact = (lowLim - pos) / delta_max;
  71                 }
  72                 else if (pos < lowLim)
  73                 {
  74                         lim_fact = btScalar(0.0f);
  75                 }
  76                 else
  77                 {
  78                         lim_fact = btScalar(1.0f);
  79                 }
  80         }
  81         else if (delta_max > btScalar(0.0f))
  82         {
  83                 if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
  84                 {
  85                         lim_fact = (uppLim - pos) / delta_max;
  86                 }
  87                 else if (pos > uppLim)
  88                 {
  89                         lim_fact = btScalar(0.0f);
  90                 }
  91                 else
  92                 {
  93                         lim_fact = btScalar(1.0f);
  94                 }
  95         }
  96         else
  97         {
  98                 lim_fact = btScalar(0.0f);
  99         }
 100         return lim_fact;
 101 }
 102
 103 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 104 const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 105 {
 106         btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer;
 107
 108         tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
 109         tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
 110         char* name = (char*)serializer->findNameForPointer(this);
 111         tcd->m_name = (char*)serializer->getUniquePointer(name);
 112         if (tcd->m_name)
 113         {
 114                 serializer->serializeName(name);
 115         }
 116
 117         tcd->m_objectType = m_objectType;
 118         tcd->m_needsFeedback = m_needsFeedback;
 119         tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
 120         tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
 121         tcd->m_isEnabled = m_isEnabled ? 1 : 0;
 122
 123         tcd->m_userConstraintId = m_userConstraintId;
 124         tcd->m_userConstraintType = m_userConstraintType;
 125
 126         tcd->m_appliedImpulse = m_appliedImpulse;
 127         tcd->m_dbgDrawSize = m_dbgDrawSize;
 128
 129         tcd->m_disableCollisionsBetweenLinkedBodies = false;
 130
 131         int i;
 132         for (i = 0; i < m_rbA.getNumConstraintRefs(); i++)
 133                 if (m_rbA.getConstraintRef(i) == this)
 134                         tcd->m_disableCollisionsBetweenLinkedBodies = true;
 135         for (i = 0; i < m_rbB.getNumConstraintRefs(); i++)
 136                 if (m_rbB.getConstraintRef(i) == this)
 137                         tcd->m_disableCollisionsBetweenLinkedBodies = true;
 138
 139         return btTypedConstraintDataName;
 140 }
 141
 142 btRigidBody& btTypedConstraint::getFixedBody()
 143 {
 144         static btRigidBody s_fixed(0, 0, 0);
 145         s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
 146         return s_fixed;
 147 }
 148
 149 void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
 150 {
 151         m_halfRange = (high - low) / 2.0f;
 152         m_center = btNormalizeAngle(low + m_halfRange);
 153         m_softness = _softness;
 154         m_biasFactor = _biasFactor;
 155         m_relaxationFactor = _relaxationFactor;
 156 }
 157
 158 void btAngularLimit::test(const btScalar angle)
 159 {
 160         m_correction = 0.0f;
 161         m_sign = 0.0f;
 162         m_solveLimit = false;
 163
 164         if (m_halfRange >= 0.0f)
 165         {
 166                 btScalar deviation = btNormalizeAngle(angle - m_center);
 167                 if (deviation < -m_halfRange)
 168                 {
 169                         m_solveLimit = true;
 170                         m_correction = -(deviation + m_halfRange);
 171                         m_sign = +1.0f;
 172                 }
 173                 else if (deviation > m_halfRange)
 174                 {
 175                         m_solveLimit = true;
 176                         m_correction = m_halfRange - deviation;
 177                         m_sign = -1.0f;
 178                 }
 179         }
 180 }
 181
 182 btScalar btAngularLimit::getError() const
 183 {
 184         return m_correction * m_sign;
 185 }
 186
 187 void btAngularLimit::fit(btScalar& angle) const
 188 {
 189         if (m_halfRange > 0.0f)
 190         {
 191                 btScalar relativeAngle = btNormalizeAngle(angle - m_center);
 192                 if (!btEqual(relativeAngle, m_halfRange))
 193                 {
 194                         if (relativeAngle > 0.0f)
 195                         {
 196                                 angle = getHigh();
 197                         }
 198                         else
 199                         {
 200                                 angle = getLow();
 201                         }
 202                 }
 203         }
 204 }
 205
 206 btScalar btAngularLimit::getLow() const
 207 {
 208         return btNormalizeAngle(m_center - m_halfRange);
 209 }
 210
 211 btScalar btAngularLimit::getHigh() const
 212 {
 213         return btNormalizeAngle(m_center + m_halfRange);
 214 }