btTypedConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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 }