First stage in refactoring Bullet: moved Bullet Collision and Dynamics and LinearMath into src folder, and all files in Collision Detection and Dynamics have bt prefix.
Made all buildsystems to work again (jam, msvc, cmake)
This commit is contained in:
ejcoumans
41
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
Normal file
41
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
Normal file
@@ -0,0 +1,41 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef CONSTRAINT_SOLVER_H
|
||||
#define CONSTRAINT_SOLVER_H
|
||||
|
||||
class PersistentManifold;
|
||||
class RigidBody;
|
||||
|
||||
struct ContactSolverInfo;
|
||||
struct BroadphaseProxy;
|
||||
class IDebugDraw;
|
||||
|
||||
/// ConstraintSolver provides solver interface
|
||||
class ConstraintSolver
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
virtual ~ConstraintSolver() {}
|
||||
|
||||
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,class IDebugDraw* debugDrawer = 0) = 0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //CONSTRAINT_SOLVER_H
|
||||
234
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
Normal file
234
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
Normal file
@@ -0,0 +1,234 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "LinearMath/GenMinMax.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
#define ASSERT2 assert
|
||||
|
||||
//some values to find stable tresholds
|
||||
|
||||
float useGlobalSettingContacts = false;//true;
|
||||
SimdScalar contactDamping = 0.2f;
|
||||
SimdScalar contactTau = .02f;//0.02f;//*0.02f;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//bilateral constraint between two dynamic objects
|
||||
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
|
||||
{
|
||||
float normalLenSqr = normal.length2();
|
||||
ASSERT2(fabs(normalLenSqr) < 1.1f);
|
||||
if (normalLenSqr > 1.1f)
|
||||
{
|
||||
impulse = 0.f;
|
||||
return;
|
||||
}
|
||||
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
|
||||
JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
|
||||
body2.getInvInertiaDiagLocal(),body2.getInvMass());
|
||||
|
||||
SimdScalar jacDiagAB = jac.getDiagonal();
|
||||
SimdScalar jacDiagABInv = 1.f / jacDiagAB;
|
||||
|
||||
SimdScalar rel_vel = jac.getRelativeVelocity(
|
||||
body1.getLinearVelocity(),
|
||||
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
|
||||
body2.getLinearVelocity(),
|
||||
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
|
||||
float a;
|
||||
a=jacDiagABInv;
|
||||
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
#ifdef ONLY_USE_LINEAR_MASS
|
||||
SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
|
||||
impulse = - contactDamping * rel_vel * massTerm;
|
||||
#else
|
||||
SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
||||
impulse = velocityImpulse;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
float resolveSingleCollision(
|
||||
RigidBody& body1,
|
||||
RigidBody& body2,
|
||||
ManifoldPoint& contactPoint,
|
||||
const ContactSolverInfo& solverInfo
|
||||
|
||||
)
|
||||
{
|
||||
|
||||
const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
|
||||
const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
|
||||
|
||||
|
||||
// printf("distance=%f\n",distance);
|
||||
|
||||
const SimdVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
SimdScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
|
||||
|
||||
float damping = solverInfo.m_damping ;
|
||||
float Kerp = solverInfo.m_erp;
|
||||
|
||||
if (useGlobalSettingContacts)
|
||||
{
|
||||
damping = contactDamping;
|
||||
Kerp = contactTau;
|
||||
}
|
||||
|
||||
float Kcor = Kerp *Kfps;
|
||||
|
||||
//printf("dist=%f\n",distance);
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
|
||||
SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
|
||||
|
||||
|
||||
//distance = 0.f;
|
||||
SimdScalar positionalError = Kcor *-distance;
|
||||
//jacDiagABInv;
|
||||
SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
|
||||
SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
SimdScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
SimdScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
float oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
float sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
|
||||
float resolveSingleFriction(
|
||||
RigidBody& body1,
|
||||
RigidBody& body2,
|
||||
ManifoldPoint& contactPoint,
|
||||
const ContactSolverInfo& solverInfo
|
||||
|
||||
)
|
||||
{
|
||||
|
||||
const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
|
||||
const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
|
||||
|
||||
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
|
||||
float combinedFriction = cpd->m_friction;
|
||||
|
||||
SimdScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
//if (contactPoint.m_appliedImpulse>0.f)
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
{
|
||||
// 1st tangent
|
||||
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
float total = cpd->m_accumulatedTangentImpulse0 + j;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// 2nd tangent
|
||||
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
SimdScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
float total = cpd->m_accumulatedTangentImpulse1 + j;
|
||||
GEN_set_min(total, limit);
|
||||
GEN_set_max(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
|
||||
}
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
103
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
Normal file
103
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
Normal file
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef CONTACT_CONSTRAINT_H
|
||||
#define CONTACT_CONSTRAINT_H
|
||||
|
||||
//todo: make into a proper class working with the iterative constraint solver
|
||||
|
||||
class RigidBody;
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/SimdScalar.h"
|
||||
struct ContactSolverInfo;
|
||||
class ManifoldPoint;
|
||||
|
||||
enum {
|
||||
DEFAULT_CONTACT_SOLVER_TYPE=0,
|
||||
CONTACT_SOLVER_TYPE1,
|
||||
CONTACT_SOLVER_TYPE2,
|
||||
USER_CONTACT_SOLVER_TYPE1,
|
||||
MAX_CONTACT_SOLVER_TYPES
|
||||
};
|
||||
|
||||
|
||||
typedef float (*ContactSolverFunc)(RigidBody& body1,
|
||||
RigidBody& body2,
|
||||
class ManifoldPoint& contactPoint,
|
||||
const ContactSolverInfo& info);
|
||||
|
||||
struct ConstraintPersistentData
|
||||
{
|
||||
inline ConstraintPersistentData()
|
||||
:m_appliedImpulse(0.f),
|
||||
m_prevAppliedImpulse(0.f),
|
||||
m_accumulatedTangentImpulse0(0.f),
|
||||
m_accumulatedTangentImpulse1(0.f),
|
||||
m_jacDiagABInv(0.f),
|
||||
m_persistentLifeTime(0),
|
||||
m_restitution(0.f),
|
||||
m_friction(0.f),
|
||||
m_penetration(0.f),
|
||||
m_contactSolverFunc(0),
|
||||
m_frictionSolverFunc(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/// total applied impulse during most recent frame
|
||||
float m_appliedImpulse;
|
||||
float m_prevAppliedImpulse;
|
||||
float m_accumulatedTangentImpulse0;
|
||||
float m_accumulatedTangentImpulse1;
|
||||
|
||||
float m_jacDiagABInv;
|
||||
float m_jacDiagABInvTangent0;
|
||||
float m_jacDiagABInvTangent1;
|
||||
int m_persistentLifeTime;
|
||||
float m_restitution;
|
||||
float m_friction;
|
||||
float m_penetration;
|
||||
SimdVector3 m_frictionWorldTangential0;
|
||||
SimdVector3 m_frictionWorldTangential1;
|
||||
|
||||
ContactSolverFunc m_contactSolverFunc;
|
||||
ContactSolverFunc m_frictionSolverFunc;
|
||||
|
||||
};
|
||||
|
||||
///bilateral constraint between two dynamic objects
|
||||
///positive distance = separation, negative distance = penetration
|
||||
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||||
RigidBody& body2, const SimdVector3& pos2,
|
||||
SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep);
|
||||
|
||||
|
||||
///contact constraint resolution:
|
||||
///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
|
||||
///positive distance = separation, negative distance = penetration
|
||||
float resolveSingleCollision(
|
||||
RigidBody& body1,
|
||||
RigidBody& body2,
|
||||
ManifoldPoint& contactPoint,
|
||||
const ContactSolverInfo& info);
|
||||
|
||||
float resolveSingleFriction(
|
||||
RigidBody& body1,
|
||||
RigidBody& body2,
|
||||
ManifoldPoint& contactPoint,
|
||||
const ContactSolverInfo& solverInfo
|
||||
);
|
||||
|
||||
#endif //CONTACT_CONSTRAINT_H
|
||||
47
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
Normal file
47
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
Normal file
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef CONTACT_SOLVER_INFO
|
||||
#define CONTACT_SOLVER_INFO
|
||||
|
||||
|
||||
struct ContactSolverInfo
|
||||
{
|
||||
|
||||
inline ContactSolverInfo()
|
||||
{
|
||||
m_tau = 0.6f;
|
||||
m_damping = 1.0f;
|
||||
m_friction = 0.3f;
|
||||
m_restitution = 0.f;
|
||||
m_maxErrorReduction = 20.f;
|
||||
m_numIterations = 10;
|
||||
m_erp = 0.4f;
|
||||
m_sor = 1.3f;
|
||||
}
|
||||
|
||||
float m_tau;
|
||||
float m_damping;
|
||||
float m_friction;
|
||||
float m_timeStep;
|
||||
float m_restitution;
|
||||
int m_numIterations;
|
||||
float m_maxErrorReduction;
|
||||
float m_sor;
|
||||
float m_erp;
|
||||
|
||||
};
|
||||
|
||||
#endif //CONTACT_SOLVER_INFO
|
||||
251
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
Normal file
251
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
Normal file
@@ -0,0 +1,251 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btGeneric6DofConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Dynamics/btMassProps.h"
|
||||
#include "LinearMath/SimdTransformUtil.h"
|
||||
|
||||
static const SimdScalar kSign[] = { 1.0f, -1.0f, 1.0f };
|
||||
static const int kAxisA[] = { 1, 0, 0 };
|
||||
static const int kAxisB[] = { 2, 2, 1 };
|
||||
|
||||
Generic6DofConstraint::Generic6DofConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB)
|
||||
: TypedConstraint(rbA, rbB)
|
||||
, m_frameInA(frameInA)
|
||||
, m_frameInB(frameInB)
|
||||
{
|
||||
//free means upper < lower,
|
||||
//locked means upper == lower
|
||||
//limited means upper > lower
|
||||
//so start all locked
|
||||
for (int i=0; i<6;++i)
|
||||
{
|
||||
m_lowerLimit[i] = 0.0f;
|
||||
m_upperLimit[i] = 0.0f;
|
||||
m_accumulatedImpulse[i] = 0.0f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Generic6DofConstraint::BuildJacobian()
|
||||
{
|
||||
SimdVector3 normal(0,0,0);
|
||||
|
||||
const SimdVector3& pivotInA = m_frameInA.getOrigin();
|
||||
const SimdVector3& pivotInB = m_frameInB.getOrigin();
|
||||
|
||||
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
|
||||
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
|
||||
|
||||
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
int i;
|
||||
//linear part
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i))
|
||||
{
|
||||
normal[i] = 1;
|
||||
|
||||
// Create linear atom
|
||||
new (&m_jacLinear[i]) JacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
|
||||
// Apply accumulated impulse
|
||||
SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
|
||||
|
||||
m_rbA.applyImpulse( impulse_vector, rel_pos1);
|
||||
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// angular part
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i+3))
|
||||
{
|
||||
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
|
||||
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
|
||||
|
||||
// Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
|
||||
SimdVector3 axis = kSign[i] * axisA.cross(axisB);
|
||||
|
||||
// Create angular atom
|
||||
new (&m_jacAng[i]) JacobianEntry(axis,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
// Apply accumulated impulse
|
||||
SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
|
||||
|
||||
m_rbA.applyTorqueImpulse( impulse_vector);
|
||||
m_rbB.applyTorqueImpulse(-impulse_vector);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
|
||||
{
|
||||
SimdScalar tau = 0.1f;
|
||||
SimdScalar damping = 1.0f;
|
||||
|
||||
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
|
||||
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
|
||||
|
||||
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 normal(0,0,0);
|
||||
int i;
|
||||
|
||||
// linear
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i))
|
||||
{
|
||||
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
|
||||
normal[i] = 1;
|
||||
SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
|
||||
|
||||
//velocity error (first order error)
|
||||
SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
|
||||
|
||||
SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
|
||||
m_accumulatedImpulse[i] += impulse;
|
||||
|
||||
SimdVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse( impulse_vector, rel_pos1);
|
||||
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// angular
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (isLimited(i+3))
|
||||
{
|
||||
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
|
||||
|
||||
//velocity error (first order error)
|
||||
SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
|
||||
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
|
||||
|
||||
SimdScalar rel_pos = kSign[i] * axisA.dot(axisB);
|
||||
|
||||
//impulse
|
||||
SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
|
||||
m_accumulatedImpulse[i + 3] += impulse;
|
||||
|
||||
// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
|
||||
SimdVector3 axis = kSign[i] * axisA.cross(axisB);
|
||||
SimdVector3 impulse_vector = axis * impulse;
|
||||
|
||||
m_rbA.applyTorqueImpulse( impulse_vector);
|
||||
m_rbB.applyTorqueImpulse(-impulse_vector);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
SimdScalar Generic6DofConstraint::ComputeAngle(int axis) const
|
||||
{
|
||||
SimdScalar angle;
|
||||
|
||||
switch (axis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
SimdVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
|
||||
SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
|
||||
SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
|
||||
|
||||
SimdScalar s = v1.dot(w2);
|
||||
SimdScalar c = v1.dot(v2);
|
||||
|
||||
angle = SimdAtan2( s, c );
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
{
|
||||
SimdVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
|
||||
SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
|
||||
SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
|
||||
|
||||
SimdScalar s = w1.dot(u2);
|
||||
SimdScalar c = w1.dot(w2);
|
||||
|
||||
angle = SimdAtan2( s, c );
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
{
|
||||
SimdVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
|
||||
SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
|
||||
SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
|
||||
|
||||
SimdScalar s = u1.dot(v2);
|
||||
SimdScalar c = u1.dot(u2);
|
||||
|
||||
angle = SimdAtan2( s, c );
|
||||
}
|
||||
break;
|
||||
default: assert ( 0 ) ; break ;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
114
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
Normal file
114
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
Normal file
@@ -0,0 +1,114 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef GENERIC_6DOF_CONSTRAINT_H
|
||||
#define GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class RigidBody;
|
||||
|
||||
|
||||
|
||||
/// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
|
||||
/// Work in progress (is still a Hinge actually)
|
||||
class Generic6DofConstraint : public TypedConstraint
|
||||
{
|
||||
JacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints
|
||||
JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
|
||||
|
||||
SimdTransform m_frameInA; // the constraint space w.r.t body A
|
||||
SimdTransform m_frameInB; // the constraint space w.r.t body B
|
||||
|
||||
SimdScalar m_lowerLimit[6]; // the constraint lower limits
|
||||
SimdScalar m_upperLimit[6]; // the constraint upper limits
|
||||
|
||||
SimdScalar m_accumulatedImpulse[6];
|
||||
|
||||
|
||||
public:
|
||||
Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB );
|
||||
|
||||
Generic6DofConstraint();
|
||||
|
||||
virtual void BuildJacobian();
|
||||
|
||||
virtual void SolveConstraint(SimdScalar timeStep);
|
||||
|
||||
void UpdateRHS(SimdScalar timeStep);
|
||||
|
||||
SimdScalar ComputeAngle(int axis) const;
|
||||
|
||||
void setLinearLowerLimit(const SimdVector3& linearLower)
|
||||
{
|
||||
m_lowerLimit[0] = linearLower.getX();
|
||||
m_lowerLimit[1] = linearLower.getY();
|
||||
m_lowerLimit[2] = linearLower.getZ();
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const SimdVector3& linearUpper)
|
||||
{
|
||||
m_upperLimit[0] = linearUpper.getX();
|
||||
m_upperLimit[1] = linearUpper.getY();
|
||||
m_upperLimit[2] = linearUpper.getZ();
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const SimdVector3& angularLower)
|
||||
{
|
||||
m_lowerLimit[3] = angularLower.getX();
|
||||
m_lowerLimit[4] = angularLower.getY();
|
||||
m_lowerLimit[5] = angularLower.getZ();
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const SimdVector3& angularUpper)
|
||||
{
|
||||
m_upperLimit[3] = angularUpper.getX();
|
||||
m_upperLimit[4] = angularUpper.getY();
|
||||
m_upperLimit[5] = angularUpper.getZ();
|
||||
}
|
||||
|
||||
//first 3 are linear, next 3 are angular
|
||||
void SetLimit(int axis, SimdScalar lo, SimdScalar hi)
|
||||
{
|
||||
m_lowerLimit[axis] = lo;
|
||||
m_upperLimit[axis] = hi;
|
||||
}
|
||||
|
||||
//free means upper < lower,
|
||||
//locked means upper == lower
|
||||
//limited means upper > lower
|
||||
//limitIndex: first 3 are linear, next 3 are angular
|
||||
bool isLimited(int limitIndex)
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
|
||||
const RigidBody& GetRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const RigidBody& GetRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //GENERIC_6DOF_CONSTRAINT_H
|
||||
276
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
Normal file
276
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
Normal file
@@ -0,0 +1,276 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btHingeConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Dynamics/btMassProps.h"
|
||||
#include "LinearMath/SimdTransformUtil.h"
|
||||
|
||||
|
||||
HingeConstraint::HingeConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,
|
||||
SimdVector3& axisInA,SimdVector3& axisInB)
|
||||
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
|
||||
m_axisInA(axisInA),
|
||||
m_axisInB(-axisInB),
|
||||
m_angularOnly(false)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA)
|
||||
:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
|
||||
m_axisInA(axisInA),
|
||||
//fixed axis in worldspace
|
||||
m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
|
||||
m_angularOnly(false)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void HingeConstraint::BuildJacobian()
|
||||
{
|
||||
m_appliedImpulse = 0.f;
|
||||
|
||||
SimdVector3 normal(0,0,0);
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
new (&m_jac[i]) JacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//calculate two perpendicular jointAxis, orthogonal to hingeAxis
|
||||
//these two jointAxis require equal angular velocities for both bodies
|
||||
|
||||
//this is unused for now, it's a todo
|
||||
SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
SimdVector3 jointAxis0;
|
||||
SimdVector3 jointAxis1;
|
||||
SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
|
||||
|
||||
new (&m_jacAng[0]) JacobianEntry(jointAxis0,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
new (&m_jacAng[1]) JacobianEntry(jointAxis1,
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvInertiaDiagLocal());
|
||||
|
||||
|
||||
}
|
||||
|
||||
void HingeConstraint::SolveConstraint(SimdScalar timeStep)
|
||||
{
|
||||
//#define NEW_IMPLEMENTATION
|
||||
|
||||
#ifdef NEW_IMPLEMENTATION
|
||||
SimdScalar tau = 0.3f;
|
||||
SimdScalar damping = 1.f;
|
||||
|
||||
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
|
||||
// Dirk: Don't we need to update this after each applied impulse
|
||||
SimdVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
SimdVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
SimdVector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
|
||||
|
||||
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
// Dirk: Get new angular velocity since it changed after applying an impulse
|
||||
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
//velocity error (first order error)
|
||||
SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
|
||||
//positional error (zeroth order error)
|
||||
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
|
||||
|
||||
SimdScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv;
|
||||
|
||||
SimdVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
|
||||
// constraint axes in world space
|
||||
SimdVector3 jointAxis0;
|
||||
SimdVector3 jointAxis1;
|
||||
SimdPlaneSpace1(axisA,jointAxis0,jointAxis1);
|
||||
|
||||
|
||||
// Dirk: Get new angular velocity since it changed after applying an impulse
|
||||
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
SimdScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
|
||||
SimdScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
float tau1 = tau;//0.f;
|
||||
|
||||
SimdScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
|
||||
SimdVector3 angular_impulse0 = jointAxis0 * impulse0;
|
||||
|
||||
m_rbA.applyTorqueImpulse( angular_impulse0);
|
||||
m_rbB.applyTorqueImpulse(-angular_impulse0);
|
||||
|
||||
|
||||
|
||||
// Dirk: Get new angular velocity since it changed after applying an impulse
|
||||
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
SimdScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
|
||||
SimdScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);;
|
||||
|
||||
SimdScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
|
||||
SimdVector3 angular_impulse1 = jointAxis1 * impulse1;
|
||||
|
||||
m_rbA.applyTorqueImpulse( angular_impulse1);
|
||||
m_rbB.applyTorqueImpulse(-angular_impulse1);
|
||||
|
||||
#else
|
||||
|
||||
|
||||
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
|
||||
SimdVector3 normal(0,0,0);
|
||||
SimdScalar tau = 0.3f;
|
||||
SimdScalar damping = 1.f;
|
||||
|
||||
//linear part
|
||||
{
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
|
||||
|
||||
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
SimdScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
SimdScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
|
||||
m_appliedImpulse += impulse;
|
||||
SimdVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
|
||||
const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity();
|
||||
const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity();
|
||||
SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA);
|
||||
SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB);
|
||||
SimdVector3 velrel = angA-angB;
|
||||
|
||||
//solve angular velocity correction
|
||||
float relaxation = 1.f;
|
||||
float len = velrel.length();
|
||||
if (len > 0.00001f)
|
||||
{
|
||||
SimdVector3 normal = velrel.normalized();
|
||||
float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
|
||||
GetRigidBodyB().ComputeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
velrel *= (1.f/denom) * 0.9;
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
|
||||
float len2 = angularError.length();
|
||||
if (len2>0.00001f)
|
||||
{
|
||||
SimdVector3 normal2 = angularError.normalized();
|
||||
float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) +
|
||||
GetRigidBodyB().ComputeAngularImpulseDenominator(normal2);
|
||||
angularError *= (1.f/denom2) * relaxation;
|
||||
}
|
||||
|
||||
m_rbA.applyTorqueImpulse(-velrel+angularError);
|
||||
m_rbB.applyTorqueImpulse(velrel-angularError);
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void HingeConstraint::UpdateRHS(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
73
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
Normal file
73
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
Normal file
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef HINGECONSTRAINT_H
|
||||
#define HINGECONSTRAINT_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class RigidBody;
|
||||
|
||||
|
||||
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/// axis defines the orientation of the hinge axis
|
||||
class HingeConstraint : public TypedConstraint
|
||||
{
|
||||
JacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
JacobianEntry m_jacAng[2]; //2 orthogonal angular constraints
|
||||
|
||||
SimdVector3 m_pivotInA;
|
||||
SimdVector3 m_pivotInB;
|
||||
SimdVector3 m_axisInA;
|
||||
SimdVector3 m_axisInB;
|
||||
|
||||
bool m_angularOnly;
|
||||
|
||||
public:
|
||||
|
||||
HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,SimdVector3& axisInA,SimdVector3& axisInB);
|
||||
|
||||
HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA);
|
||||
|
||||
HingeConstraint();
|
||||
|
||||
virtual void BuildJacobian();
|
||||
|
||||
virtual void SolveConstraint(SimdScalar timeStep);
|
||||
|
||||
void UpdateRHS(SimdScalar timeStep);
|
||||
|
||||
const RigidBody& GetRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const RigidBody& GetRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //HINGECONSTRAINT_H
|
||||
156
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
Normal file
156
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
Normal file
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef JACOBIAN_ENTRY_H
|
||||
#define JACOBIAN_ENTRY_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
|
||||
//notes:
|
||||
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
|
||||
// which makes the JacobianEntry memory layout 16 bytes
|
||||
// if you only are interested in angular part, just feed massInvA and massInvB zero
|
||||
|
||||
/// Jacobian entry is an abstraction that allows to describe constraints
|
||||
/// it can be used in combination with a constraint solver
|
||||
/// Can be used to relate the effect of an impulse to the constraint error
|
||||
class JacobianEntry
|
||||
{
|
||||
public:
|
||||
JacobianEntry() {};
|
||||
//constraint between two different rigidbodies
|
||||
JacobianEntry(
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdMatrix3x3& world2B,
|
||||
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
|
||||
const SimdVector3& jointAxis,
|
||||
const SimdVector3& inertiaInvA,
|
||||
const SimdScalar massInvA,
|
||||
const SimdVector3& inertiaInvB,
|
||||
const SimdScalar massInvB)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
|
||||
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
ASSERT(m_Adiag > 0.0f);
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
JacobianEntry(const SimdVector3& jointAxis,
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdMatrix3x3& world2B,
|
||||
const SimdVector3& inertiaInvA,
|
||||
const SimdVector3& inertiaInvB)
|
||||
:m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
|
||||
{
|
||||
m_aJ= world2A*jointAxis;
|
||||
m_bJ = world2B*-jointAxis;
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
ASSERT(m_Adiag > 0.0f);
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
JacobianEntry(const SimdVector3& axisInA,
|
||||
const SimdVector3& axisInB,
|
||||
const SimdVector3& inertiaInvA,
|
||||
const SimdVector3& inertiaInvB)
|
||||
: m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
|
||||
, m_aJ(axisInA)
|
||||
, m_bJ(-axisInB)
|
||||
{
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
ASSERT(m_Adiag > 0.0f);
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
JacobianEntry(
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
|
||||
const SimdVector3& jointAxis,
|
||||
const SimdVector3& inertiaInvA,
|
||||
const SimdScalar massInvA)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ= world2A*(rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = SimdVector3(0.f,0.f,0.f);
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
ASSERT(m_Adiag > 0.0f);
|
||||
}
|
||||
|
||||
SimdScalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
|
||||
{
|
||||
const JacobianEntry& jacA = *this;
|
||||
SimdScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
|
||||
SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
|
||||
return lin + ang;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
|
||||
SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
|
||||
{
|
||||
const JacobianEntry& jacA = *this;
|
||||
SimdVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
SimdVector3 lin0 = massInvA * lin ;
|
||||
SimdVector3 lin1 = massInvB * lin;
|
||||
SimdVector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
}
|
||||
|
||||
SimdScalar getRelativeVelocity(const SimdVector3& linvelA,const SimdVector3& angvelA,const SimdVector3& linvelB,const SimdVector3& angvelB)
|
||||
{
|
||||
SimdVector3 linrel = linvelA - linvelB;
|
||||
SimdVector3 angvela = angvelA * m_aJ;
|
||||
SimdVector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
return rel_vel2 + SIMD_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
SimdVector3 m_linearJointAxis;
|
||||
SimdVector3 m_aJ;
|
||||
SimdVector3 m_bJ;
|
||||
SimdVector3 m_0MinvJt;
|
||||
SimdVector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
SimdScalar m_Adiag;
|
||||
|
||||
};
|
||||
|
||||
#endif //JACOBIAN_ENTRY_H
|
||||
116
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
Normal file
116
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
Normal file
@@ -0,0 +1,116 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btPoint2PointConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Dynamics/btMassProps.h"
|
||||
|
||||
|
||||
|
||||
|
||||
Point2PointConstraint::Point2PointConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB)
|
||||
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA)
|
||||
:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void Point2PointConstraint::BuildJacobian()
|
||||
{
|
||||
m_appliedImpulse = 0.f;
|
||||
|
||||
SimdVector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
new (&m_jac[i]) JacobianEntry(
|
||||
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
|
||||
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Point2PointConstraint::SolveConstraint(SimdScalar timeStep)
|
||||
{
|
||||
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
|
||||
|
||||
SimdVector3 normal(0,0,0);
|
||||
|
||||
|
||||
// SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
// SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
|
||||
|
||||
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
|
||||
SimdScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
/*
|
||||
//velocity error (first order error)
|
||||
SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
*/
|
||||
|
||||
//positional error (zeroth order error)
|
||||
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
|
||||
SimdScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse+=impulse;
|
||||
SimdVector3 impulse_vector = normal * impulse;
|
||||
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
|
||||
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Point2PointConstraint::UpdateRHS(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef POINT2POINTCONSTRAINT_H
|
||||
#define POINT2POINTCONSTRAINT_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class RigidBody;
|
||||
|
||||
struct ConstraintSetting
|
||||
{
|
||||
ConstraintSetting() :
|
||||
m_tau(0.3f),
|
||||
m_damping(1.f)
|
||||
{
|
||||
}
|
||||
float m_tau;
|
||||
float m_damping;
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
class Point2PointConstraint : public TypedConstraint
|
||||
{
|
||||
JacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
SimdVector3 m_pivotInA;
|
||||
SimdVector3 m_pivotInB;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
ConstraintSetting m_setting;
|
||||
|
||||
Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB);
|
||||
|
||||
Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA);
|
||||
|
||||
Point2PointConstraint();
|
||||
|
||||
virtual void BuildJacobian();
|
||||
|
||||
|
||||
virtual void SolveConstraint(SimdScalar timeStep);
|
||||
|
||||
void UpdateRHS(SimdScalar timeStep);
|
||||
|
||||
void SetPivotA(const SimdVector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void SetPivotB(const SimdVector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //POINT2POINTCONSTRAINT_H
|
||||
@@ -0,0 +1,361 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btSequentialImpulseConstraintSolver.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btContactConstraint.h"
|
||||
#include "btSolve2LinearConstraint.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "LinearMath/GenIDebugDraw.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "LinearMath/GenMinMax.h"
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
#include "LinearMath/GenQuickprof.h"
|
||||
#endif //USE_PROFILE
|
||||
|
||||
int totalCpd = 0;
|
||||
|
||||
int gTotalContactPoints = 0;
|
||||
|
||||
bool MyContactDestroyedCallback(void* userPersistentData)
|
||||
{
|
||||
assert (userPersistentData);
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData;
|
||||
delete cpd;
|
||||
totalCpd--;
|
||||
//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver()
|
||||
{
|
||||
gContactDestroyedCallback = &MyContactDestroyedCallback;
|
||||
|
||||
//initialize default friction/contact funcs
|
||||
int i,j;
|
||||
for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
|
||||
for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
|
||||
{
|
||||
|
||||
m_contactDispatch[i][j] = resolveSingleCollision;
|
||||
m_frictionDispatch[i][j] = resolveSingleFriction;
|
||||
}
|
||||
}
|
||||
|
||||
/// SequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
ContactSolverInfo info = infoGlobal;
|
||||
|
||||
int numiter = infoGlobal.m_numIterations;
|
||||
#ifdef USE_PROFILE
|
||||
Profiler::beginBlock("Solve");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
//interleaving the preparation with solving impacts the behaviour a lot, todo: find out why
|
||||
|
||||
PrepareConstraints(manifoldPtr[k],info,debugDrawer);
|
||||
Solve(manifoldPtr[k],info,0,debugDrawer);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int i;
|
||||
for ( i = 0;i<numiter-1;i++)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
|
||||
Solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
Profiler::endBlock("Solve");
|
||||
|
||||
Profiler::beginBlock("SolveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
//now solve the friction
|
||||
for (i = 0;i<numiter-1;i++)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k = j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
SolveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
Profiler::endBlock("SolveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
|
||||
float penetrationResolveFactor = 0.9f;
|
||||
SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
|
||||
{
|
||||
SimdScalar rest = restitution * -rel_vel;
|
||||
return rest;
|
||||
}
|
||||
|
||||
|
||||
void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,IDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
|
||||
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
|
||||
|
||||
|
||||
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
|
||||
{
|
||||
manifoldPtr->RefreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
|
||||
|
||||
int numpoints = manifoldPtr->GetNumContacts();
|
||||
|
||||
gTotalContactPoints += numpoints;
|
||||
|
||||
SimdVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
{
|
||||
const SimdVector3& pos1 = cp.GetPositionWorldOnA();
|
||||
const SimdVector3& pos2 = cp.GetPositionWorldOnB();
|
||||
|
||||
SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
|
||||
|
||||
|
||||
//this jacobian entry is re-used for all iterations
|
||||
JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
|
||||
body1->getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
|
||||
body1->getInvInertiaDiagLocal(),body1->getInvMass());
|
||||
|
||||
|
||||
SimdScalar jacDiagAB = jac.getDiagonal();
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
|
||||
if (cpd)
|
||||
{
|
||||
//might be invalid
|
||||
cpd->m_persistentLifeTime++;
|
||||
if (cpd->m_persistentLifeTime != cp.GetLifeTime())
|
||||
{
|
||||
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
new (cpd) ConstraintPersistentData;
|
||||
cpd->m_persistentLifeTime = cp.GetLifeTime();
|
||||
|
||||
} else
|
||||
{
|
||||
//printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
|
||||
cpd = new ConstraintPersistentData();
|
||||
totalCpd ++;
|
||||
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
|
||||
cp.m_userPersistentData = cpd;
|
||||
cpd->m_persistentLifeTime = cp.GetLifeTime();
|
||||
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
|
||||
}
|
||||
assert(cpd);
|
||||
|
||||
cpd->m_jacDiagABInv = 1.f / jacDiagAB;
|
||||
|
||||
//Dependent on Rigidbody A and B types, fetch the contact/friction response func
|
||||
//perhaps do a similar thing for friction/restutution combiner funcs...
|
||||
|
||||
cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
|
||||
cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
|
||||
|
||||
SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
SimdScalar rel_vel;
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
float combinedRestitution = cp.m_combinedRestitution;
|
||||
|
||||
cpd->m_penetration = cp.GetDistance();
|
||||
cpd->m_friction = cp.m_combinedFriction;
|
||||
cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
|
||||
if (cpd->m_restitution <= 0.) //0.f)
|
||||
{
|
||||
cpd->m_restitution = 0.0f;
|
||||
|
||||
};
|
||||
|
||||
//restitution and penetration work in same direction so
|
||||
//rel_vel
|
||||
|
||||
SimdScalar penVel = -cpd->m_penetration/info.m_timeStep;
|
||||
|
||||
if (cpd->m_restitution >= penVel)
|
||||
{
|
||||
cpd->m_penetration = 0.f;
|
||||
}
|
||||
|
||||
|
||||
float relaxation = info.m_damping;
|
||||
cpd->m_appliedImpulse *= relaxation;
|
||||
//for friction
|
||||
cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
|
||||
|
||||
//re-calculate friction direction every frame, todo: check if this is really needed
|
||||
SimdPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
|
||||
|
||||
|
||||
#define NO_FRICTION_WARMSTART 1
|
||||
|
||||
#ifdef NO_FRICTION_WARMSTART
|
||||
cpd->m_accumulatedTangentImpulse0 = 0.f;
|
||||
cpd->m_accumulatedTangentImpulse1 = 0.f;
|
||||
#endif //NO_FRICTION_WARMSTART
|
||||
float denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
|
||||
float denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
|
||||
float denom = relaxation/(denom0+denom1);
|
||||
cpd->m_jacDiagABInvTangent0 = denom;
|
||||
|
||||
|
||||
denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
|
||||
denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
|
||||
denom = relaxation/(denom0+denom1);
|
||||
cpd->m_jacDiagABInvTangent1 = denom;
|
||||
|
||||
SimdVector3 totalImpulse =
|
||||
#ifndef NO_FRICTION_WARMSTART
|
||||
cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
|
||||
cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
|
||||
#endif //NO_FRICTION_WARMSTART
|
||||
cp.m_normalWorldOnB*cpd->m_appliedImpulse;
|
||||
|
||||
//apply previous frames impulse on both bodies
|
||||
body0->applyImpulse(totalImpulse, rel_pos1);
|
||||
body1->applyImpulse(-totalImpulse, rel_pos2);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
|
||||
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
|
||||
|
||||
float maxImpulse = 0.f;
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->GetNumContacts();
|
||||
|
||||
SimdVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
|
||||
int j=i;
|
||||
if (iter % 2)
|
||||
j = numpoints-1-i;
|
||||
else
|
||||
j=i;
|
||||
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
{
|
||||
|
||||
if (iter == 0)
|
||||
{
|
||||
if (debugDrawer)
|
||||
debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
|
||||
float impulse = cpd->m_contactSolverFunc(
|
||||
*body0,*body1,
|
||||
cp,
|
||||
info);
|
||||
|
||||
if (maxImpulse < impulse)
|
||||
maxImpulse = impulse;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return maxImpulse;
|
||||
}
|
||||
|
||||
float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
|
||||
{
|
||||
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
|
||||
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
|
||||
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->GetNumContacts();
|
||||
|
||||
SimdVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
|
||||
int j=i;
|
||||
//if (iter % 2)
|
||||
// j = numpoints-1-i;
|
||||
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
{
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
|
||||
cpd->m_frictionSolverFunc(
|
||||
*body0,*body1,
|
||||
cp,
|
||||
info);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
return 0.f;
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "btConstraintSolver.h"
|
||||
class IDebugDraw;
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
|
||||
|
||||
|
||||
/// SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
|
||||
/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
|
||||
/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
|
||||
/// Applies impulses for combined restitution and penetration recovery and to simulate friction
|
||||
class SequentialImpulseConstraintSolver : public ConstraintSolver
|
||||
{
|
||||
float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
|
||||
float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
|
||||
void PrepareConstraints(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,IDebugDraw* debugDrawer);
|
||||
|
||||
ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
|
||||
public:
|
||||
|
||||
SequentialImpulseConstraintSolver();
|
||||
|
||||
///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
|
||||
///See RigidBody::m_contactSolverType and RigidBody::m_frictionSolverType
|
||||
void SetContactSolverFunc(ContactSolverFunc func,int type0,int type1)
|
||||
{
|
||||
m_contactDispatch[type0][type1] = func;
|
||||
}
|
||||
|
||||
///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
|
||||
///See RigidBody::m_contactSolverType and RigidBody::m_frictionSolverType
|
||||
void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
|
||||
{
|
||||
m_frictionDispatch[type0][type1] = func;
|
||||
}
|
||||
|
||||
virtual ~SequentialImpulseConstraintSolver() {}
|
||||
|
||||
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);
|
||||
|
||||
};
|
||||
|
||||
#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
241
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
Normal file
241
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
Normal file
@@ -0,0 +1,241 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btSolve2LinearConstraint.h"
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
|
||||
|
||||
void Solve2LinearConstraint::resolveUnilateralPairConstraint(
|
||||
RigidBody* body1,
|
||||
RigidBody* body2,
|
||||
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdMatrix3x3& world2B,
|
||||
|
||||
const SimdVector3& invInertiaADiag,
|
||||
const SimdScalar invMassA,
|
||||
const SimdVector3& linvelA,const SimdVector3& angvelA,
|
||||
const SimdVector3& rel_posA1,
|
||||
const SimdVector3& invInertiaBDiag,
|
||||
const SimdScalar invMassB,
|
||||
const SimdVector3& linvelB,const SimdVector3& angvelB,
|
||||
const SimdVector3& rel_posA2,
|
||||
|
||||
SimdScalar depthA, const SimdVector3& normalA,
|
||||
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
|
||||
SimdScalar depthB, const SimdVector3& normalB,
|
||||
SimdScalar& imp0,SimdScalar& imp1)
|
||||
{
|
||||
|
||||
imp0 = 0.f;
|
||||
imp1 = 0.f;
|
||||
|
||||
SimdScalar len = fabs(normalA.length())-1.f;
|
||||
if (fabs(len) >= SIMD_EPSILON)
|
||||
return;
|
||||
|
||||
ASSERT(len < SIMD_EPSILON);
|
||||
|
||||
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
|
||||
//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
|
||||
const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
|
||||
|
||||
// SimdScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
|
||||
SimdScalar massTerm = 1.f / (invMassA + invMassB);
|
||||
|
||||
|
||||
// calculate rhs (or error) terms
|
||||
const SimdScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
|
||||
const SimdScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
|
||||
|
||||
|
||||
// dC/dv * dv = -C
|
||||
|
||||
// jacobian * impulse = -error
|
||||
//
|
||||
|
||||
//impulse = jacobianInverse * -error
|
||||
|
||||
// inverting 2x2 symmetric system (offdiagonal are equal!)
|
||||
//
|
||||
|
||||
|
||||
SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
|
||||
SimdScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
|
||||
|
||||
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
//[a b] [d -c]
|
||||
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
|
||||
|
||||
//[jA nD] * [imp0] = [dv0]
|
||||
//[nD jB] [imp1] [dv1]
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Solve2LinearConstraint::resolveBilateralPairConstraint(
|
||||
RigidBody* body1,
|
||||
RigidBody* body2,
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdMatrix3x3& world2B,
|
||||
|
||||
const SimdVector3& invInertiaADiag,
|
||||
const SimdScalar invMassA,
|
||||
const SimdVector3& linvelA,const SimdVector3& angvelA,
|
||||
const SimdVector3& rel_posA1,
|
||||
const SimdVector3& invInertiaBDiag,
|
||||
const SimdScalar invMassB,
|
||||
const SimdVector3& linvelB,const SimdVector3& angvelB,
|
||||
const SimdVector3& rel_posA2,
|
||||
|
||||
SimdScalar depthA, const SimdVector3& normalA,
|
||||
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
|
||||
SimdScalar depthB, const SimdVector3& normalB,
|
||||
SimdScalar& imp0,SimdScalar& imp1)
|
||||
{
|
||||
|
||||
imp0 = 0.f;
|
||||
imp1 = 0.f;
|
||||
|
||||
SimdScalar len = fabs(normalA.length())-1.f;
|
||||
if (fabs(len) >= SIMD_EPSILON)
|
||||
return;
|
||||
|
||||
ASSERT(len < SIMD_EPSILON);
|
||||
|
||||
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
|
||||
invInertiaBDiag,invMassB);
|
||||
|
||||
//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
|
||||
|
||||
const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
|
||||
const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
|
||||
|
||||
// calculate rhs (or error) terms
|
||||
const SimdScalar dv0 = depthA * m_tau - vel0 * m_damping;
|
||||
const SimdScalar dv1 = depthB * m_tau - vel1 * m_damping;
|
||||
|
||||
// dC/dv * dv = -C
|
||||
|
||||
// jacobian * impulse = -error
|
||||
//
|
||||
|
||||
//impulse = jacobianInverse * -error
|
||||
|
||||
// inverting 2x2 symmetric system (offdiagonal are equal!)
|
||||
//
|
||||
|
||||
|
||||
SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
|
||||
SimdScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
|
||||
|
||||
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
|
||||
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
|
||||
|
||||
//[a b] [d -c]
|
||||
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
|
||||
|
||||
//[jA nD] * [imp0] = [dv0]
|
||||
//[nD jB] [imp1] [dv1]
|
||||
|
||||
if ( imp0 > 0.0f)
|
||||
{
|
||||
if ( imp1 > 0.0f )
|
||||
{
|
||||
//both positive
|
||||
}
|
||||
else
|
||||
{
|
||||
imp1 = 0.f;
|
||||
|
||||
// now imp0>0 imp1<0
|
||||
imp0 = dv0 / jacA.getDiagonal();
|
||||
if ( imp0 > 0.0f )
|
||||
{
|
||||
} else
|
||||
{
|
||||
imp0 = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
imp0 = 0.f;
|
||||
|
||||
imp1 = dv1 / jacB.getDiagonal();
|
||||
if ( imp1 <= 0.0f )
|
||||
{
|
||||
imp1 = 0.f;
|
||||
// now imp0>0 imp1<0
|
||||
imp0 = dv0 / jacA.getDiagonal();
|
||||
if ( imp0 > 0.0f )
|
||||
{
|
||||
} else
|
||||
{
|
||||
imp0 = 0.f;
|
||||
}
|
||||
} else
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Solve2LinearConstraint::resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS,
|
||||
const SimdScalar invMassA,
|
||||
const SimdVector3& linvelA,const SimdVector3& angvelA,
|
||||
const SimdVector3& rel_posA1,
|
||||
const SimdMatrix3x3& invInertiaBWS,
|
||||
const SimdScalar invMassB,
|
||||
const SimdVector3& linvelB,const SimdVector3& angvelB,
|
||||
const SimdVector3& rel_posA2,
|
||||
|
||||
SimdScalar depthA, const SimdVector3& normalA,
|
||||
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
|
||||
SimdScalar depthB, const SimdVector3& normalB,
|
||||
SimdScalar& imp0,SimdScalar& imp1)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
106
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
Normal file
106
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
Normal file
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef SOLVE_2LINEAR_CONSTRAINT_H
|
||||
#define SOLVE_2LINEAR_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/SimdMatrix3x3.h"
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
|
||||
|
||||
class RigidBody;
|
||||
|
||||
|
||||
|
||||
/// constraint class used for lateral tyre friction.
|
||||
class Solve2LinearConstraint
|
||||
{
|
||||
SimdScalar m_tau;
|
||||
SimdScalar m_damping;
|
||||
|
||||
public:
|
||||
|
||||
Solve2LinearConstraint(SimdScalar tau,SimdScalar damping)
|
||||
{
|
||||
m_tau = tau;
|
||||
m_damping = damping;
|
||||
}
|
||||
//
|
||||
// solve unilateral constraint (equality, direct method)
|
||||
//
|
||||
void resolveUnilateralPairConstraint(
|
||||
RigidBody* body0,
|
||||
RigidBody* body1,
|
||||
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdMatrix3x3& world2B,
|
||||
|
||||
const SimdVector3& invInertiaADiag,
|
||||
const SimdScalar invMassA,
|
||||
const SimdVector3& linvelA,const SimdVector3& angvelA,
|
||||
const SimdVector3& rel_posA1,
|
||||
const SimdVector3& invInertiaBDiag,
|
||||
const SimdScalar invMassB,
|
||||
const SimdVector3& linvelB,const SimdVector3& angvelB,
|
||||
const SimdVector3& rel_posA2,
|
||||
|
||||
SimdScalar depthA, const SimdVector3& normalA,
|
||||
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
|
||||
SimdScalar depthB, const SimdVector3& normalB,
|
||||
SimdScalar& imp0,SimdScalar& imp1);
|
||||
|
||||
|
||||
//
|
||||
// solving 2x2 lcp problem (inequality, direct solution )
|
||||
//
|
||||
void resolveBilateralPairConstraint(
|
||||
RigidBody* body0,
|
||||
RigidBody* body1,
|
||||
const SimdMatrix3x3& world2A,
|
||||
const SimdMatrix3x3& world2B,
|
||||
|
||||
const SimdVector3& invInertiaADiag,
|
||||
const SimdScalar invMassA,
|
||||
const SimdVector3& linvelA,const SimdVector3& angvelA,
|
||||
const SimdVector3& rel_posA1,
|
||||
const SimdVector3& invInertiaBDiag,
|
||||
const SimdScalar invMassB,
|
||||
const SimdVector3& linvelB,const SimdVector3& angvelB,
|
||||
const SimdVector3& rel_posA2,
|
||||
|
||||
SimdScalar depthA, const SimdVector3& normalA,
|
||||
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
|
||||
SimdScalar depthB, const SimdVector3& normalB,
|
||||
SimdScalar& imp0,SimdScalar& imp1);
|
||||
|
||||
|
||||
void resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS,
|
||||
const SimdScalar invMassA,
|
||||
const SimdVector3& linvelA,const SimdVector3& angvelA,
|
||||
const SimdVector3& rel_posA1,
|
||||
const SimdMatrix3x3& invInertiaBWS,
|
||||
const SimdScalar invMassB,
|
||||
const SimdVector3& linvelB,const SimdVector3& angvelB,
|
||||
const SimdVector3& rel_posA2,
|
||||
|
||||
SimdScalar depthA, const SimdVector3& normalA,
|
||||
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
|
||||
SimdScalar depthB, const SimdVector3& normalB,
|
||||
SimdScalar& imp0,SimdScalar& imp1);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //SOLVE_2LINEAR_CONSTRAINT_H
|
||||
54
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
Normal file
54
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
Normal file
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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 "btTypedConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Dynamics/btMassProps.h"
|
||||
|
||||
static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f);
|
||||
|
||||
TypedConstraint::TypedConstraint()
|
||||
: m_userConstraintType(-1),
|
||||
m_userConstraintId(-1),
|
||||
m_rbA(s_fixed),
|
||||
m_rbB(s_fixed),
|
||||
m_appliedImpulse(0.f)
|
||||
{
|
||||
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
|
||||
}
|
||||
TypedConstraint::TypedConstraint(RigidBody& rbA)
|
||||
: m_userConstraintType(-1),
|
||||
m_userConstraintId(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(s_fixed),
|
||||
m_appliedImpulse(0.f)
|
||||
{
|
||||
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
|
||||
|
||||
}
|
||||
|
||||
|
||||
TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB)
|
||||
: m_userConstraintType(-1),
|
||||
m_userConstraintId(-1),
|
||||
m_rbA(rbA),
|
||||
m_rbB(rbB),
|
||||
m_appliedImpulse(0.f)
|
||||
{
|
||||
s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
|
||||
|
||||
}
|
||||
|
||||
90
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
Normal file
90
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
Normal file
@@ -0,0 +1,90 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef TYPED_CONSTRAINT_H
|
||||
#define TYPED_CONSTRAINT_H
|
||||
|
||||
class RigidBody;
|
||||
#include "LinearMath/SimdScalar.h"
|
||||
|
||||
//TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
class TypedConstraint
|
||||
{
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
|
||||
|
||||
protected:
|
||||
RigidBody& m_rbA;
|
||||
RigidBody& m_rbB;
|
||||
float m_appliedImpulse;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
TypedConstraint();
|
||||
virtual ~TypedConstraint() {};
|
||||
TypedConstraint(RigidBody& rbA);
|
||||
|
||||
TypedConstraint(RigidBody& rbA,RigidBody& rbB);
|
||||
|
||||
virtual void BuildJacobian() = 0;
|
||||
|
||||
virtual void SolveConstraint(SimdScalar timeStep) = 0;
|
||||
|
||||
const RigidBody& GetRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const RigidBody& GetRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
RigidBody& GetRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
RigidBody& GetRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
int GetUserConstraintType() const
|
||||
{
|
||||
return m_userConstraintType ;
|
||||
}
|
||||
|
||||
void SetUserConstraintType(int userConstraintType)
|
||||
{
|
||||
m_userConstraintType = userConstraintType;
|
||||
};
|
||||
|
||||
void SetUserConstraintId(int uid)
|
||||
{
|
||||
m_userConstraintId = uid;
|
||||
}
|
||||
|
||||
int GetUserConstraintId()
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
float GetAppliedImpulse()
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //TYPED_CONSTRAINT_H
|
||||
Reference in New Issue
Block a user