Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
00120 int i;
00121
00122 for(i = 0; i < 3; i++)
00123 {
00124 if(m_springEnabled[i])
00125 {
00126
00127 btScalar currPos = m_calculatedLinearDiff[i];
00128
00129 btScalar delta = currPos - m_equilibriumPoint[i];
00130
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
00142 btScalar currPos = m_calculatedAxisAngleDiff[i];
00143
00144 btScalar delta = currPos - m_equilibriumPoint[i+3];
00145
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
00158
00159 internalUpdateSprings(info);
00160
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);
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
00178 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
00179 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
00180
00181 calculateTransforms();
00182 }
00183
00184
00185