152 lines
4.6 KiB
C++
152 lines
4.6 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
|
|
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
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.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "btGeneric6DofSpringConstraint.h"
|
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
|
#include "LinearMath/btTransformUtil.h"
|
|
|
|
|
|
btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
|
|
: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
|
|
{
|
|
for(int i = 0; i < 6; i++)
|
|
{
|
|
m_springEnabled[i] = false;
|
|
m_equilibriumPoint[i] = btScalar(0.f);
|
|
m_springStiffness[i] = btScalar(0.f);
|
|
m_springDamping[i] = btScalar(1.f);
|
|
}
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_springEnabled[index] = onOff;
|
|
if(index < 3)
|
|
{
|
|
m_linearLimits.m_enableMotor[index] = onOff;
|
|
}
|
|
else
|
|
{
|
|
m_angularLimits[index - 3].m_enableMotor = onOff;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_springStiffness[index] = stiffness;
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_springDamping[index] = damping;
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setEquilibriumPoint()
|
|
{
|
|
calculateTransforms();
|
|
int i;
|
|
|
|
for( i = 0; i < 3; i++)
|
|
{
|
|
m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
|
|
}
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
calculateTransforms();
|
|
if(index < 3)
|
|
{
|
|
m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
|
|
}
|
|
else
|
|
{
|
|
m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3];
|
|
}
|
|
}
|
|
|
|
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
|
|
{
|
|
btAssert((index >= 0) && (index < 6));
|
|
m_equilibriumPoint[index] = val;
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
|
|
{
|
|
// it is assumed that calculateTransforms() have been called before this call
|
|
int i;
|
|
btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
if(m_springEnabled[i])
|
|
{
|
|
// get current position of constraint
|
|
btScalar currPos = m_calculatedLinearDiff[i];
|
|
// calculate difference
|
|
btScalar delta = currPos - m_equilibriumPoint[i];
|
|
// spring force is (delta * m_stiffness) according to Hooke's Law
|
|
btScalar force = delta * m_springStiffness[i];
|
|
btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
|
|
m_linearLimits.m_targetVelocity[i] = velFactor * force;
|
|
m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
|
|
}
|
|
}
|
|
for(i = 0; i < 3; i++)
|
|
{
|
|
if(m_springEnabled[i + 3])
|
|
{
|
|
// get current position of constraint
|
|
btScalar currPos = m_calculatedAxisAngleDiff[i];
|
|
// calculate difference
|
|
btScalar delta = currPos - m_equilibriumPoint[i+3];
|
|
// spring force is (-delta * m_stiffness) according to Hooke's Law
|
|
btScalar force = -delta * m_springStiffness[i+3];
|
|
btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
|
|
m_angularLimits[i].m_targetVelocity = velFactor * force;
|
|
m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
|
|
{
|
|
// this will be called by constraint solver at the constraint setup stage
|
|
// set current motor parameters
|
|
internalUpdateSprings(info);
|
|
// do the rest of job for constraint setup
|
|
btGeneric6DofConstraint::getInfo2(info);
|
|
}
|
|
|
|
|
|
|
|
|