dali-physics/third-party/bullet3/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.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 "b3TypedConstraint.h"
  17 //#include "Bullet3Common/b3Serializer.h"
  18
  19 #define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
  20
  21 b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
  22         : b3TypedObject(type),
  23           m_userConstraintType(-1),
  24           m_userConstraintPtr((void*)-1),
  25           m_breakingImpulseThreshold(B3_INFINITY),
  26           m_isEnabled(true),
  27           m_needsFeedback(false),
  28           m_overrideNumSolverIterations(-1),
  29           m_rbA(rbA),
  30           m_rbB(rbB),
  31           m_appliedImpulse(b3Scalar(0.)),
  32           m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
  33           m_jointFeedback(0)
  34 {
  35 }
  36
  37 b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
  38 {
  39         if (lowLim > uppLim)
  40         {
  41                 return b3Scalar(1.0f);
  42         }
  43         else if (lowLim == uppLim)
  44         {
  45                 return b3Scalar(0.0f);
  46         }
  47         b3Scalar lim_fact = b3Scalar(1.0f);
  48         b3Scalar delta_max = vel / timeFact;
  49         if (delta_max < b3Scalar(0.0f))
  50         {
  51                 if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
  52                 {
  53                         lim_fact = (lowLim - pos) / delta_max;
  54                 }
  55                 else if (pos < lowLim)
  56                 {
  57                         lim_fact = b3Scalar(0.0f);
  58                 }
  59                 else
  60                 {
  61                         lim_fact = b3Scalar(1.0f);
  62                 }
  63         }
  64         else if (delta_max > b3Scalar(0.0f))
  65         {
  66                 if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
  67                 {
  68                         lim_fact = (uppLim - pos) / delta_max;
  69                 }
  70                 else if (pos > uppLim)
  71                 {
  72                         lim_fact = b3Scalar(0.0f);
  73                 }
  74                 else
  75                 {
  76                         lim_fact = b3Scalar(1.0f);
  77                 }
  78         }
  79         else
  80         {
  81                 lim_fact = b3Scalar(0.0f);
  82         }
  83         return lim_fact;
  84 }
  85
  86 void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
  87 {
  88         m_halfRange = (high - low) / 2.0f;
  89         m_center = b3NormalizeAngle(low + m_halfRange);
  90         m_softness = _softness;
  91         m_biasFactor = _biasFactor;
  92         m_relaxationFactor = _relaxationFactor;
  93 }
  94
  95 void b3AngularLimit::test(const b3Scalar angle)
  96 {
  97         m_correction = 0.0f;
  98         m_sign = 0.0f;
  99         m_solveLimit = false;
 100
 101         if (m_halfRange >= 0.0f)
 102         {
 103                 b3Scalar deviation = b3NormalizeAngle(angle - m_center);
 104                 if (deviation < -m_halfRange)
 105                 {
 106                         m_solveLimit = true;
 107                         m_correction = -(deviation + m_halfRange);
 108                         m_sign = +1.0f;
 109                 }
 110                 else if (deviation > m_halfRange)
 111                 {
 112                         m_solveLimit = true;
 113                         m_correction = m_halfRange - deviation;
 114                         m_sign = -1.0f;
 115                 }
 116         }
 117 }
 118
 119 b3Scalar b3AngularLimit::getError() const
 120 {
 121         return m_correction * m_sign;
 122 }
 123
 124 void b3AngularLimit::fit(b3Scalar& angle) const
 125 {
 126         if (m_halfRange > 0.0f)
 127         {
 128                 b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center);
 129                 if (!b3Equal(relativeAngle, m_halfRange))
 130                 {
 131                         if (relativeAngle > 0.0f)
 132                         {
 133                                 angle = getHigh();
 134                         }
 135                         else
 136                         {
 137                                 angle = getLow();
 138                         }
 139                 }
 140         }
 141 }
 142
 143 b3Scalar b3AngularLimit::getLow() const
 144 {
 145         return b3NormalizeAngle(m_center - m_halfRange);
 146 }
 147
 148 b3Scalar b3AngularLimit::getHigh() const
 149 {
 150         return b3NormalizeAngle(m_center + m_halfRange);
 151 }