btGeneric6DofSpringConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
00003 Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
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 #include "btGeneric6DofSpringConstraint.h"
00017 #include "BulletDynamics/Dynamics/btRigidBody.h"
00018 #include "LinearMath/btTransformUtil.h"
00019 
00020 
00021 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
00022         : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
00023 {
00024     init();
00025 }
00026 
00027 
00028 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
00029         : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
00030 {
00031     init();
00032 }
00033 
00034 
00035 void btGeneric6DofSpringConstraint::init()
00036 {
00037         m_objectType = D6_SPRING_CONSTRAINT_TYPE;
00038 
00039         for(int i = 0; i < 6; i++)
00040         {
00041                 m_springEnabled[i] = false;
00042                 m_equilibriumPoint[i] = btScalar(0.f);
00043                 m_springStiffness[i] = btScalar(0.f);
00044                 m_springDamping[i] = btScalar(1.f);
00045         }
00046 }
00047 
00048 
00049 void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
00050 {
00051         btAssert((index >= 0) && (index < 6));
00052         m_springEnabled[index] = onOff;
00053         if(index < 3)
00054         {
00055                 m_linearLimits.m_enableMotor[index] = onOff;
00056         }
00057         else
00058         {
00059                 m_angularLimits[index - 3].m_enableMotor = onOff;
00060         }
00061 }
00062 
00063 
00064 
00065 void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
00066 {
00067         btAssert((index >= 0) && (index < 6));
00068         m_springStiffness[index] = stiffness;
00069 }
00070 
00071 
00072 void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
00073 {
00074         btAssert((index >= 0) && (index < 6));
00075         m_springDamping[index] = damping;
00076 }
00077 
00078 
00079 void btGeneric6DofSpringConstraint::setEquilibriumPoint()
00080 {
00081         calculateTransforms();
00082         int i;
00083 
00084         for( i = 0; i < 3; i++)
00085         {
00086                 m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
00087         }
00088         for(i = 0; i < 3; i++)
00089         {
00090                 m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
00091         }
00092 }
00093 
00094 
00095 
00096 void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
00097 {
00098         btAssert((index >= 0) && (index < 6));
00099         calculateTransforms();
00100         if(index < 3)
00101         {
00102                 m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
00103         }
00104         else
00105         {
00106                 m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3];
00107         }
00108 }
00109 
00110 void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
00111 {
00112         btAssert((index >= 0) && (index < 6));
00113         m_equilibriumPoint[index] = val;
00114 }
00115 
00116 
00117 void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
00118 {
00119         // it is assumed that calculateTransforms() have been called before this call
00120         int i;
00121         //btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
00122         for(i = 0; i < 3; i++)
00123         {
00124                 if(m_springEnabled[i])
00125                 {
00126                         // get current position of constraint
00127                         btScalar currPos = m_calculatedLinearDiff[i];
00128                         // calculate difference
00129                         btScalar delta = currPos - m_equilibriumPoint[i];
00130                         // spring force is (delta * m_stiffness) according to Hooke's Law
00131                         btScalar force = delta * m_springStiffness[i];
00132                         btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
00133                         m_linearLimits.m_targetVelocity[i] =  velFactor * force;
00134                         m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
00135                 }
00136         }
00137         for(i = 0; i < 3; i++)
00138         {
00139                 if(m_springEnabled[i + 3])
00140                 {
00141                         // get current position of constraint
00142                         btScalar currPos = m_calculatedAxisAngleDiff[i];
00143                         // calculate difference
00144                         btScalar delta = currPos - m_equilibriumPoint[i+3];
00145                         // spring force is (-delta * m_stiffness) according to Hooke's Law
00146                         btScalar force = -delta * m_springStiffness[i+3];
00147                         btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
00148                         m_angularLimits[i].m_targetVelocity = velFactor * force;
00149                         m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
00150                 }
00151         }
00152 }
00153 
00154 
00155 void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
00156 {
00157         // this will be called by constraint solver at the constraint setup stage
00158         // set current motor parameters
00159         internalUpdateSprings(info);
00160         // do the rest of job for constraint setup
00161         btGeneric6DofConstraint::getInfo2(info);
00162 }
00163 
00164 
00165 void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
00166 {
00167         btVector3 zAxis = axis1.normalized();
00168         btVector3 yAxis = axis2.normalized();
00169         btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
00170 
00171         btTransform frameInW;
00172         frameInW.setIdentity();
00173         frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
00174                                 xAxis[1], yAxis[1], zAxis[1],
00175                                 xAxis[2], yAxis[2], zAxis[2]);
00176 
00177         // now get constraint frame in local coordinate systems
00178         m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
00179         m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
00180 
00181   calculateTransforms();
00182 }
00183 
00184 
00185