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
17
src/BulletDynamics/CMakeLists.txt
Normal file
17
src/BulletDynamics/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/src }
|
||||
)
|
||||
|
||||
ADD_LIBRARY(LibBulletDynamics
|
||||
|
||||
ConstraintSolver/btContactConstraint.cpp
|
||||
ConstraintSolver/btGeneric6DofConstraint.cpp
|
||||
ConstraintSolver/btHingeConstraint.cpp
|
||||
ConstraintSolver/btPoint2PointConstraint.cpp
|
||||
ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
|
||||
ConstraintSolver/btSolve2LinearConstraint.cpp
|
||||
ConstraintSolver/btTypedConstraint.cpp
|
||||
Dynamics/btRigidBody.cpp
|
||||
Vehicle/btRaycastVehicle.cpp
|
||||
Vehicle/btWheelInfo.cpp
|
||||
)
|
||||
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
|
||||
33
src/BulletDynamics/Dynamics/btMassProps.h
Normal file
33
src/BulletDynamics/Dynamics/btMassProps.h
Normal file
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
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 MASS_PROPS_H
|
||||
#define MASS_PROPS_H
|
||||
|
||||
#include <LinearMath/SimdVector3.h>
|
||||
|
||||
struct MassProps {
|
||||
MassProps(float mass,const SimdVector3& inertiaLocal):
|
||||
m_mass(mass),
|
||||
m_inertiaLocal(inertiaLocal)
|
||||
{
|
||||
}
|
||||
float m_mass;
|
||||
SimdVector3 m_inertiaLocal;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
225
src/BulletDynamics/Dynamics/btRigidBody.cpp
Normal file
225
src/BulletDynamics/Dynamics/btRigidBody.cpp
Normal file
@@ -0,0 +1,225 @@
|
||||
/*
|
||||
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 "btRigidBody.h"
|
||||
#include "btMassProps.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
#include "LinearMath/GenMinMax.h"
|
||||
#include <LinearMath/SimdTransformUtil.h>
|
||||
|
||||
float gLinearAirDamping = 1.f;
|
||||
|
||||
static int uniqueId = 0;
|
||||
|
||||
RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
|
||||
:
|
||||
m_gravity(0.0f, 0.0f, 0.0f),
|
||||
m_totalForce(0.0f, 0.0f, 0.0f),
|
||||
m_totalTorque(0.0f, 0.0f, 0.0f),
|
||||
m_linearVelocity(0.0f, 0.0f, 0.0f),
|
||||
m_angularVelocity(0.f,0.f,0.f),
|
||||
m_linearDamping(0.f),
|
||||
m_angularDamping(0.5f),
|
||||
m_kinematicTimeStep(0.f),
|
||||
m_contactSolverType(0),
|
||||
m_frictionSolverType(0)
|
||||
{
|
||||
|
||||
//moved to CollisionObject
|
||||
m_friction = friction;
|
||||
m_restitution = restitution;
|
||||
|
||||
m_debugBodyId = uniqueId++;
|
||||
|
||||
setMassProps(massProps.m_mass, massProps.m_inertiaLocal);
|
||||
setDamping(linearDamping, angularDamping);
|
||||
m_worldTransform.setIdentity();
|
||||
updateInertiaTensor();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::setLinearVelocity(const SimdVector3& lin_vel)
|
||||
{
|
||||
|
||||
m_linearVelocity = lin_vel;
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::predictIntegratedTransform(SimdScalar timeStep,SimdTransform& predictedTransform) const
|
||||
{
|
||||
SimdTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
|
||||
}
|
||||
|
||||
void RigidBody::saveKinematicState(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
if (m_kinematicTimeStep)
|
||||
{
|
||||
SimdVector3 linVel,angVel;
|
||||
SimdTransformUtil::CalculateVelocity(m_interpolationWorldTransform,m_worldTransform,m_kinematicTimeStep,m_linearVelocity,m_angularVelocity);
|
||||
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
|
||||
}
|
||||
|
||||
|
||||
m_interpolationWorldTransform = m_worldTransform;
|
||||
|
||||
m_kinematicTimeStep = timeStep;
|
||||
}
|
||||
|
||||
void RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
|
||||
{
|
||||
GetCollisionShape()->GetAabb(m_worldTransform,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void RigidBody::setGravity(const SimdVector3& acceleration)
|
||||
{
|
||||
if (m_inverseMass != 0.0f)
|
||||
{
|
||||
m_gravity = acceleration * (1.0f / m_inverseMass);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
|
||||
{
|
||||
m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f);
|
||||
m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void RigidBody::applyForces(SimdScalar step)
|
||||
{
|
||||
if (IsStatic())
|
||||
return;
|
||||
|
||||
|
||||
applyCentralForce(m_gravity);
|
||||
|
||||
m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f);
|
||||
m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
|
||||
|
||||
#define FORCE_VELOCITY_DAMPING 1
|
||||
#ifdef FORCE_VELOCITY_DAMPING
|
||||
float speed = m_linearVelocity.length();
|
||||
if (speed < m_linearDamping)
|
||||
{
|
||||
float dampVel = 0.005f;
|
||||
if (speed > dampVel)
|
||||
{
|
||||
SimdVector3 dir = m_linearVelocity.normalized();
|
||||
m_linearVelocity -= dir * dampVel;
|
||||
} else
|
||||
{
|
||||
m_linearVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
|
||||
float angSpeed = m_angularVelocity.length();
|
||||
if (angSpeed < m_angularDamping)
|
||||
{
|
||||
float angDampVel = 0.005f;
|
||||
if (angSpeed > angDampVel)
|
||||
{
|
||||
SimdVector3 dir = m_angularVelocity.normalized();
|
||||
m_angularVelocity -= dir * angDampVel;
|
||||
} else
|
||||
{
|
||||
m_angularVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
}
|
||||
#endif //FORCE_VELOCITY_DAMPING
|
||||
|
||||
}
|
||||
|
||||
void RigidBody::proceedToTransform(const SimdTransform& newTrans)
|
||||
{
|
||||
setCenterOfMassTransform( newTrans );
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia)
|
||||
{
|
||||
if (mass == 0.f)
|
||||
{
|
||||
m_collisionFlags = CollisionObject::isStatic;
|
||||
m_inverseMass = 0.f;
|
||||
} else
|
||||
{
|
||||
m_collisionFlags = 0;
|
||||
m_inverseMass = 1.0f / mass;
|
||||
}
|
||||
|
||||
|
||||
m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f,
|
||||
inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f,
|
||||
inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void RigidBody::updateInertiaTensor()
|
||||
{
|
||||
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::integrateVelocities(SimdScalar step)
|
||||
{
|
||||
if (IsStatic())
|
||||
return;
|
||||
|
||||
m_linearVelocity += m_totalForce * (m_inverseMass * step);
|
||||
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
|
||||
|
||||
#define MAX_ANGVEL SIMD_HALF_PI
|
||||
/// clamp angular velocity. collision calculations will fail on higher angular velocities
|
||||
float angvel = m_angularVelocity.length();
|
||||
if (angvel*step > MAX_ANGVEL)
|
||||
{
|
||||
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
|
||||
}
|
||||
|
||||
clearForces();
|
||||
}
|
||||
|
||||
SimdQuaternion RigidBody::getOrientation() const
|
||||
{
|
||||
SimdQuaternion orn;
|
||||
m_worldTransform.getBasis().getRotation(orn);
|
||||
return orn;
|
||||
}
|
||||
|
||||
|
||||
void RigidBody::setCenterOfMassTransform(const SimdTransform& xform)
|
||||
{
|
||||
m_worldTransform = xform;
|
||||
updateInertiaTensor();
|
||||
}
|
||||
|
||||
|
||||
|
||||
261
src/BulletDynamics/Dynamics/btRigidBody.h
Normal file
261
src/BulletDynamics/Dynamics/btRigidBody.h
Normal file
@@ -0,0 +1,261 @@
|
||||
/*
|
||||
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 RIGIDBODY_H
|
||||
#define RIGIDBODY_H
|
||||
|
||||
#include <vector>
|
||||
#include <LinearMath/SimdPoint3.h>
|
||||
#include <LinearMath/SimdTransform.h>
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
class CollisionShape;
|
||||
struct MassProps;
|
||||
typedef SimdScalar dMatrix3[4*3];
|
||||
|
||||
extern float gLinearAirDamping;
|
||||
extern bool gUseEpa;
|
||||
|
||||
|
||||
|
||||
/// RigidBody class for RigidBody Dynamics
|
||||
///
|
||||
class RigidBody : public CollisionObject
|
||||
{
|
||||
public:
|
||||
|
||||
RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution);
|
||||
|
||||
void proceedToTransform(const SimdTransform& newTrans);
|
||||
|
||||
|
||||
/// continuous collision detection needs prediction
|
||||
void predictIntegratedTransform(SimdScalar step, SimdTransform& predictedTransform) const;
|
||||
|
||||
void saveKinematicState(SimdScalar step);
|
||||
|
||||
|
||||
void applyForces(SimdScalar step);
|
||||
|
||||
void setGravity(const SimdVector3& acceleration);
|
||||
|
||||
void setDamping(SimdScalar lin_damping, SimdScalar ang_damping);
|
||||
|
||||
inline const CollisionShape* GetCollisionShape() const {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
inline CollisionShape* GetCollisionShape() {
|
||||
return m_collisionShape;
|
||||
}
|
||||
|
||||
void setMassProps(SimdScalar mass, const SimdVector3& inertia);
|
||||
|
||||
SimdScalar getInvMass() const { return m_inverseMass; }
|
||||
const SimdMatrix3x3& getInvInertiaTensorWorld() const {
|
||||
return m_invInertiaTensorWorld;
|
||||
}
|
||||
|
||||
void integrateVelocities(SimdScalar step);
|
||||
|
||||
void setCenterOfMassTransform(const SimdTransform& xform);
|
||||
|
||||
void applyCentralForce(const SimdVector3& force)
|
||||
{
|
||||
m_totalForce += force;
|
||||
}
|
||||
|
||||
const SimdVector3& getInvInertiaDiagLocal()
|
||||
{
|
||||
return m_invInertiaLocal;
|
||||
};
|
||||
|
||||
void setInvInertiaDiagLocal(const SimdVector3& diagInvInertia)
|
||||
{
|
||||
m_invInertiaLocal = diagInvInertia;
|
||||
}
|
||||
|
||||
void applyTorque(const SimdVector3& torque)
|
||||
{
|
||||
m_totalTorque += torque;
|
||||
}
|
||||
|
||||
void applyForce(const SimdVector3& force, const SimdVector3& rel_pos)
|
||||
{
|
||||
applyCentralForce(force);
|
||||
applyTorque(rel_pos.cross(force));
|
||||
}
|
||||
|
||||
void applyCentralImpulse(const SimdVector3& impulse)
|
||||
{
|
||||
m_linearVelocity += impulse * m_inverseMass;
|
||||
}
|
||||
|
||||
void applyTorqueImpulse(const SimdVector3& torque)
|
||||
{
|
||||
if (!IsStatic())
|
||||
m_angularVelocity += m_invInertiaTensorWorld * torque;
|
||||
|
||||
}
|
||||
|
||||
void applyImpulse(const SimdVector3& impulse, const SimdVector3& rel_pos)
|
||||
{
|
||||
if (m_inverseMass != 0.f)
|
||||
{
|
||||
applyCentralImpulse(impulse);
|
||||
applyTorqueImpulse(rel_pos.cross(impulse));
|
||||
}
|
||||
}
|
||||
|
||||
void clearForces()
|
||||
{
|
||||
m_totalForce.setValue(0.0f, 0.0f, 0.0f);
|
||||
m_totalTorque.setValue(0.0f, 0.0f, 0.0f);
|
||||
}
|
||||
|
||||
void updateInertiaTensor();
|
||||
|
||||
const SimdPoint3& getCenterOfMassPosition() const {
|
||||
return m_worldTransform.getOrigin();
|
||||
}
|
||||
SimdQuaternion getOrientation() const;
|
||||
|
||||
const SimdTransform& getCenterOfMassTransform() const {
|
||||
return m_worldTransform;
|
||||
}
|
||||
const SimdVector3& getLinearVelocity() const {
|
||||
return m_linearVelocity;
|
||||
}
|
||||
const SimdVector3& getAngularVelocity() const {
|
||||
return m_angularVelocity;
|
||||
}
|
||||
|
||||
|
||||
void setLinearVelocity(const SimdVector3& lin_vel);
|
||||
void setAngularVelocity(const SimdVector3& ang_vel) {
|
||||
if (!IsStatic())
|
||||
{
|
||||
m_angularVelocity = ang_vel;
|
||||
}
|
||||
}
|
||||
|
||||
SimdVector3 getVelocityInLocalPoint(const SimdVector3& rel_pos) const
|
||||
{
|
||||
//we also calculate lin/ang velocity for kinematic objects
|
||||
return m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
|
||||
//for kinematic objects, we could also use use:
|
||||
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
|
||||
}
|
||||
|
||||
void translate(const SimdVector3& v)
|
||||
{
|
||||
m_worldTransform.getOrigin() += v;
|
||||
}
|
||||
|
||||
|
||||
void getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline float ComputeImpulseDenominator(const SimdPoint3& pos, const SimdVector3& normal) const
|
||||
{
|
||||
SimdVector3 r0 = pos - getCenterOfMassPosition();
|
||||
|
||||
SimdVector3 c0 = (r0).cross(normal);
|
||||
|
||||
SimdVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
|
||||
|
||||
return m_inverseMass + normal.dot(vec);
|
||||
|
||||
}
|
||||
|
||||
inline float ComputeAngularImpulseDenominator(const SimdVector3& axis) const
|
||||
{
|
||||
SimdVector3 vec = axis * getInvInertiaTensorWorld();
|
||||
return axis.dot(vec);
|
||||
}
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
SimdMatrix3x3 m_invInertiaTensorWorld;
|
||||
SimdVector3 m_gravity;
|
||||
SimdVector3 m_invInertiaLocal;
|
||||
SimdVector3 m_totalForce;
|
||||
SimdVector3 m_totalTorque;
|
||||
// SimdQuaternion m_orn1;
|
||||
|
||||
SimdVector3 m_linearVelocity;
|
||||
|
||||
SimdVector3 m_angularVelocity;
|
||||
|
||||
SimdScalar m_linearDamping;
|
||||
SimdScalar m_angularDamping;
|
||||
SimdScalar m_inverseMass;
|
||||
|
||||
|
||||
SimdScalar m_kinematicTimeStep;
|
||||
|
||||
BroadphaseProxy* m_broadphaseProxy;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
public:
|
||||
const BroadphaseProxy* GetBroadphaseProxy() const
|
||||
{
|
||||
return m_broadphaseProxy;
|
||||
}
|
||||
BroadphaseProxy* GetBroadphaseProxy()
|
||||
{
|
||||
return m_broadphaseProxy;
|
||||
}
|
||||
void SetBroadphaseProxy(BroadphaseProxy* broadphaseProxy)
|
||||
{
|
||||
m_broadphaseProxy = broadphaseProxy;
|
||||
}
|
||||
|
||||
//for experimental overriding of friction/contact solver func
|
||||
int m_contactSolverType;
|
||||
int m_frictionSolverType;
|
||||
|
||||
|
||||
/// for ode solver-binding
|
||||
dMatrix3 m_R;//temp
|
||||
dMatrix3 m_I;
|
||||
dMatrix3 m_invI;
|
||||
|
||||
int m_odeTag;
|
||||
|
||||
SimdVector3 m_tacc;//temp
|
||||
SimdVector3 m_facc;
|
||||
|
||||
|
||||
|
||||
int m_debugBodyId;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
9
src/BulletDynamics/Jamfile
Normal file
9
src/BulletDynamics/Jamfile
Normal file
@@ -0,0 +1,9 @@
|
||||
SubDir TOP src BulletDynamics ;
|
||||
|
||||
Description bulletdynamics : "Bullet Rigidbody Dynamics" ;
|
||||
Library bulletdynamics :
|
||||
[ Wildcard ConstraintSolver : *.h *.cpp ]
|
||||
[ Wildcard Dynamics : *.h *.cpp ]
|
||||
[ Wildcard Vehicle : *.h *.cpp ]
|
||||
;
|
||||
Recurse InstallHeader : .h ;
|
||||
596
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
Normal file
596
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
Normal file
@@ -0,0 +1,596 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
|
||||
#include "btRaycastVehicle.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
|
||||
#include "LinearMath/SimdQuaternion.h"
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "btVehicleRaycaster.h"
|
||||
#include "btWheelInfo.h"
|
||||
|
||||
|
||||
#include "BulletDynamics/Dynamics/btMassProps.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
|
||||
|
||||
|
||||
|
||||
static RigidBody s_fixedObject( MassProps ( 0.0f, SimdVector3(0,0,0) ),0.f,0.f,0.f,0.f);
|
||||
|
||||
RaycastVehicle::RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis, VehicleRaycaster* raycaster )
|
||||
:m_vehicleRaycaster(raycaster),
|
||||
m_pitchControl(0.f)
|
||||
{
|
||||
m_chassisBody = chassis;
|
||||
m_indexRightAxis = 0;
|
||||
m_indexUpAxis = 2;
|
||||
m_indexForwardAxis = 1;
|
||||
DefaultInit(tuning);
|
||||
}
|
||||
|
||||
|
||||
void RaycastVehicle::DefaultInit(const VehicleTuning& tuning)
|
||||
{
|
||||
m_currentVehicleSpeedKmHour = 0.f;
|
||||
m_steeringValue = 0.f;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
RaycastVehicle::~RaycastVehicle()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
|
||||
//
|
||||
WheelInfo& RaycastVehicle::AddWheel( const SimdVector3& connectionPointCS, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS, SimdScalar suspensionRestLength, SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel)
|
||||
{
|
||||
|
||||
WheelInfoConstructionInfo ci;
|
||||
|
||||
ci.m_chassisConnectionCS = connectionPointCS;
|
||||
ci.m_wheelDirectionCS = wheelDirectionCS0;
|
||||
ci.m_wheelAxleCS = wheelAxleCS;
|
||||
ci.m_suspensionRestLength = suspensionRestLength;
|
||||
ci.m_wheelRadius = wheelRadius;
|
||||
ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
|
||||
ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
|
||||
ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
|
||||
ci.m_frictionSlip = tuning.m_frictionSlip;
|
||||
ci.m_bIsFrontWheel = isFrontWheel;
|
||||
ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
|
||||
|
||||
m_wheelInfo.push_back( WheelInfo(ci));
|
||||
|
||||
WheelInfo& wheel = m_wheelInfo[GetNumWheels()-1];
|
||||
|
||||
UpdateWheelTransformsWS( wheel );
|
||||
UpdateWheelTransform(GetNumWheels()-1);
|
||||
return wheel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
const SimdTransform& RaycastVehicle::GetWheelTransformWS( int wheelIndex ) const
|
||||
{
|
||||
assert(wheelIndex < GetNumWheels());
|
||||
const WheelInfo& wheel = m_wheelInfo[wheelIndex];
|
||||
return wheel.m_worldTransform;
|
||||
|
||||
}
|
||||
|
||||
void RaycastVehicle::UpdateWheelTransform( int wheelIndex )
|
||||
{
|
||||
|
||||
WheelInfo& wheel = m_wheelInfo[ wheelIndex ];
|
||||
UpdateWheelTransformsWS(wheel);
|
||||
SimdVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
const SimdVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
|
||||
SimdVector3 fwd = up.cross(right);
|
||||
fwd = fwd.normalize();
|
||||
//rotate around steering over de wheelAxleWS
|
||||
float steering = wheel.m_steering;
|
||||
|
||||
SimdQuaternion steeringOrn(up,steering);//wheel.m_steering);
|
||||
SimdMatrix3x3 steeringMat(steeringOrn);
|
||||
|
||||
SimdQuaternion rotatingOrn(right,wheel.m_rotation);
|
||||
SimdMatrix3x3 rotatingMat(rotatingOrn);
|
||||
|
||||
SimdMatrix3x3 basis2(
|
||||
right[0],fwd[0],up[0],
|
||||
right[1],fwd[1],up[1],
|
||||
right[2],fwd[2],up[2]
|
||||
);
|
||||
|
||||
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
|
||||
wheel.m_worldTransform.setOrigin(
|
||||
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
|
||||
);
|
||||
}
|
||||
|
||||
void RaycastVehicle::ResetSuspension()
|
||||
{
|
||||
|
||||
std::vector<WheelInfo>::iterator wheelIt;
|
||||
for (wheelIt = m_wheelInfo.begin();
|
||||
!(wheelIt == m_wheelInfo.end());wheelIt++)
|
||||
{
|
||||
WheelInfo& wheel = *wheelIt;
|
||||
wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
|
||||
wheel.m_suspensionRelativeVelocity = 0.0f;
|
||||
|
||||
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
//wheel_info.setContactFriction(0.0f);
|
||||
wheel.m_clippedInvContactDotSuspension = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void RaycastVehicle::UpdateWheelTransformsWS(WheelInfo& wheel )
|
||||
{
|
||||
wheel.m_raycastInfo.m_isInContact = false;
|
||||
|
||||
const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
|
||||
|
||||
wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
|
||||
wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
|
||||
wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
|
||||
}
|
||||
|
||||
SimdScalar RaycastVehicle::Raycast(WheelInfo& wheel)
|
||||
{
|
||||
UpdateWheelTransformsWS( wheel );
|
||||
|
||||
|
||||
SimdScalar depth = -1;
|
||||
|
||||
SimdScalar raylen = wheel.GetSuspensionRestLength()+wheel.m_wheelsRadius;
|
||||
|
||||
SimdVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
|
||||
const SimdVector3& source = wheel.m_raycastInfo.m_hardPointWS;
|
||||
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
|
||||
const SimdVector3& target = wheel.m_raycastInfo.m_contactPointWS;
|
||||
|
||||
SimdScalar param = 0.f;
|
||||
|
||||
VehicleRaycaster::VehicleRaycasterResult rayResults;
|
||||
|
||||
void* object = m_vehicleRaycaster->CastRay(source,target,rayResults);
|
||||
|
||||
wheel.m_raycastInfo.m_groundObject = 0;
|
||||
|
||||
if (object)
|
||||
{
|
||||
param = rayResults.m_distFraction;
|
||||
depth = raylen * rayResults.m_distFraction;
|
||||
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
|
||||
wheel.m_raycastInfo.m_isInContact = true;
|
||||
|
||||
wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!;
|
||||
//wheel.m_raycastInfo.m_groundObject = object;
|
||||
|
||||
|
||||
SimdScalar hitDistance = param*raylen;
|
||||
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
|
||||
//clamp on max suspension travel
|
||||
|
||||
float minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*0.01f;
|
||||
float maxSuspensionLength = wheel.GetSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*0.01f;
|
||||
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
|
||||
{
|
||||
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
|
||||
}
|
||||
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
|
||||
{
|
||||
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
|
||||
}
|
||||
|
||||
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
|
||||
|
||||
SimdScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
|
||||
|
||||
SimdVector3 chassis_velocity_at_contactPoint;
|
||||
SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-GetRigidBody()->getCenterOfMassPosition();
|
||||
|
||||
chassis_velocity_at_contactPoint = GetRigidBody()->getVelocityInLocalPoint(relpos);
|
||||
|
||||
SimdScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
|
||||
|
||||
if ( denominator >= -0.1f)
|
||||
{
|
||||
wheel.m_suspensionRelativeVelocity = 0.0f;
|
||||
wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
|
||||
}
|
||||
else
|
||||
{
|
||||
SimdScalar inv = -1.f / denominator;
|
||||
wheel.m_suspensionRelativeVelocity = projVel * inv;
|
||||
wheel.m_clippedInvContactDotSuspension = inv;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
//put wheel info as in rest position
|
||||
wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
|
||||
wheel.m_suspensionRelativeVelocity = 0.0f;
|
||||
wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
|
||||
wheel.m_clippedInvContactDotSuspension = 1.0f;
|
||||
}
|
||||
|
||||
return depth;
|
||||
}
|
||||
|
||||
|
||||
void RaycastVehicle::UpdateVehicle( SimdScalar step )
|
||||
{
|
||||
|
||||
m_currentVehicleSpeedKmHour = 3.6f * GetRigidBody()->getLinearVelocity().length();
|
||||
|
||||
const SimdTransform& chassisTrans = GetRigidBody()->getCenterOfMassTransform();
|
||||
SimdVector3 forwardW (
|
||||
chassisTrans.getBasis()[0][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[1][m_indexForwardAxis],
|
||||
chassisTrans.getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
if (forwardW.dot(GetRigidBody()->getLinearVelocity()) < 0.f)
|
||||
{
|
||||
m_currentVehicleSpeedKmHour *= -1.f;
|
||||
}
|
||||
|
||||
//
|
||||
// simulate suspension
|
||||
//
|
||||
std::vector<WheelInfo>::iterator wheelIt;
|
||||
int i=0;
|
||||
for (wheelIt = m_wheelInfo.begin();
|
||||
!(wheelIt == m_wheelInfo.end());wheelIt++,i++)
|
||||
{
|
||||
SimdScalar depth;
|
||||
depth = Raycast( *wheelIt );
|
||||
}
|
||||
|
||||
UpdateSuspension(step);
|
||||
|
||||
|
||||
for (wheelIt = m_wheelInfo.begin();
|
||||
!(wheelIt == m_wheelInfo.end());wheelIt++)
|
||||
{
|
||||
//apply suspension force
|
||||
WheelInfo& wheel = *wheelIt;
|
||||
|
||||
float suspensionForce = wheel.m_wheelsSuspensionForce;
|
||||
|
||||
float gMaxSuspensionForce = 6000.f;
|
||||
if (suspensionForce > gMaxSuspensionForce)
|
||||
{
|
||||
suspensionForce = gMaxSuspensionForce;
|
||||
}
|
||||
SimdVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
|
||||
SimdVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody()->getCenterOfMassPosition();
|
||||
|
||||
GetRigidBody()->applyImpulse(impulse, relpos);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
UpdateFriction( step);
|
||||
|
||||
|
||||
for (wheelIt = m_wheelInfo.begin();
|
||||
!(wheelIt == m_wheelInfo.end());wheelIt++)
|
||||
{
|
||||
WheelInfo& wheel = *wheelIt;
|
||||
SimdVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - GetRigidBody()->getCenterOfMassPosition();
|
||||
SimdVector3 vel = GetRigidBody()->getVelocityInLocalPoint( relpos );
|
||||
|
||||
if (wheel.m_raycastInfo.m_isInContact)
|
||||
{
|
||||
SimdVector3 fwd (
|
||||
GetRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis],
|
||||
GetRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis],
|
||||
GetRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]);
|
||||
|
||||
SimdScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
|
||||
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
|
||||
|
||||
SimdScalar proj2 = fwd.dot(vel);
|
||||
|
||||
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
|
||||
wheel.m_rotation += wheel.m_deltaRotation;
|
||||
|
||||
} else
|
||||
{
|
||||
wheel.m_rotation += wheel.m_deltaRotation;
|
||||
}
|
||||
|
||||
wheel.m_deltaRotation *= 0.99f;//damping of rotation when not in contact
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void RaycastVehicle::SetSteeringValue(SimdScalar steering,int wheel)
|
||||
{
|
||||
assert(wheel>=0 && wheel < GetNumWheels());
|
||||
|
||||
WheelInfo& wheelInfo = GetWheelInfo(wheel);
|
||||
wheelInfo.m_steering = steering;
|
||||
}
|
||||
|
||||
|
||||
|
||||
SimdScalar RaycastVehicle::GetSteeringValue(int wheel) const
|
||||
{
|
||||
return GetWheelInfo(wheel).m_steering;
|
||||
}
|
||||
|
||||
|
||||
void RaycastVehicle::ApplyEngineForce(SimdScalar force, int wheel)
|
||||
{
|
||||
assert(wheel>=0 && wheel < GetNumWheels());
|
||||
WheelInfo& wheelInfo = GetWheelInfo(wheel);
|
||||
wheelInfo.m_engineForce = force;
|
||||
}
|
||||
|
||||
|
||||
const WheelInfo& RaycastVehicle::GetWheelInfo(int index) const
|
||||
{
|
||||
ASSERT((index >= 0) && (index < GetNumWheels()));
|
||||
|
||||
return m_wheelInfo[index];
|
||||
}
|
||||
|
||||
WheelInfo& RaycastVehicle::GetWheelInfo(int index)
|
||||
{
|
||||
ASSERT((index >= 0) && (index < GetNumWheels()));
|
||||
|
||||
return m_wheelInfo[index];
|
||||
}
|
||||
|
||||
void RaycastVehicle::SetBrake(float brake,int wheelIndex)
|
||||
{
|
||||
ASSERT((wheelIndex >= 0) && (wheelIndex < GetNumWheels()));
|
||||
GetWheelInfo(wheelIndex).m_brake;
|
||||
}
|
||||
|
||||
|
||||
void RaycastVehicle::UpdateSuspension(SimdScalar deltaTime)
|
||||
{
|
||||
|
||||
SimdScalar chassisMass = 1.f / m_chassisBody->getInvMass();
|
||||
|
||||
for (int w_it=0; w_it<GetNumWheels(); w_it++)
|
||||
{
|
||||
WheelInfo &wheel_info = m_wheelInfo[w_it];
|
||||
|
||||
if ( wheel_info.m_raycastInfo.m_isInContact )
|
||||
{
|
||||
SimdScalar force;
|
||||
// Spring
|
||||
{
|
||||
SimdScalar susp_length = wheel_info.GetSuspensionRestLength();
|
||||
SimdScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
|
||||
|
||||
SimdScalar length_diff = (susp_length - current_length);
|
||||
|
||||
force = wheel_info.m_suspensionStiffness
|
||||
* length_diff * wheel_info.m_clippedInvContactDotSuspension;
|
||||
}
|
||||
|
||||
// Damper
|
||||
{
|
||||
SimdScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
|
||||
{
|
||||
SimdScalar susp_damping;
|
||||
if ( projected_rel_vel < 0.0f )
|
||||
{
|
||||
susp_damping = wheel_info.m_wheelsDampingCompression;
|
||||
}
|
||||
else
|
||||
{
|
||||
susp_damping = wheel_info.m_wheelsDampingRelaxation;
|
||||
}
|
||||
force -= susp_damping * projected_rel_vel;
|
||||
}
|
||||
}
|
||||
|
||||
// RESULT
|
||||
wheel_info.m_wheelsSuspensionForce = force * chassisMass;
|
||||
if (wheel_info.m_wheelsSuspensionForce < 0.f)
|
||||
{
|
||||
wheel_info.m_wheelsSuspensionForce = 0.f;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
wheel_info.m_wheelsSuspensionForce = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float sideFrictionStiffness2 = 1.0f;
|
||||
void RaycastVehicle::UpdateFriction(SimdScalar timeStep)
|
||||
{
|
||||
|
||||
//calculate the impulse, so that the wheels don't move sidewards
|
||||
int numWheel = GetNumWheels();
|
||||
if (!numWheel)
|
||||
return;
|
||||
|
||||
|
||||
SimdVector3* forwardWS = new SimdVector3[numWheel];
|
||||
SimdVector3* axle = new SimdVector3[numWheel];
|
||||
SimdScalar* forwardImpulse = new SimdScalar[numWheel];
|
||||
SimdScalar* sideImpulse = new SimdScalar[numWheel];
|
||||
|
||||
int numWheelsOnGround = 0;
|
||||
|
||||
|
||||
//collapse all those loops into one!
|
||||
for (int i=0;i<GetNumWheels();i++)
|
||||
{
|
||||
WheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
if (groundObject)
|
||||
numWheelsOnGround++;
|
||||
sideImpulse[i] = 0.f;
|
||||
forwardImpulse[i] = 0.f;
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
for (int i=0;i<GetNumWheels();i++)
|
||||
{
|
||||
|
||||
WheelInfo& wheelInfo = m_wheelInfo[i];
|
||||
|
||||
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
|
||||
const SimdTransform& wheelTrans = GetWheelTransformWS( i );
|
||||
|
||||
SimdMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
|
||||
axle[i] = SimdVector3(
|
||||
wheelBasis0[0][m_indexRightAxis],
|
||||
wheelBasis0[1][m_indexRightAxis],
|
||||
wheelBasis0[2][m_indexRightAxis]);
|
||||
|
||||
const SimdVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
|
||||
SimdScalar proj = axle[i].dot(surfNormalWS);
|
||||
axle[i] -= surfNormalWS * proj;
|
||||
axle[i] = axle[i].normalize();
|
||||
|
||||
forwardWS[i] = surfNormalWS.cross(axle[i]);
|
||||
forwardWS[i].normalize();
|
||||
|
||||
|
||||
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
|
||||
0.f, axle[i],sideImpulse[i],timeStep);
|
||||
|
||||
sideImpulse[i] *= sideFrictionStiffness2;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
SimdScalar sideFactor = 1.f;
|
||||
SimdScalar fwdFactor = 0.5;
|
||||
|
||||
bool sliding = false;
|
||||
{
|
||||
for (int wheel =0;wheel <GetNumWheels();wheel++)
|
||||
{
|
||||
WheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
class RigidBody* groundObject = (class RigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
|
||||
|
||||
|
||||
forwardImpulse[wheel] = 0.f;
|
||||
m_wheelInfo[wheel].m_skidInfo= 1.f;
|
||||
|
||||
if (groundObject)
|
||||
{
|
||||
m_wheelInfo[wheel].m_skidInfo= 1.f;
|
||||
|
||||
SimdScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
|
||||
SimdScalar maximpSide = maximp;
|
||||
|
||||
SimdScalar maximpSquared = maximp * maximpSide;
|
||||
|
||||
forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep;
|
||||
|
||||
float x = (forwardImpulse[wheel] ) * fwdFactor;
|
||||
float y = (sideImpulse[wheel] ) * sideFactor;
|
||||
|
||||
float impulseSquared = (x*x + y*y);
|
||||
|
||||
if (impulseSquared > maximpSquared)
|
||||
{
|
||||
sliding = true;
|
||||
|
||||
SimdScalar factor = maximp / SimdSqrt(impulseSquared);
|
||||
|
||||
m_wheelInfo[wheel].m_skidInfo *= factor;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if (sliding)
|
||||
{
|
||||
for (int wheel = 0;wheel < GetNumWheels(); wheel++)
|
||||
{
|
||||
if (sideImpulse[wheel] != 0.f)
|
||||
{
|
||||
if (m_wheelInfo[wheel].m_skidInfo< 1.f)
|
||||
{
|
||||
forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
|
||||
sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// apply the impulses
|
||||
{
|
||||
for (int wheel = 0;wheel<GetNumWheels() ; wheel++)
|
||||
{
|
||||
WheelInfo& wheelInfo = m_wheelInfo[wheel];
|
||||
|
||||
SimdVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
m_chassisBody->getCenterOfMassPosition();
|
||||
|
||||
if (forwardImpulse[wheel] != 0.f)
|
||||
{
|
||||
m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
|
||||
}
|
||||
if (sideImpulse[wheel] != 0.f)
|
||||
{
|
||||
class RigidBody* groundObject = (class RigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
|
||||
|
||||
SimdVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
|
||||
groundObject->getCenterOfMassPosition();
|
||||
|
||||
|
||||
SimdVector3 sideImp = axle[wheel] * sideImpulse[wheel];
|
||||
|
||||
rel_pos[2] *= wheelInfo.m_rollInfluence;
|
||||
m_chassisBody->applyImpulse(sideImp,rel_pos);
|
||||
|
||||
//apply friction impulse on the ground
|
||||
groundObject->applyImpulse(-sideImp,rel_pos2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
delete []forwardWS;
|
||||
delete [] axle;
|
||||
delete[]forwardImpulse;
|
||||
delete[] sideImpulse;
|
||||
}
|
||||
167
src/BulletDynamics/Vehicle/btRaycastVehicle.h
Normal file
167
src/BulletDynamics/Vehicle/btRaycastVehicle.h
Normal file
@@ -0,0 +1,167 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#ifndef RAYCASTVEHICLE_H
|
||||
#define RAYCASTVEHICLE_H
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
|
||||
struct MassProps;
|
||||
#include "btWheelInfo.h"
|
||||
|
||||
struct VehicleRaycaster;
|
||||
class VehicleTuning;
|
||||
|
||||
///Raycast vehicle, very special constraint that turn a rigidbody into a vehicle.
|
||||
class RaycastVehicle : public TypedConstraint
|
||||
{
|
||||
public:
|
||||
class VehicleTuning
|
||||
{
|
||||
public:
|
||||
|
||||
VehicleTuning()
|
||||
:m_suspensionStiffness(5.88f),
|
||||
m_suspensionCompression(0.83f),
|
||||
m_suspensionDamping(0.88f),
|
||||
m_maxSuspensionTravelCm(500.f),
|
||||
m_frictionSlip(10.5f)
|
||||
{
|
||||
}
|
||||
float m_suspensionStiffness;
|
||||
float m_suspensionCompression;
|
||||
float m_suspensionDamping;
|
||||
float m_maxSuspensionTravelCm;
|
||||
float m_frictionSlip;
|
||||
|
||||
};
|
||||
private:
|
||||
|
||||
SimdScalar m_tau;
|
||||
SimdScalar m_damping;
|
||||
VehicleRaycaster* m_vehicleRaycaster;
|
||||
float m_pitchControl;
|
||||
float m_steeringValue;
|
||||
float m_currentVehicleSpeedKmHour;
|
||||
|
||||
RigidBody* m_chassisBody;
|
||||
|
||||
int m_indexRightAxis;
|
||||
int m_indexUpAxis;
|
||||
int m_indexForwardAxis;
|
||||
|
||||
void DefaultInit(const VehicleTuning& tuning);
|
||||
|
||||
public:
|
||||
|
||||
//constructor to create a car from an existing rigidbody
|
||||
RaycastVehicle(const VehicleTuning& tuning,RigidBody* chassis, VehicleRaycaster* raycaster );
|
||||
|
||||
virtual ~RaycastVehicle() ;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
SimdScalar Raycast(WheelInfo& wheel);
|
||||
|
||||
virtual void UpdateVehicle(SimdScalar step);
|
||||
|
||||
void ResetSuspension();
|
||||
|
||||
SimdScalar GetSteeringValue(int wheel) const;
|
||||
|
||||
void SetSteeringValue(SimdScalar steering,int wheel);
|
||||
|
||||
|
||||
void ApplyEngineForce(SimdScalar force, int wheel);
|
||||
|
||||
const SimdTransform& GetWheelTransformWS( int wheelIndex ) const;
|
||||
|
||||
void UpdateWheelTransform( int wheelIndex );
|
||||
|
||||
void SetRaycastWheelInfo( int wheelIndex , bool isInContact, const SimdVector3& hitPoint, const SimdVector3& hitNormal,SimdScalar depth);
|
||||
|
||||
WheelInfo& AddWheel( const SimdVector3& connectionPointCS0, const SimdVector3& wheelDirectionCS0,const SimdVector3& wheelAxleCS,SimdScalar suspensionRestLength,SimdScalar wheelRadius,const VehicleTuning& tuning, bool isFrontWheel);
|
||||
|
||||
inline int GetNumWheels() const {
|
||||
return m_wheelInfo.size();
|
||||
}
|
||||
|
||||
std::vector<WheelInfo> m_wheelInfo;
|
||||
|
||||
|
||||
const WheelInfo& GetWheelInfo(int index) const;
|
||||
|
||||
WheelInfo& GetWheelInfo(int index);
|
||||
|
||||
void UpdateWheelTransformsWS(WheelInfo& wheel );
|
||||
|
||||
|
||||
void SetBrake(float brake,int wheelIndex);
|
||||
|
||||
void SetPitchControl(float pitch)
|
||||
{
|
||||
m_pitchControl = pitch;
|
||||
}
|
||||
|
||||
void UpdateSuspension(SimdScalar deltaTime);
|
||||
|
||||
void UpdateFriction(SimdScalar timeStep);
|
||||
|
||||
|
||||
|
||||
inline RigidBody* GetRigidBody()
|
||||
{
|
||||
return m_chassisBody;
|
||||
}
|
||||
|
||||
const RigidBody* GetRigidBody() const
|
||||
{
|
||||
return m_chassisBody;
|
||||
}
|
||||
|
||||
inline int GetRightAxis() const
|
||||
{
|
||||
return m_indexRightAxis;
|
||||
}
|
||||
inline int GetUpAxis() const
|
||||
{
|
||||
return m_indexUpAxis;
|
||||
}
|
||||
|
||||
inline int GetForwardAxis() const
|
||||
{
|
||||
return m_indexForwardAxis;
|
||||
}
|
||||
|
||||
virtual void SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
|
||||
{
|
||||
m_indexRightAxis = rightIndex;
|
||||
m_indexUpAxis = upIndex;
|
||||
m_indexForwardAxis = forwardIndex;
|
||||
}
|
||||
|
||||
virtual void BuildJacobian()
|
||||
{
|
||||
//not yet
|
||||
}
|
||||
|
||||
virtual void SolveConstraint(SimdScalar timeStep)
|
||||
{
|
||||
//not yet
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //RAYCASTVEHICLE_H
|
||||
|
||||
35
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
Normal file
35
src/BulletDynamics/Vehicle/btVehicleRaycaster.h
Normal file
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#ifndef VEHICLE_RAYCASTER_H
|
||||
#define VEHICLE_RAYCASTER_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
|
||||
/// VehicleRaycaster is provides interface for between vehicle simulation and raycasting
|
||||
struct VehicleRaycaster
|
||||
{
|
||||
virtual ~VehicleRaycaster()
|
||||
{
|
||||
}
|
||||
struct VehicleRaycasterResult
|
||||
{
|
||||
VehicleRaycasterResult() :m_distFraction(-1.f){};
|
||||
SimdVector3 m_hitPointInWorld;
|
||||
SimdVector3 m_hitNormalInWorld;
|
||||
SimdScalar m_distFraction;
|
||||
};
|
||||
|
||||
virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result) = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif //VEHICLE_RAYCASTER_H
|
||||
|
||||
55
src/BulletDynamics/Vehicle/btWheelInfo.cpp
Normal file
55
src/BulletDynamics/Vehicle/btWheelInfo.cpp
Normal file
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#include "btWheelInfo.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
|
||||
|
||||
|
||||
SimdScalar WheelInfo::GetSuspensionRestLength() const
|
||||
{
|
||||
|
||||
return m_suspensionRestLength1;
|
||||
|
||||
}
|
||||
|
||||
void WheelInfo::UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo)
|
||||
{
|
||||
|
||||
|
||||
if (m_raycastInfo.m_isInContact)
|
||||
|
||||
{
|
||||
SimdScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
|
||||
SimdVector3 chassis_velocity_at_contactPoint;
|
||||
SimdVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
|
||||
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
|
||||
SimdScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
|
||||
if ( project >= -0.1f)
|
||||
{
|
||||
m_suspensionRelativeVelocity = 0.0f;
|
||||
m_clippedInvContactDotSuspension = 1.0f / 0.1f;
|
||||
}
|
||||
else
|
||||
{
|
||||
SimdScalar inv = -1.f / project;
|
||||
m_suspensionRelativeVelocity = projVel * inv;
|
||||
m_clippedInvContactDotSuspension = inv;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
else // Not in contact : position wheel in a nice (rest length) position
|
||||
{
|
||||
m_raycastInfo.m_suspensionLength = this->GetSuspensionRestLength();
|
||||
m_suspensionRelativeVelocity = 0.0f;
|
||||
m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
|
||||
m_clippedInvContactDotSuspension = 1.0f;
|
||||
}
|
||||
}
|
||||
116
src/BulletDynamics/Vehicle/btWheelInfo.h
Normal file
116
src/BulletDynamics/Vehicle/btWheelInfo.h
Normal file
@@ -0,0 +1,116 @@
|
||||
/*
|
||||
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
*
|
||||
* Permission to use, copy, modify, distribute and sell this software
|
||||
* and its documentation for any purpose is hereby granted without fee,
|
||||
* provided that the above copyright notice appear in all copies.
|
||||
* Erwin Coumans makes no representations about the suitability
|
||||
* of this software for any purpose.
|
||||
* It is provided "as is" without express or implied warranty.
|
||||
*/
|
||||
#ifndef WHEEL_INFO_H
|
||||
#define WHEEL_INFO_H
|
||||
|
||||
#include "LinearMath/SimdVector3.h"
|
||||
#include "LinearMath/SimdTransform.h"
|
||||
|
||||
class RigidBody;
|
||||
|
||||
struct WheelInfoConstructionInfo
|
||||
{
|
||||
SimdVector3 m_chassisConnectionCS;
|
||||
SimdVector3 m_wheelDirectionCS;
|
||||
SimdVector3 m_wheelAxleCS;
|
||||
SimdScalar m_suspensionRestLength;
|
||||
SimdScalar m_maxSuspensionTravelCm;
|
||||
SimdScalar m_wheelRadius;
|
||||
|
||||
float m_suspensionStiffness;
|
||||
float m_wheelsDampingCompression;
|
||||
float m_wheelsDampingRelaxation;
|
||||
float m_frictionSlip;
|
||||
bool m_bIsFrontWheel;
|
||||
|
||||
};
|
||||
|
||||
/// WheelInfo contains information per wheel about friction and suspension.
|
||||
struct WheelInfo
|
||||
{
|
||||
struct RaycastInfo
|
||||
{
|
||||
//set by raycaster
|
||||
SimdVector3 m_contactNormalWS;//contactnormal
|
||||
SimdVector3 m_contactPointWS;//raycast hitpoint
|
||||
SimdScalar m_suspensionLength;
|
||||
SimdVector3 m_hardPointWS;//raycast starting point
|
||||
SimdVector3 m_wheelDirectionWS; //direction in worldspace
|
||||
SimdVector3 m_wheelAxleWS; // axle in worldspace
|
||||
bool m_isInContact;
|
||||
void* m_groundObject; //could be general void* ptr
|
||||
};
|
||||
|
||||
RaycastInfo m_raycastInfo;
|
||||
|
||||
SimdTransform m_worldTransform;
|
||||
|
||||
SimdVector3 m_chassisConnectionPointCS; //const
|
||||
SimdVector3 m_wheelDirectionCS;//const
|
||||
SimdVector3 m_wheelAxleCS; // const or modified by steering
|
||||
SimdScalar m_suspensionRestLength1;//const
|
||||
SimdScalar m_maxSuspensionTravelCm;
|
||||
SimdScalar GetSuspensionRestLength() const;
|
||||
SimdScalar m_wheelsRadius;//const
|
||||
SimdScalar m_suspensionStiffness;//const
|
||||
SimdScalar m_wheelsDampingCompression;//const
|
||||
SimdScalar m_wheelsDampingRelaxation;//const
|
||||
SimdScalar m_frictionSlip;
|
||||
SimdScalar m_steering;
|
||||
SimdScalar m_rotation;
|
||||
SimdScalar m_deltaRotation;
|
||||
SimdScalar m_rollInfluence;
|
||||
|
||||
SimdScalar m_engineForce;
|
||||
|
||||
SimdScalar m_brake;
|
||||
|
||||
bool m_bIsFrontWheel;
|
||||
|
||||
void* m_clientInfo;//can be used to store pointer to sync transforms...
|
||||
|
||||
WheelInfo(WheelInfoConstructionInfo& ci)
|
||||
|
||||
{
|
||||
|
||||
m_suspensionRestLength1 = ci.m_suspensionRestLength;
|
||||
m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
|
||||
|
||||
m_wheelsRadius = ci.m_wheelRadius;
|
||||
m_suspensionStiffness = ci.m_suspensionStiffness;
|
||||
m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
|
||||
m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
|
||||
m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
|
||||
m_wheelDirectionCS = ci.m_wheelDirectionCS;
|
||||
m_wheelAxleCS = ci.m_wheelAxleCS;
|
||||
m_frictionSlip = ci.m_frictionSlip;
|
||||
m_steering = 0.f;
|
||||
m_engineForce = 0.f;
|
||||
m_rotation = 0.f;
|
||||
m_deltaRotation = 0.f;
|
||||
m_brake = 0.f;
|
||||
m_rollInfluence = 0.1f;
|
||||
m_bIsFrontWheel = ci.m_bIsFrontWheel;
|
||||
|
||||
}
|
||||
|
||||
void UpdateWheel(const RigidBody& chassis,RaycastInfo& raycastInfo);
|
||||
|
||||
SimdScalar m_clippedInvContactDotSuspension;
|
||||
SimdScalar m_suspensionRelativeVelocity;
|
||||
//calculated by suspension
|
||||
SimdScalar m_wheelsSuspensionForce;
|
||||
SimdScalar m_skidInfo;
|
||||
|
||||
};
|
||||
|
||||
#endif //WHEEL_INFO_H
|
||||
|
||||
Reference in New Issue
Block a user