Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btTypedConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include "LinearMath/btSerializer.h"
00020
00021
00022 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
00023
00024 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
00025 :btTypedObject(type),
00026 m_userConstraintType(-1),
00027 m_userConstraintId(-1),
00028 m_breakingImpulseThreshold(SIMD_INFINITY),
00029 m_isEnabled(true),
00030 m_needsFeedback(false),
00031 m_overrideNumSolverIterations(-1),
00032 m_rbA(rbA),
00033 m_rbB(getFixedBody()),
00034 m_appliedImpulse(btScalar(0.)),
00035 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
00036 m_jointFeedback(0)
00037 {
00038 }
00039
00040
00041 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
00042 :btTypedObject(type),
00043 m_userConstraintType(-1),
00044 m_userConstraintId(-1),
00045 m_breakingImpulseThreshold(SIMD_INFINITY),
00046 m_isEnabled(true),
00047 m_needsFeedback(false),
00048 m_overrideNumSolverIterations(-1),
00049 m_rbA(rbA),
00050 m_rbB(rbB),
00051 m_appliedImpulse(btScalar(0.)),
00052 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
00053 m_jointFeedback(0)
00054 {
00055 }
00056
00057
00058
00059
00060 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
00061 {
00062 if(lowLim > uppLim)
00063 {
00064 return btScalar(1.0f);
00065 }
00066 else if(lowLim == uppLim)
00067 {
00068 return btScalar(0.0f);
00069 }
00070 btScalar lim_fact = btScalar(1.0f);
00071 btScalar delta_max = vel / timeFact;
00072 if(delta_max < btScalar(0.0f))
00073 {
00074 if((pos >= lowLim) && (pos < (lowLim - delta_max)))
00075 {
00076 lim_fact = (lowLim - pos) / delta_max;
00077 }
00078 else if(pos < lowLim)
00079 {
00080 lim_fact = btScalar(0.0f);
00081 }
00082 else
00083 {
00084 lim_fact = btScalar(1.0f);
00085 }
00086 }
00087 else if(delta_max > btScalar(0.0f))
00088 {
00089 if((pos <= uppLim) && (pos > (uppLim - delta_max)))
00090 {
00091 lim_fact = (uppLim - pos) / delta_max;
00092 }
00093 else if(pos > uppLim)
00094 {
00095 lim_fact = btScalar(0.0f);
00096 }
00097 else
00098 {
00099 lim_fact = btScalar(1.0f);
00100 }
00101 }
00102 else
00103 {
00104 lim_fact = btScalar(0.0f);
00105 }
00106 return lim_fact;
00107 }
00108
00110 const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
00111 {
00112 btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
00113
00114 tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
00115 tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
00116 char* name = (char*) serializer->findNameForPointer(this);
00117 tcd->m_name = (char*)serializer->getUniquePointer(name);
00118 if (tcd->m_name)
00119 {
00120 serializer->serializeName(name);
00121 }
00122
00123 tcd->m_objectType = m_objectType;
00124 tcd->m_needsFeedback = m_needsFeedback;
00125 tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
00126 tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold);
00127 tcd->m_isEnabled = m_isEnabled? 1: 0;
00128
00129 tcd->m_userConstraintId =m_userConstraintId;
00130 tcd->m_userConstraintType =m_userConstraintType;
00131
00132 tcd->m_appliedImpulse = float(m_appliedImpulse);
00133 tcd->m_dbgDrawSize = float(m_dbgDrawSize );
00134
00135 tcd->m_disableCollisionsBetweenLinkedBodies = false;
00136
00137 int i;
00138 for (i=0;i<m_rbA.getNumConstraintRefs();i++)
00139 if (m_rbA.getConstraintRef(i) == this)
00140 tcd->m_disableCollisionsBetweenLinkedBodies = true;
00141 for (i=0;i<m_rbB.getNumConstraintRefs();i++)
00142 if (m_rbB.getConstraintRef(i) == this)
00143 tcd->m_disableCollisionsBetweenLinkedBodies = true;
00144
00145 return "btTypedConstraintData";
00146 }
00147
00148 btRigidBody& btTypedConstraint::getFixedBody()
00149 {
00150 static btRigidBody s_fixed(0, 0,0);
00151 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
00152 return s_fixed;
00153 }
00154
00155
00156 void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
00157 {
00158 m_halfRange = (high - low) / 2.0f;
00159 m_center = btNormalizeAngle(low + m_halfRange);
00160 m_softness = _softness;
00161 m_biasFactor = _biasFactor;
00162 m_relaxationFactor = _relaxationFactor;
00163 }
00164
00165 void btAngularLimit::test(const btScalar angle)
00166 {
00167 m_correction = 0.0f;
00168 m_sign = 0.0f;
00169 m_solveLimit = false;
00170
00171 if (m_halfRange >= 0.0f)
00172 {
00173 btScalar deviation = btNormalizeAngle(angle - m_center);
00174 if (deviation < -m_halfRange)
00175 {
00176 m_solveLimit = true;
00177 m_correction = - (deviation + m_halfRange);
00178 m_sign = +1.0f;
00179 }
00180 else if (deviation > m_halfRange)
00181 {
00182 m_solveLimit = true;
00183 m_correction = m_halfRange - deviation;
00184 m_sign = -1.0f;
00185 }
00186 }
00187 }
00188
00189
00190 btScalar btAngularLimit::getError() const
00191 {
00192 return m_correction * m_sign;
00193 }
00194
00195 void btAngularLimit::fit(btScalar& angle) const
00196 {
00197 if (m_halfRange > 0.0f)
00198 {
00199 btScalar relativeAngle = btNormalizeAngle(angle - m_center);
00200 if (!btEqual(relativeAngle, m_halfRange))
00201 {
00202 if (relativeAngle > 0.0f)
00203 {
00204 angle = getHigh();
00205 }
00206 else
00207 {
00208 angle = getLow();
00209 }
00210 }
00211 }
00212 }
00213
00214 btScalar btAngularLimit::getLow() const
00215 {
00216 return btNormalizeAngle(m_center - m_halfRange);
00217 }
00218
00219 btScalar btAngularLimit::getHigh() const
00220 {
00221 return btNormalizeAngle(m_center + m_halfRange);
00222 }