mirror of
https://github.com/Ragora/T2-CPP.git
synced 2026-07-12 08:54:33 +00:00
Ported Bullet to the mod loader system; needs further work
This commit is contained in:
Robert MacGregor
parent
527474ff24
commit
06810b6cca
353 changed files with 80265 additions and 0 deletions
|
|
@ -0,0 +1,332 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
|
||||
|
||||
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.
|
||||
|
||||
Written by: Marcus Hennix
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Overview:
|
||||
|
||||
btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
|
||||
It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
|
||||
It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
|
||||
Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
|
||||
(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
|
||||
|
||||
In the contraint's frame of reference:
|
||||
twist is along the x-axis,
|
||||
and swing 1 and 2 are along the z and y axes respectively.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef CONETWISTCONSTRAINT_H
|
||||
#define CONETWISTCONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
enum btConeTwistFlags
|
||||
{
|
||||
BT_CONETWIST_FLAGS_LIN_CFM = 1,
|
||||
BT_CONETWIST_FLAGS_LIN_ERP = 2,
|
||||
BT_CONETWIST_FLAGS_ANG_CFM = 4
|
||||
};
|
||||
|
||||
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
|
||||
class btConeTwistConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btTransform m_rbAFrame;
|
||||
btTransform m_rbBFrame;
|
||||
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
|
||||
btScalar m_damping;
|
||||
|
||||
btScalar m_swingSpan1;
|
||||
btScalar m_swingSpan2;
|
||||
btScalar m_twistSpan;
|
||||
|
||||
btScalar m_fixThresh;
|
||||
|
||||
btVector3 m_swingAxis;
|
||||
btVector3 m_twistAxis;
|
||||
|
||||
btScalar m_kSwing;
|
||||
btScalar m_kTwist;
|
||||
|
||||
btScalar m_twistLimitSign;
|
||||
btScalar m_swingCorrection;
|
||||
btScalar m_twistCorrection;
|
||||
|
||||
btScalar m_twistAngle;
|
||||
|
||||
btScalar m_accSwingLimitImpulse;
|
||||
btScalar m_accTwistLimitImpulse;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_solveTwistLimit;
|
||||
bool m_solveSwingLimit;
|
||||
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
// not yet used...
|
||||
btScalar m_swingLimitRatio;
|
||||
btScalar m_twistLimitRatio;
|
||||
btVector3 m_twistAxisA;
|
||||
|
||||
// motor
|
||||
bool m_bMotorEnabled;
|
||||
bool m_bNormalizedMotorStrength;
|
||||
btQuaternion m_qTarget;
|
||||
btScalar m_maxMotorImpulse;
|
||||
btVector3 m_accMotorImpulse;
|
||||
|
||||
// parameters
|
||||
int m_flags;
|
||||
btScalar m_linCFM;
|
||||
btScalar m_linERP;
|
||||
btScalar m_angCFM;
|
||||
|
||||
protected:
|
||||
|
||||
void init();
|
||||
|
||||
void computeConeLimitInfo(const btQuaternion& qCone, // in
|
||||
btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
|
||||
|
||||
void computeTwistLimitInfo(const btQuaternion& qTwist, // in
|
||||
btScalar& twistAngle, btVector3& vTwistAxis); // all outs
|
||||
|
||||
void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
|
||||
|
||||
btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
void setLimit(int limitIndex,btScalar limitValue)
|
||||
{
|
||||
switch (limitIndex)
|
||||
{
|
||||
case 3:
|
||||
{
|
||||
m_twistSpan = limitValue;
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
m_swingSpan2 = limitValue;
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
{
|
||||
m_swingSpan1 = limitValue;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
// setLimit(), a few notes:
|
||||
// _softness:
|
||||
// 0->1, recommend ~0.8->1.
|
||||
// describes % of limits where movement is free.
|
||||
// beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
|
||||
// _biasFactor:
|
||||
// 0->1?, recommend 0.3 +/-0.3 or so.
|
||||
// strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
|
||||
// __relaxationFactor:
|
||||
// 0->1, recommend to stay near 1.
|
||||
// the lower the value, the less the constraint will fight velocities which violate the angular limits.
|
||||
void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_swingSpan1 = _swingSpan1;
|
||||
m_swingSpan2 = _swingSpan2;
|
||||
m_twistSpan = _twistSpan;
|
||||
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
}
|
||||
|
||||
const btTransform& getAFrame() { return m_rbAFrame; };
|
||||
const btTransform& getBFrame() { return m_rbBFrame; };
|
||||
|
||||
inline int getSolveTwistLimit()
|
||||
{
|
||||
return m_solveTwistLimit;
|
||||
}
|
||||
|
||||
inline int getSolveSwingLimit()
|
||||
{
|
||||
return m_solveTwistLimit;
|
||||
}
|
||||
|
||||
inline btScalar getTwistLimitSign()
|
||||
{
|
||||
return m_twistLimitSign;
|
||||
}
|
||||
|
||||
void calcAngleInfo();
|
||||
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
inline btScalar getSwingSpan1()
|
||||
{
|
||||
return m_swingSpan1;
|
||||
}
|
||||
inline btScalar getSwingSpan2()
|
||||
{
|
||||
return m_swingSpan2;
|
||||
}
|
||||
inline btScalar getTwistSpan()
|
||||
{
|
||||
return m_twistSpan;
|
||||
}
|
||||
inline btScalar getTwistAngle()
|
||||
{
|
||||
return m_twistAngle;
|
||||
}
|
||||
bool isPastSwingLimit() { return m_solveSwingLimit; }
|
||||
|
||||
|
||||
void setDamping(btScalar damping) { m_damping = damping; }
|
||||
|
||||
void enableMotor(bool b) { m_bMotorEnabled = b; }
|
||||
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
|
||||
void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
|
||||
|
||||
btScalar getFixThresh() { return m_fixThresh; }
|
||||
void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
|
||||
|
||||
// setMotorTarget:
|
||||
// q: the desired rotation of bodyA wrt bodyB.
|
||||
// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
|
||||
// note: don't forget to enableMotor()
|
||||
void setMotorTarget(const btQuaternion &q);
|
||||
|
||||
// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
|
||||
void setMotorTargetInConstraintSpace(const btQuaternion &q);
|
||||
|
||||
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btConeTwistConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame;
|
||||
btTransformFloatData m_rbBFrame;
|
||||
|
||||
//limits
|
||||
float m_swingSpan1;
|
||||
float m_swingSpan2;
|
||||
float m_twistSpan;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
|
||||
float m_damping;
|
||||
|
||||
char m_pad[4];
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btConeTwistConstraintData);
|
||||
|
||||
}
|
||||
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
|
||||
btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
|
||||
|
||||
m_rbAFrame.serializeFloat(cone->m_rbAFrame);
|
||||
m_rbBFrame.serializeFloat(cone->m_rbBFrame);
|
||||
|
||||
cone->m_swingSpan1 = float(m_swingSpan1);
|
||||
cone->m_swingSpan2 = float(m_swingSpan2);
|
||||
cone->m_twistSpan = float(m_twistSpan);
|
||||
cone->m_limitSoftness = float(m_limitSoftness);
|
||||
cone->m_biasFactor = float(m_biasFactor);
|
||||
cone->m_relaxationFactor = float(m_relaxationFactor);
|
||||
cone->m_damping = float(m_damping);
|
||||
|
||||
return "btConeTwistConstraintData";
|
||||
}
|
||||
|
||||
|
||||
#endif //CONETWISTCONSTRAINT_H
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/*
|
||||
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
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
class btPersistentManifold;
|
||||
class btRigidBody;
|
||||
class btCollisionObject;
|
||||
class btTypedConstraint;
|
||||
struct btContactSolverInfo;
|
||||
struct btBroadphaseProxy;
|
||||
class btIDebugDraw;
|
||||
class btStackAlloc;
|
||||
class btDispatcher;
|
||||
/// btConstraintSolver provides solver interface
|
||||
class btConstraintSolver
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
virtual ~btConstraintSolver() {}
|
||||
|
||||
virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
|
||||
|
||||
///solve a group of constraints
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
|
||||
|
||||
virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset() = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //CONSTRAINT_SOLVER_H
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/*
|
||||
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
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
|
||||
///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
|
||||
ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btPersistentManifold m_contactManifold;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
|
||||
|
||||
void setContactManifold(btPersistentManifold* contactManifold);
|
||||
|
||||
btPersistentManifold* getContactManifold()
|
||||
{
|
||||
return &m_contactManifold;
|
||||
}
|
||||
|
||||
const btPersistentManifold* getContactManifold() const
|
||||
{
|
||||
return &m_contactManifold;
|
||||
}
|
||||
|
||||
virtual ~btContactConstraint();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
///obsolete methods
|
||||
virtual void buildJacobian();
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
|
||||
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
|
||||
|
||||
|
||||
|
||||
#endif //CONTACT_CONSTRAINT_H
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/*
|
||||
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
|
||||
|
||||
enum btSolverMode
|
||||
{
|
||||
SOLVER_RANDMIZE_ORDER = 1,
|
||||
SOLVER_FRICTION_SEPARATE = 2,
|
||||
SOLVER_USE_WARMSTARTING = 4,
|
||||
SOLVER_USE_FRICTION_WARMSTARTING = 8,
|
||||
SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
|
||||
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
|
||||
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
|
||||
SOLVER_CACHE_FRIENDLY = 128,
|
||||
SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
|
||||
SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster.
|
||||
};
|
||||
|
||||
struct btContactSolverInfoData
|
||||
{
|
||||
|
||||
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
|
||||
btScalar m_friction;
|
||||
btScalar m_timeStep;
|
||||
btScalar m_restitution;
|
||||
int m_numIterations;
|
||||
btScalar m_maxErrorReduction;
|
||||
btScalar m_sor;
|
||||
btScalar m_erp;//used as Baumgarte factor
|
||||
btScalar m_erp2;//used in Split Impulse
|
||||
btScalar m_globalCfm;//constraint force mixing
|
||||
int m_splitImpulse;
|
||||
btScalar m_splitImpulsePenetrationThreshold;
|
||||
btScalar m_linearSlop;
|
||||
btScalar m_warmstartingFactor;
|
||||
|
||||
int m_solverMode;
|
||||
int m_restingContactRestitutionThreshold;
|
||||
int m_minimumSolverBatchSize;
|
||||
|
||||
|
||||
};
|
||||
|
||||
struct btContactSolverInfo : public btContactSolverInfoData
|
||||
{
|
||||
|
||||
|
||||
|
||||
inline btContactSolverInfo()
|
||||
{
|
||||
m_tau = btScalar(0.6);
|
||||
m_damping = btScalar(1.0);
|
||||
m_friction = btScalar(0.3);
|
||||
m_restitution = btScalar(0.);
|
||||
m_maxErrorReduction = btScalar(20.);
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.2);
|
||||
m_erp2 = btScalar(0.1);
|
||||
m_globalCfm = btScalar(0.);
|
||||
m_sor = btScalar(1.);
|
||||
m_splitImpulse = false;
|
||||
m_splitImpulsePenetrationThreshold = -0.02f;
|
||||
m_linearSlop = btScalar(0.0);
|
||||
m_warmstartingFactor=btScalar(0.85);
|
||||
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
|
||||
m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
|
||||
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
|
||||
}
|
||||
};
|
||||
|
||||
#endif //CONTACT_SOLVER_INFO
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org
|
||||
|
||||
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 BT_GEAR_CONSTRAINT_H
|
||||
#define BT_GEAR_CONSTRAINT_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
|
||||
///See Bullet/Demos/ConstraintDemo for an example use.
|
||||
class btGearConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
btVector3 m_axisInA;
|
||||
btVector3 m_axisInB;
|
||||
bool m_useFrameA;
|
||||
btScalar m_ratio;
|
||||
|
||||
public:
|
||||
btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
|
||||
virtual ~btGearConstraint ();
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
virtual void setParam(int num, btScalar value, int axis = -1)
|
||||
{
|
||||
btAssert(0);
|
||||
};
|
||||
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const
|
||||
{
|
||||
btAssert(0);
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_GEAR_CONSTRAINT_H
|
||||
|
|
@ -0,0 +1,588 @@
|
|||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
|
||||
/// Added support for generic constraint solver through getInfo1/getInfo2 methods
|
||||
|
||||
/*
|
||||
2007-09-09
|
||||
btGeneric6DofConstraint Refactored by Francisco Le?n
|
||||
email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GENERIC_6DOF_CONSTRAINT_H
|
||||
#define GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
|
||||
//! Rotation Limit structure for generic joints
|
||||
class btRotationalLimitMotor
|
||||
{
|
||||
public:
|
||||
//! limit_parameters
|
||||
//!@{
|
||||
btScalar m_loLimit;//!< joint limit
|
||||
btScalar m_hiLimit;//!< joint limit
|
||||
btScalar m_targetVelocity;//!< target motor velocity
|
||||
btScalar m_maxMotorForce;//!< max force on motor
|
||||
btScalar m_maxLimitForce;//!< max force on limit
|
||||
btScalar m_damping;//!< Damping.
|
||||
btScalar m_limitSoftness;//! Relaxation factor
|
||||
btScalar m_normalCFM;//!< Constraint force mixing factor
|
||||
btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
|
||||
btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
|
||||
btScalar m_bounce;//!< restitution factor
|
||||
bool m_enableMotor;
|
||||
|
||||
//!@}
|
||||
|
||||
//! temp_variables
|
||||
//!@{
|
||||
btScalar m_currentLimitError;//! How much is violated this limit
|
||||
btScalar m_currentPosition; //! current value of angle
|
||||
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
|
||||
btScalar m_accumulatedImpulse;
|
||||
//!@}
|
||||
|
||||
btRotationalLimitMotor()
|
||||
{
|
||||
m_accumulatedImpulse = 0.f;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 0.1f;
|
||||
m_maxLimitForce = 300.0f;
|
||||
m_loLimit = 1.0f;
|
||||
m_hiLimit = -1.0f;
|
||||
m_normalCFM = 0.f;
|
||||
m_stopERP = 0.2f;
|
||||
m_stopCFM = 0.f;
|
||||
m_bounce = 0.0f;
|
||||
m_damping = 1.0f;
|
||||
m_limitSoftness = 0.5f;
|
||||
m_currentLimit = 0;
|
||||
m_currentLimitError = 0;
|
||||
m_enableMotor = false;
|
||||
}
|
||||
|
||||
btRotationalLimitMotor(const btRotationalLimitMotor & limot)
|
||||
{
|
||||
m_targetVelocity = limot.m_targetVelocity;
|
||||
m_maxMotorForce = limot.m_maxMotorForce;
|
||||
m_limitSoftness = limot.m_limitSoftness;
|
||||
m_loLimit = limot.m_loLimit;
|
||||
m_hiLimit = limot.m_hiLimit;
|
||||
m_normalCFM = limot.m_normalCFM;
|
||||
m_stopERP = limot.m_stopERP;
|
||||
m_stopCFM = limot.m_stopCFM;
|
||||
m_bounce = limot.m_bounce;
|
||||
m_currentLimit = limot.m_currentLimit;
|
||||
m_currentLimitError = limot.m_currentLimitError;
|
||||
m_enableMotor = limot.m_enableMotor;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//! Is limited
|
||||
bool isLimited()
|
||||
{
|
||||
if(m_loLimit > m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! Need apply correction
|
||||
bool needApplyTorques()
|
||||
{
|
||||
if(m_currentLimit == 0 && m_enableMotor == false) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! calculates error
|
||||
/*!
|
||||
calculates m_currentLimit and m_currentLimitError.
|
||||
*/
|
||||
int testLimitValue(btScalar test_value);
|
||||
|
||||
//! apply the correction impulses for two bodies
|
||||
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class btTranslationalLimitMotor
|
||||
{
|
||||
public:
|
||||
btVector3 m_lowerLimit;//!< the constraint lower limits
|
||||
btVector3 m_upperLimit;//!< the constraint upper limits
|
||||
btVector3 m_accumulatedImpulse;
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
btScalar m_limitSoftness;//!< Softness for linear limit
|
||||
btScalar m_damping;//!< Damping for linear limit
|
||||
btScalar m_restitution;//! Bounce parameter for linear limit
|
||||
btVector3 m_normalCFM;//!< Constraint force mixing factor
|
||||
btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit
|
||||
btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
|
||||
//!@}
|
||||
bool m_enableMotor[3];
|
||||
btVector3 m_targetVelocity;//!< target motor velocity
|
||||
btVector3 m_maxMotorForce;//!< max force on motor
|
||||
btVector3 m_currentLimitError;//! How much is violated this limit
|
||||
btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames
|
||||
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
|
||||
|
||||
btTranslationalLimitMotor()
|
||||
{
|
||||
m_lowerLimit.setValue(0.f,0.f,0.f);
|
||||
m_upperLimit.setValue(0.f,0.f,0.f);
|
||||
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
|
||||
m_normalCFM.setValue(0.f, 0.f, 0.f);
|
||||
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
|
||||
m_stopCFM.setValue(0.f, 0.f, 0.f);
|
||||
|
||||
m_limitSoftness = 0.7f;
|
||||
m_damping = btScalar(1.0f);
|
||||
m_restitution = btScalar(0.5f);
|
||||
for(int i=0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = false;
|
||||
m_targetVelocity[i] = btScalar(0.f);
|
||||
m_maxMotorForce[i] = btScalar(0.f);
|
||||
}
|
||||
}
|
||||
|
||||
btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
|
||||
{
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_accumulatedImpulse = other.m_accumulatedImpulse;
|
||||
|
||||
m_limitSoftness = other.m_limitSoftness ;
|
||||
m_damping = other.m_damping;
|
||||
m_restitution = other.m_restitution;
|
||||
m_normalCFM = other.m_normalCFM;
|
||||
m_stopERP = other.m_stopERP;
|
||||
m_stopCFM = other.m_stopCFM;
|
||||
|
||||
for(int i=0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = other.m_enableMotor[i];
|
||||
m_targetVelocity[i] = other.m_targetVelocity[i];
|
||||
m_maxMotorForce[i] = other.m_maxMotorForce[i];
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
inline bool isLimited(int limitIndex)
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
inline bool needApplyForce(int limitIndex)
|
||||
{
|
||||
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
|
||||
return true;
|
||||
}
|
||||
int testLimitValue(int limitIndex, btScalar test_value);
|
||||
|
||||
|
||||
btScalar solveLinearAxis(
|
||||
btScalar timeStep,
|
||||
btScalar jacDiagABInv,
|
||||
btRigidBody& body1,const btVector3 &pointInA,
|
||||
btRigidBody& body2,const btVector3 &pointInB,
|
||||
int limit_index,
|
||||
const btVector3 & axis_normal_on_a,
|
||||
const btVector3 & anchorPos);
|
||||
|
||||
|
||||
};
|
||||
|
||||
enum bt6DofFlags
|
||||
{
|
||||
BT_6DOF_FLAGS_CFM_NORM = 1,
|
||||
BT_6DOF_FLAGS_CFM_STOP = 2,
|
||||
BT_6DOF_FLAGS_ERP_STOP = 4
|
||||
};
|
||||
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
|
||||
|
||||
|
||||
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/*!
|
||||
btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
|
||||
currently this limit supports rotational motors<br>
|
||||
<ul>
|
||||
<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
|
||||
At this moment translational motors are not supported. May be in the future. </li>
|
||||
|
||||
<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
|
||||
This is accessible through btGeneric6DofConstraint.getLimitMotor method,
|
||||
This brings support for limit parameters and motors. </li>
|
||||
|
||||
<li> Angulars limits have these possible ranges:
|
||||
<table border=1 >
|
||||
<tr>
|
||||
<td><b>AXIS</b></td>
|
||||
<td><b>MIN ANGLE</b></td>
|
||||
<td><b>MAX ANGLE</b></td>
|
||||
</tr><tr>
|
||||
<td>X</td>
|
||||
<td>-PI</td>
|
||||
<td>PI</td>
|
||||
</tr><tr>
|
||||
<td>Y</td>
|
||||
<td>-PI/2</td>
|
||||
<td>PI/2</td>
|
||||
</tr><tr>
|
||||
<td>Z</td>
|
||||
<td>-PI</td>
|
||||
<td>PI</td>
|
||||
</tr>
|
||||
</table>
|
||||
</li>
|
||||
</ul>
|
||||
|
||||
*/
|
||||
class btGeneric6DofConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
//! relative_frames
|
||||
//!@{
|
||||
btTransform m_frameInA;//!< the constraint space w.r.t body A
|
||||
btTransform m_frameInB;//!< the constraint space w.r.t body B
|
||||
//!@}
|
||||
|
||||
//! Jacobians
|
||||
//!@{
|
||||
btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
|
||||
//!@}
|
||||
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
btTranslationalLimitMotor m_linearLimits;
|
||||
//!@}
|
||||
|
||||
|
||||
//! hinge_parameters
|
||||
//!@{
|
||||
btRotationalLimitMotor m_angularLimits[3];
|
||||
//!@}
|
||||
|
||||
|
||||
protected:
|
||||
//! temporal variables
|
||||
//!@{
|
||||
btScalar m_timeStep;
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
btVector3 m_calculatedAxisAngleDiff;
|
||||
btVector3 m_calculatedAxis[3];
|
||||
btVector3 m_calculatedLinearDiff;
|
||||
btScalar m_factA;
|
||||
btScalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
|
||||
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
||||
|
||||
bool m_useLinearReferenceFrameA;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
|
||||
int m_flags;
|
||||
|
||||
//!@}
|
||||
|
||||
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
void buildLinearJacobian(
|
||||
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
|
||||
const btVector3 & pivotAInW,const btVector3 & pivotBInW);
|
||||
|
||||
void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
|
||||
|
||||
// tests linear limits
|
||||
void calculateLinearInfo();
|
||||
|
||||
//! calcs the euler angles between the two bodies.
|
||||
void calculateAngleInfo();
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
||||
|
||||
//! Calcs global transform of the offsets
|
||||
/*!
|
||||
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
|
||||
\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
|
||||
*/
|
||||
void calculateTransforms(const btTransform& transA,const btTransform& transB);
|
||||
|
||||
void calculateTransforms();
|
||||
|
||||
//! Gets the global transform of the offset for body A
|
||||
/*!
|
||||
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const btTransform & getCalculatedTransformA() const
|
||||
{
|
||||
return m_calculatedTransformA;
|
||||
}
|
||||
|
||||
//! Gets the global transform of the offset for body B
|
||||
/*!
|
||||
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const btTransform & getCalculatedTransformB() const
|
||||
{
|
||||
return m_calculatedTransformB;
|
||||
}
|
||||
|
||||
const btTransform & getFrameOffsetA() const
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
const btTransform & getFrameOffsetB() const
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
|
||||
btTransform & getFrameOffsetA()
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
btTransform & getFrameOffsetB()
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
|
||||
//! performs Jacobian calculation, and also calculates angle differences and axis
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
//! Get the rotation axis in global coordinates
|
||||
/*!
|
||||
\pre btGeneric6DofConstraint.buildJacobian must be called previously.
|
||||
*/
|
||||
btVector3 getAxis(int axis_index) const;
|
||||
|
||||
//! Get the relative Euler angle
|
||||
/*!
|
||||
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
btScalar getAngle(int axis_index) const;
|
||||
|
||||
//! Get the relative position of the constraint pivot
|
||||
/*!
|
||||
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
btScalar getRelativePivotPosition(int axis_index) const;
|
||||
|
||||
|
||||
//! Test angular limit.
|
||||
/*!
|
||||
Calculates angular correction and returns true if limit needs to be corrected.
|
||||
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
bool testAngularLimitMotor(int axis_index);
|
||||
|
||||
void setLinearLowerLimit(const btVector3& linearLower)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit = linearLower;
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const btVector3& linearUpper)
|
||||
{
|
||||
m_linearLimits.m_upperLimit = linearUpper;
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const btVector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const btVector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
|
||||
}
|
||||
|
||||
//! Retrieves the angular limit informacion
|
||||
btRotationalLimitMotor * getRotationalLimitMotor(int index)
|
||||
{
|
||||
return &m_angularLimits[index];
|
||||
}
|
||||
|
||||
//! Retrieves the limit informacion
|
||||
btTranslationalLimitMotor * getTranslationalLimitMotor()
|
||||
{
|
||||
return &m_linearLimits;
|
||||
}
|
||||
|
||||
//first 3 are linear, next 3 are angular
|
||||
void setLimit(int axis, btScalar lo, btScalar hi)
|
||||
{
|
||||
if(axis<3)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit[axis] = lo;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
}
|
||||
else
|
||||
{
|
||||
lo = btNormalizeAngle(lo);
|
||||
hi = btNormalizeAngle(hi);
|
||||
m_angularLimits[axis-3].m_loLimit = lo;
|
||||
m_angularLimits[axis-3].m_hiLimit = hi;
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- 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)
|
||||
{
|
||||
if(limitIndex<3)
|
||||
{
|
||||
return m_linearLimits.isLimited(limitIndex);
|
||||
|
||||
}
|
||||
return m_angularLimits[limitIndex-3].isLimited();
|
||||
}
|
||||
|
||||
virtual void calcAnchorPos(void); // overridable
|
||||
|
||||
int get_limit_motor_info2( btRotationalLimitMotor * limot,
|
||||
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
|
||||
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
|
||||
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btGeneric6DofConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformFloatData m_rbBFrame;
|
||||
|
||||
btVector3FloatData m_linearUpperLimit;
|
||||
btVector3FloatData m_linearLowerLimit;
|
||||
|
||||
btVector3FloatData m_angularUpperLimit;
|
||||
btVector3FloatData m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btGeneric6DofConstraintData);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
|
||||
btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
|
||||
btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
|
||||
|
||||
m_frameInA.serializeFloat(dof->m_rbAFrame);
|
||||
m_frameInB.serializeFloat(dof->m_rbBFrame);
|
||||
|
||||
|
||||
int i;
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit);
|
||||
dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit);
|
||||
dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
|
||||
dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
|
||||
}
|
||||
|
||||
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
|
||||
dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
|
||||
|
||||
return "btGeneric6DofConstraintData";
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //GENERIC_6DOF_CONSTRAINT_H
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
|
||||
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
#define GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofConstraint.h"
|
||||
|
||||
|
||||
/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
|
||||
|
||||
/// DOF index used in enableSpring() and setStiffness() means:
|
||||
/// 0 : translation X
|
||||
/// 1 : translation Y
|
||||
/// 2 : translation Z
|
||||
/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] )
|
||||
/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
|
||||
/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
|
||||
|
||||
class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
|
||||
{
|
||||
protected:
|
||||
bool m_springEnabled[6];
|
||||
btScalar m_equilibriumPoint[6];
|
||||
btScalar m_springStiffness[6];
|
||||
btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping)
|
||||
void internalUpdateSprings(btConstraintInfo2* info);
|
||||
public:
|
||||
btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
void enableSpring(int index, bool onOff);
|
||||
void setStiffness(int index, btScalar stiffness);
|
||||
void setDamping(int index, btScalar damping);
|
||||
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
|
||||
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
|
||||
void setEquilibriumPoint(int index, btScalar val);
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
};
|
||||
|
||||
#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H
|
||||
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
|
||||
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef HINGE2_CONSTRAINT_H
|
||||
#define HINGE2_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofSpringConstraint.h"
|
||||
|
||||
|
||||
|
||||
// Constraint similar to ODE Hinge2 Joint
|
||||
// has 3 degrees of frredom:
|
||||
// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
|
||||
// 1 translational (along axis Z) with suspension spring
|
||||
|
||||
class btHinge2Constraint : public btGeneric6DofSpringConstraint
|
||||
{
|
||||
protected:
|
||||
btVector3 m_anchor;
|
||||
btVector3 m_axis1;
|
||||
btVector3 m_axis2;
|
||||
public:
|
||||
// constructor
|
||||
// anchor, axis1 and axis2 are in world coordinate system
|
||||
// axis1 must be orthogonal to axis2
|
||||
btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
|
||||
// access
|
||||
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
|
||||
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
|
||||
const btVector3& getAxis1() { return m_axis1; }
|
||||
const btVector3& getAxis2() { return m_axis2; }
|
||||
btScalar getAngle1() { return getAngle(2); }
|
||||
btScalar getAngle2() { return getAngle(0); }
|
||||
// limits
|
||||
void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
|
||||
void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // HINGE2_CONSTRAINT_H
|
||||
|
||||
|
|
@ -0,0 +1,332 @@
|
|||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
|
||||
|
||||
#ifndef HINGECONSTRAINT_H
|
||||
#define HINGECONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btHingeConstraintData btHingeConstraintDoubleData
|
||||
#define btHingeConstraintDataName "btHingeConstraintDoubleData"
|
||||
#else
|
||||
#define btHingeConstraintData btHingeConstraintFloatData
|
||||
#define btHingeConstraintDataName "btHingeConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
|
||||
enum btHingeFlags
|
||||
{
|
||||
BT_HINGE_FLAGS_CFM_STOP = 1,
|
||||
BT_HINGE_FLAGS_ERP_STOP = 2,
|
||||
BT_HINGE_FLAGS_CFM_NORM = 4
|
||||
};
|
||||
|
||||
|
||||
/// 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
|
||||
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
|
||||
|
||||
btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransform m_rbBFrame;
|
||||
|
||||
btScalar m_motorTargetVelocity;
|
||||
btScalar m_maxMotorImpulse;
|
||||
|
||||
btScalar m_limitSoftness;
|
||||
btScalar m_biasFactor;
|
||||
btScalar m_relaxationFactor;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
|
||||
btScalar m_kHinge;
|
||||
|
||||
btScalar m_limitSign;
|
||||
btScalar m_correction;
|
||||
|
||||
btScalar m_accLimitImpulse;
|
||||
btScalar m_hingeAngle;
|
||||
btScalar m_referenceSign;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_enableAngularMotor;
|
||||
bool m_solveLimit;
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
bool m_useReferenceFrameA;
|
||||
|
||||
btScalar m_accMotorImpulse;
|
||||
|
||||
int m_flags;
|
||||
btScalar m_normalCFM;
|
||||
btScalar m_stopCFM;
|
||||
btScalar m_stopERP;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
|
||||
|
||||
btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
|
||||
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
|
||||
{
|
||||
m_enableAngularMotor = enableMotor;
|
||||
m_motorTargetVelocity = targetVelocity;
|
||||
m_maxMotorImpulse = maxMotorImpulse;
|
||||
}
|
||||
|
||||
// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
|
||||
// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
|
||||
// maintain a given angular target.
|
||||
void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
|
||||
void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
|
||||
void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
|
||||
void setMotorTarget(btScalar targetAngle, btScalar dt);
|
||||
|
||||
|
||||
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_lowerLimit = btNormalizeAngle(low);
|
||||
m_upperLimit = btNormalizeAngle(high);
|
||||
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
|
||||
}
|
||||
|
||||
void setAxis(btVector3& axisInA)
|
||||
{
|
||||
btVector3 rbAxisA1, rbAxisA2;
|
||||
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
|
||||
btVector3 pivotInA = m_rbAFrame.getOrigin();
|
||||
// m_rbAFrame.getOrigin() = pivotInA;
|
||||
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
|
||||
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
|
||||
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
|
||||
|
||||
btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
|
||||
|
||||
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
|
||||
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
|
||||
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
|
||||
|
||||
|
||||
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
|
||||
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
|
||||
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
|
||||
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
|
||||
}
|
||||
|
||||
btScalar getLowerLimit() const
|
||||
{
|
||||
return m_lowerLimit;
|
||||
}
|
||||
|
||||
btScalar getUpperLimit() const
|
||||
{
|
||||
return m_upperLimit;
|
||||
}
|
||||
|
||||
|
||||
btScalar getHingeAngle();
|
||||
|
||||
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
|
||||
|
||||
void testLimit(const btTransform& transA,const btTransform& transB);
|
||||
|
||||
|
||||
const btTransform& getAFrame() const { return m_rbAFrame; };
|
||||
const btTransform& getBFrame() const { return m_rbBFrame; };
|
||||
|
||||
btTransform& getAFrame() { return m_rbAFrame; };
|
||||
btTransform& getBFrame() { return m_rbBFrame; };
|
||||
|
||||
inline int getSolveLimit()
|
||||
{
|
||||
return m_solveLimit;
|
||||
}
|
||||
|
||||
inline btScalar getLimitSign()
|
||||
{
|
||||
return m_limitSign;
|
||||
}
|
||||
|
||||
inline bool getAngularOnly()
|
||||
{
|
||||
return m_angularOnly;
|
||||
}
|
||||
inline bool getEnableAngularMotor()
|
||||
{
|
||||
return m_enableAngularMotor;
|
||||
}
|
||||
inline btScalar getMotorTargetVelosity()
|
||||
{
|
||||
return m_motorTargetVelocity;
|
||||
}
|
||||
inline btScalar getMaxMotorImpulse()
|
||||
{
|
||||
return m_maxMotorImpulse;
|
||||
}
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btHingeConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformDoubleData m_rbBFrame;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
int m_enableAngularMotor;
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
|
||||
};
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btHingeConstraintFloatData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformFloatData m_rbBFrame;
|
||||
int m_useReferenceFrameA;
|
||||
int m_angularOnly;
|
||||
|
||||
int m_enableAngularMotor;
|
||||
float m_motorTargetVelocity;
|
||||
float m_maxMotorImpulse;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_limitSoftness;
|
||||
float m_biasFactor;
|
||||
float m_relaxationFactor;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btHingeConstraintData);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
|
||||
btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
|
||||
|
||||
m_rbAFrame.serialize(hingeData->m_rbAFrame);
|
||||
m_rbBFrame.serialize(hingeData->m_rbBFrame);
|
||||
|
||||
hingeData->m_angularOnly = m_angularOnly;
|
||||
hingeData->m_enableAngularMotor = m_enableAngularMotor;
|
||||
hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
|
||||
hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
|
||||
hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
|
||||
|
||||
hingeData->m_lowerLimit = float(m_lowerLimit);
|
||||
hingeData->m_upperLimit = float(m_upperLimit);
|
||||
hingeData->m_limitSoftness = float(m_limitSoftness);
|
||||
hingeData->m_biasFactor = float(m_biasFactor);
|
||||
hingeData->m_relaxationFactor = float(m_relaxationFactor);
|
||||
|
||||
return btHingeConstraintDataName;
|
||||
}
|
||||
|
||||
#endif //HINGECONSTRAINT_H
|
||||
|
|
@ -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/btVector3.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 btJacobianEntry 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
|
||||
ATTRIBUTE_ALIGNED16(class) btJacobianEntry
|
||||
{
|
||||
public:
|
||||
btJacobianEntry() {};
|
||||
//constraint between two different rigidbodies
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar massInvA,
|
||||
const btVector3& inertiaInvB,
|
||||
const btScalar 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);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& jointAxis,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
{
|
||||
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);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
btJacobianEntry(const btVector3& axisInA,
|
||||
const btVector3& axisInB,
|
||||
const btVector3& inertiaInvA,
|
||||
const btVector3& inertiaInvB)
|
||||
: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
|
||||
, 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);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
btJacobianEntry(
|
||||
const btMatrix3x3& world2A,
|
||||
const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
const btVector3& jointAxis,
|
||||
const btVector3& inertiaInvA,
|
||||
const btScalar 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 = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
btAssert(m_Adiag > btScalar(0.0));
|
||||
}
|
||||
|
||||
btScalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
|
||||
btScalar 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)
|
||||
btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
|
||||
{
|
||||
const btJacobianEntry& jacA = *this;
|
||||
btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
btVector3 lin0 = massInvA * lin ;
|
||||
btVector3 lin1 = massInvB * lin;
|
||||
btVector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
}
|
||||
|
||||
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
|
||||
{
|
||||
btVector3 linrel = linvelA - linvelB;
|
||||
btVector3 angvela = angvelA * m_aJ;
|
||||
btVector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
return rel_vel2 + SIMD_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
btVector3 m_linearJointAxis;
|
||||
btVector3 m_aJ;
|
||||
btVector3 m_bJ;
|
||||
btVector3 m_0MinvJt;
|
||||
btVector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
btScalar m_Adiag;
|
||||
|
||||
};
|
||||
|
||||
#endif //JACOBIAN_ENTRY_H
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
/*
|
||||
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/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
|
||||
#else
|
||||
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
|
||||
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
|
||||
#endif //BT_USE_DOUBLE_PRECISION
|
||||
|
||||
struct btConstraintSetting
|
||||
{
|
||||
btConstraintSetting() :
|
||||
m_tau(btScalar(0.3)),
|
||||
m_damping(btScalar(1.)),
|
||||
m_impulseClamp(btScalar(0.))
|
||||
{
|
||||
}
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
btScalar m_impulseClamp;
|
||||
};
|
||||
|
||||
enum btPoint2PointFlags
|
||||
{
|
||||
BT_P2P_FLAGS_ERP = 1,
|
||||
BT_P2P_FLAGS_CFM = 2
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
|
||||
int m_flags;
|
||||
btScalar m_erp;
|
||||
btScalar m_cfm;
|
||||
|
||||
public:
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
btConstraintSetting m_setting;
|
||||
|
||||
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
|
||||
|
||||
btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
|
||||
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
void setPivotA(const btVector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void setPivotB(const btVector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
|
||||
const btVector3& getPivotInA() const
|
||||
{
|
||||
return m_pivotInA;
|
||||
}
|
||||
|
||||
const btVector3& getPivotInB() const
|
||||
{
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintFloatData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3FloatData m_pivotInA;
|
||||
btVector3FloatData m_pivotInB;
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btPoint2PointConstraintDoubleData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btVector3DoubleData m_pivotInA;
|
||||
btVector3DoubleData m_pivotInB;
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btPoint2PointConstraintData);
|
||||
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
|
||||
|
||||
btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
|
||||
m_pivotInA.serialize(p2pData->m_pivotInA);
|
||||
m_pivotInB.serialize(p2pData->m_pivotInB);
|
||||
|
||||
return btPoint2PointConstraintDataName;
|
||||
}
|
||||
|
||||
#endif //POINT2POINTCONSTRAINT_H
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
/*
|
||||
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 btIDebugDraw;
|
||||
#include "btContactConstraint.h"
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
|
||||
class btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
{
|
||||
protected:
|
||||
|
||||
btConstraintArray m_tmpSolverContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverNonContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderTmpConstraintPool;
|
||||
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
|
||||
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
|
||||
|
||||
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB,
|
||||
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2);
|
||||
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
// void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
|
||||
|
||||
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
void resolveSplitPenetrationSIMD(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSplitPenetrationImpulseCacheFriendly(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
//internal method
|
||||
int getOrInitSolverBody(btCollisionObject& body);
|
||||
|
||||
void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
|
||||
protected:
|
||||
static btRigidBody& getFixedBody();
|
||||
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
|
||||
btSequentialImpulseConstraintSolver();
|
||||
virtual ~btSequentialImpulseConstraintSolver();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
|
||||
|
||||
|
||||
|
||||
///clear internal cached data and reset random seed
|
||||
virtual void reset();
|
||||
|
||||
unsigned long btRand2();
|
||||
|
||||
int btRandInt2 (int n);
|
||||
|
||||
void setRandSeed(unsigned long seed)
|
||||
{
|
||||
m_btSeed2 = seed;
|
||||
}
|
||||
unsigned long getRandSeed() const
|
||||
{
|
||||
return m_btSeed2;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#ifndef BT_PREFER_SIMD
|
||||
typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
|
||||
#endif
|
||||
|
||||
|
||||
#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
|
|
@ -0,0 +1,321 @@
|
|||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
Added by Roman Ponomarev (rponom@gmail.com)
|
||||
April 04, 2008
|
||||
|
||||
TODO:
|
||||
- add clamping od accumulated impulse to improve stability
|
||||
- add conversion for ODE constraint solver
|
||||
*/
|
||||
|
||||
#ifndef SLIDER_CONSTRAINT_H
|
||||
#define SLIDER_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
|
||||
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
|
||||
|
||||
|
||||
enum btSliderFlags
|
||||
{
|
||||
BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
|
||||
BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
|
||||
BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
|
||||
BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
|
||||
BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
|
||||
BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
|
||||
BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
|
||||
BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
|
||||
BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
|
||||
BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
|
||||
BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
|
||||
BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
|
||||
};
|
||||
|
||||
|
||||
class btSliderConstraint : public btTypedConstraint
|
||||
{
|
||||
protected:
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
btTransform m_frameInA;
|
||||
btTransform m_frameInB;
|
||||
// use frameA fo define limits, if true
|
||||
bool m_useLinearReferenceFrameA;
|
||||
// linear limits
|
||||
btScalar m_lowerLinLimit;
|
||||
btScalar m_upperLinLimit;
|
||||
// angular limits
|
||||
btScalar m_lowerAngLimit;
|
||||
btScalar m_upperAngLimit;
|
||||
// softness, restitution and damping for different cases
|
||||
// DirLin - moving inside linear limits
|
||||
// LimLin - hitting linear limit
|
||||
// DirAng - moving inside angular limits
|
||||
// LimAng - hitting angular limit
|
||||
// OrthoLin, OrthoAng - against constraint axis
|
||||
btScalar m_softnessDirLin;
|
||||
btScalar m_restitutionDirLin;
|
||||
btScalar m_dampingDirLin;
|
||||
btScalar m_cfmDirLin;
|
||||
|
||||
btScalar m_softnessDirAng;
|
||||
btScalar m_restitutionDirAng;
|
||||
btScalar m_dampingDirAng;
|
||||
btScalar m_cfmDirAng;
|
||||
|
||||
btScalar m_softnessLimLin;
|
||||
btScalar m_restitutionLimLin;
|
||||
btScalar m_dampingLimLin;
|
||||
btScalar m_cfmLimLin;
|
||||
|
||||
btScalar m_softnessLimAng;
|
||||
btScalar m_restitutionLimAng;
|
||||
btScalar m_dampingLimAng;
|
||||
btScalar m_cfmLimAng;
|
||||
|
||||
btScalar m_softnessOrthoLin;
|
||||
btScalar m_restitutionOrthoLin;
|
||||
btScalar m_dampingOrthoLin;
|
||||
btScalar m_cfmOrthoLin;
|
||||
|
||||
btScalar m_softnessOrthoAng;
|
||||
btScalar m_restitutionOrthoAng;
|
||||
btScalar m_dampingOrthoAng;
|
||||
btScalar m_cfmOrthoAng;
|
||||
|
||||
// for interlal use
|
||||
bool m_solveLinLim;
|
||||
bool m_solveAngLim;
|
||||
|
||||
int m_flags;
|
||||
|
||||
btJacobianEntry m_jacLin[3];
|
||||
btScalar m_jacLinDiagABInv[3];
|
||||
|
||||
btJacobianEntry m_jacAng[3];
|
||||
|
||||
btScalar m_timeStep;
|
||||
btTransform m_calculatedTransformA;
|
||||
btTransform m_calculatedTransformB;
|
||||
|
||||
btVector3 m_sliderAxis;
|
||||
btVector3 m_realPivotAInW;
|
||||
btVector3 m_realPivotBInW;
|
||||
btVector3 m_projPivotInW;
|
||||
btVector3 m_delta;
|
||||
btVector3 m_depth;
|
||||
btVector3 m_relPosA;
|
||||
btVector3 m_relPosB;
|
||||
|
||||
btScalar m_linPos;
|
||||
btScalar m_angPos;
|
||||
|
||||
btScalar m_angDepth;
|
||||
btScalar m_kAngle;
|
||||
|
||||
bool m_poweredLinMotor;
|
||||
btScalar m_targetLinMotorVelocity;
|
||||
btScalar m_maxLinMotorForce;
|
||||
btScalar m_accumulatedLinMotorImpulse;
|
||||
|
||||
bool m_poweredAngMotor;
|
||||
btScalar m_targetAngMotorVelocity;
|
||||
btScalar m_maxAngMotorForce;
|
||||
btScalar m_accumulatedAngMotorImpulse;
|
||||
|
||||
//------------------------
|
||||
void initParams();
|
||||
public:
|
||||
// constructors
|
||||
btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
||||
|
||||
// overrides
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
|
||||
|
||||
|
||||
// access
|
||||
const btRigidBody& getRigidBodyA() const { return m_rbA; }
|
||||
const btRigidBody& getRigidBodyB() const { return m_rbB; }
|
||||
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
|
||||
const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
|
||||
const btTransform & getFrameOffsetA() const { return m_frameInA; }
|
||||
const btTransform & getFrameOffsetB() const { return m_frameInB; }
|
||||
btTransform & getFrameOffsetA() { return m_frameInA; }
|
||||
btTransform & getFrameOffsetB() { return m_frameInB; }
|
||||
btScalar getLowerLinLimit() { return m_lowerLinLimit; }
|
||||
void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
|
||||
btScalar getUpperLinLimit() { return m_upperLinLimit; }
|
||||
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
|
||||
btScalar getLowerAngLimit() { return m_lowerAngLimit; }
|
||||
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
|
||||
btScalar getUpperAngLimit() { return m_upperAngLimit; }
|
||||
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
|
||||
bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
|
||||
btScalar getSoftnessDirLin() { return m_softnessDirLin; }
|
||||
btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
|
||||
btScalar getDampingDirLin() { return m_dampingDirLin ; }
|
||||
btScalar getSoftnessDirAng() { return m_softnessDirAng; }
|
||||
btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
|
||||
btScalar getDampingDirAng() { return m_dampingDirAng; }
|
||||
btScalar getSoftnessLimLin() { return m_softnessLimLin; }
|
||||
btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
|
||||
btScalar getDampingLimLin() { return m_dampingLimLin; }
|
||||
btScalar getSoftnessLimAng() { return m_softnessLimAng; }
|
||||
btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
|
||||
btScalar getDampingLimAng() { return m_dampingLimAng; }
|
||||
btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
|
||||
btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
|
||||
btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
|
||||
btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
|
||||
btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
|
||||
btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
|
||||
void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
|
||||
void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
|
||||
void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
|
||||
void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
|
||||
void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
|
||||
void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
|
||||
void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
|
||||
void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
|
||||
void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
|
||||
void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
|
||||
void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
|
||||
void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
|
||||
void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
|
||||
void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
|
||||
void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
|
||||
void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
|
||||
void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
|
||||
void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
|
||||
void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
|
||||
bool getPoweredLinMotor() { return m_poweredLinMotor; }
|
||||
void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
|
||||
btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
|
||||
void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
|
||||
btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
|
||||
void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
|
||||
bool getPoweredAngMotor() { return m_poweredAngMotor; }
|
||||
void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
|
||||
btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
|
||||
void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
|
||||
btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
|
||||
btScalar getLinearPos() { return m_linPos; }
|
||||
|
||||
|
||||
// access for ODE solver
|
||||
bool getSolveLinLimit() { return m_solveLinLim; }
|
||||
btScalar getLinDepth() { return m_depth[0]; }
|
||||
bool getSolveAngLimit() { return m_solveAngLim; }
|
||||
btScalar getAngDepth() { return m_angDepth; }
|
||||
// shared code used by ODE solver
|
||||
void calculateTransforms(const btTransform& transA,const btTransform& transB);
|
||||
void testLinLimits();
|
||||
void testAngLimits();
|
||||
// access for PE Solver
|
||||
btVector3 getAncorInA();
|
||||
btVector3 getAncorInB();
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btSliderConstraintData
|
||||
{
|
||||
btTypedConstraintData m_typeConstraintData;
|
||||
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
btTransformFloatData m_rbBFrame;
|
||||
|
||||
float m_linearUpperLimit;
|
||||
float m_linearLowerLimit;
|
||||
|
||||
float m_angularUpperLimit;
|
||||
float m_angularLowerLimit;
|
||||
|
||||
int m_useLinearReferenceFrameA;
|
||||
int m_useOffsetForConstraintFrame;
|
||||
|
||||
};
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btSliderConstraintData);
|
||||
}
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
||||
{
|
||||
|
||||
btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
|
||||
btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
|
||||
|
||||
m_frameInA.serializeFloat(sliderData->m_rbAFrame);
|
||||
m_frameInB.serializeFloat(sliderData->m_rbBFrame);
|
||||
|
||||
sliderData->m_linearUpperLimit = float(m_upperLinLimit);
|
||||
sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
|
||||
|
||||
sliderData->m_angularUpperLimit = float(m_upperAngLimit);
|
||||
sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
|
||||
|
||||
sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
|
||||
sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
|
||||
|
||||
return "btSliderConstraintData";
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //SLIDER_CONSTRAINT_H
|
||||
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/*
|
||||
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/btMatrix3x3.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
|
||||
|
||||
/// constraint class used for lateral tyre friction.
|
||||
class btSolve2LinearConstraint
|
||||
{
|
||||
btScalar m_tau;
|
||||
btScalar m_damping;
|
||||
|
||||
public:
|
||||
|
||||
btSolve2LinearConstraint(btScalar tau,btScalar damping)
|
||||
{
|
||||
m_tau = tau;
|
||||
m_damping = damping;
|
||||
}
|
||||
//
|
||||
// solve unilateral constraint (equality, direct method)
|
||||
//
|
||||
void resolveUnilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
|
||||
//
|
||||
// solving 2x2 lcp problem (inequality, direct solution )
|
||||
//
|
||||
void resolveBilateralPairConstraint(
|
||||
btRigidBody* body0,
|
||||
btRigidBody* body1,
|
||||
const btMatrix3x3& world2A,
|
||||
const btMatrix3x3& world2B,
|
||||
|
||||
const btVector3& invInertiaADiag,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btVector3& invInertiaBDiag,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
/*
|
||||
void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
|
||||
const btScalar invMassA,
|
||||
const btVector3& linvelA,const btVector3& angvelA,
|
||||
const btVector3& rel_posA1,
|
||||
const btMatrix3x3& invInertiaBWS,
|
||||
const btScalar invMassB,
|
||||
const btVector3& linvelB,const btVector3& angvelB,
|
||||
const btVector3& rel_posA2,
|
||||
|
||||
btScalar depthA, const btVector3& normalA,
|
||||
const btVector3& rel_posB1,const btVector3& rel_posB2,
|
||||
btScalar depthB, const btVector3& normalB,
|
||||
btScalar& imp0,btScalar& imp1);
|
||||
|
||||
*/
|
||||
|
||||
};
|
||||
|
||||
#endif //SOLVE_2LINEAR_CONSTRAINT_H
|
||||
|
|
@ -0,0 +1,191 @@
|
|||
/*
|
||||
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 BT_SOLVER_BODY_H
|
||||
#define BT_SOLVER_BODY_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
|
||||
#ifdef BT_USE_SSE
|
||||
#define USE_SIMD 1
|
||||
#endif //
|
||||
|
||||
|
||||
#ifdef USE_SIMD
|
||||
|
||||
struct btSimdScalar
|
||||
{
|
||||
SIMD_FORCE_INLINE btSimdScalar()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(float fl)
|
||||
:m_vec128 (_mm_set1_ps(fl))
|
||||
{
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
|
||||
:m_vec128(v128)
|
||||
{
|
||||
}
|
||||
union
|
||||
{
|
||||
__m128 m_vec128;
|
||||
float m_floats[4];
|
||||
int m_ints[4];
|
||||
btScalar m_unusedPadding;
|
||||
};
|
||||
SIMD_FORCE_INLINE __m128 get128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE const __m128 get128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void set128(__m128 v128)
|
||||
{
|
||||
m_vec128 = v128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator __m128()
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
SIMD_FORCE_INLINE operator const __m128() const
|
||||
{
|
||||
return m_vec128;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE operator float() const
|
||||
{
|
||||
return m_floats[0];
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
///@brief Return the elementwise product of two btSimdScalar
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
{
|
||||
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
|
||||
}
|
||||
|
||||
///@brief Return the elementwise product of two btSimdScalar
|
||||
SIMD_FORCE_INLINE btSimdScalar
|
||||
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
{
|
||||
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
#define btSimdScalar btScalar
|
||||
#endif
|
||||
|
||||
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btVector3 m_angularFactor;
|
||||
btVector3 m_invMass;
|
||||
btRigidBody* m_originalBody;
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
|
||||
else
|
||||
velocity.setValue(0,0,0);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
|
||||
else
|
||||
angVel.setValue(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
//if (m_invMass)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_pushVelocity += linearComponent*impulseMagnitude;
|
||||
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocity(btScalar timeStep)
|
||||
{
|
||||
(void) timeStep;
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
btTransform newTransform;
|
||||
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
|
||||
m_originalBody->setWorldTransform(newTransform);
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
/*
|
||||
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 BT_SOLVER_CONSTRAINT_H
|
||||
#define BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
|
||||
//#define NO_FRICTION_TANGENTIALS 1
|
||||
#include "btSolverBody.h"
|
||||
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal;
|
||||
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
//btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
union
|
||||
{
|
||||
int m_numConsecutiveRowsPerKernel;
|
||||
btScalar m_unusedPadding0;
|
||||
};
|
||||
|
||||
union
|
||||
{
|
||||
int m_frictionIndex;
|
||||
btScalar m_unusedPadding1;
|
||||
};
|
||||
union
|
||||
{
|
||||
btRigidBody* m_solverBodyA;
|
||||
int m_companionIdA;
|
||||
};
|
||||
union
|
||||
{
|
||||
btRigidBody* m_solverBodyB;
|
||||
int m_companionIdB;
|
||||
};
|
||||
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
};
|
||||
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
|
||||
btScalar m_rhsPenetration;
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
|
||||
|
||||
|
||||
#endif //BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,315 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2010 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 btRigidBody;
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
|
||||
class btSerializer;
|
||||
|
||||
enum btTypedConstraintType
|
||||
{
|
||||
POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
|
||||
HINGE_CONSTRAINT_TYPE,
|
||||
CONETWIST_CONSTRAINT_TYPE,
|
||||
D6_CONSTRAINT_TYPE,
|
||||
SLIDER_CONSTRAINT_TYPE,
|
||||
CONTACT_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
|
||||
enum btConstraintParams
|
||||
{
|
||||
BT_CONSTRAINT_ERP=1,
|
||||
BT_CONSTRAINT_STOP_ERP,
|
||||
BT_CONSTRAINT_CFM,
|
||||
BT_CONSTRAINT_STOP_CFM
|
||||
};
|
||||
|
||||
#if 1
|
||||
#define btAssertConstrParams(_par) btAssert(_par)
|
||||
#else
|
||||
#define btAssertConstrParams(_par)
|
||||
#endif
|
||||
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
class btTypedConstraint : public btTypedObject
|
||||
{
|
||||
int m_userConstraintType;
|
||||
|
||||
union
|
||||
{
|
||||
int m_userConstraintId;
|
||||
void* m_userConstraintPtr;
|
||||
};
|
||||
|
||||
bool m_needsFeedback;
|
||||
|
||||
btTypedConstraint& operator=(btTypedConstraint& other)
|
||||
{
|
||||
btAssert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
btRigidBody& m_rbA;
|
||||
btRigidBody& m_rbB;
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_dbgDrawSize;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
|
||||
|
||||
static btRigidBody& getFixedBody();
|
||||
|
||||
public:
|
||||
|
||||
virtual ~btTypedConstraint() {};
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
|
||||
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
|
||||
|
||||
struct btConstraintInfo1 {
|
||||
int m_numConstraintRows,nub;
|
||||
};
|
||||
|
||||
struct btConstraintInfo2 {
|
||||
// integrator parameters: frames per second (1/stepsize), default error
|
||||
// reduction parameter (0..1).
|
||||
btScalar fps,erp;
|
||||
|
||||
// for the first and second body, pointers to two (linear and angular)
|
||||
// n*3 jacobian sub matrices, stored by rows. these matrices will have
|
||||
// been initialized to 0 on entry. if the second body is zero then the
|
||||
// J2xx pointers may be 0.
|
||||
btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
|
||||
|
||||
// elements to jump from one row to the next in J's
|
||||
int rowskip;
|
||||
|
||||
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
|
||||
// "constraint force mixing" vector. c is set to zero on entry, cfm is
|
||||
// set to a constant value (typically very small or zero) value on entry.
|
||||
btScalar *m_constraintError,*cfm;
|
||||
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
btScalar *m_lowerLimit,*m_upperLimit;
|
||||
|
||||
// findex vector for variables. see the LCP solver interface for a
|
||||
// description of what this does. this is set to -1 on entry.
|
||||
// note that the returned indexes are relative to the first index of
|
||||
// the constraint.
|
||||
int *findex;
|
||||
// number of solver iterations
|
||||
int m_numIterations;
|
||||
|
||||
//damping of the velocity
|
||||
btScalar m_damping;
|
||||
};
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void buildJacobian() {};
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
|
||||
{
|
||||
(void)ca;
|
||||
(void)solverBodyA;
|
||||
(void)solverBodyB;
|
||||
(void)timeStep;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo1 (btConstraintInfo1* info)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (btConstraintInfo2* info)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
void internalSetAppliedImpulse(btScalar appliedImpulse)
|
||||
{
|
||||
m_appliedImpulse = appliedImpulse;
|
||||
}
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
btScalar internalGetAppliedImpulse()
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {};
|
||||
|
||||
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
btRigidBody& 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() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
void setUserConstraintPtr(void* ptr)
|
||||
{
|
||||
m_userConstraintPtr = ptr;
|
||||
}
|
||||
|
||||
void* getUserConstraintPtr()
|
||||
{
|
||||
return m_userConstraintPtr;
|
||||
}
|
||||
|
||||
int getUid() const
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
|
||||
bool needsFeedback() const
|
||||
{
|
||||
return m_needsFeedback;
|
||||
}
|
||||
|
||||
///enableFeedback will allow to read the applied linear and angular impulse
|
||||
///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
|
||||
void enableFeedback(bool needsFeedback)
|
||||
{
|
||||
m_needsFeedback = needsFeedback;
|
||||
}
|
||||
|
||||
///getAppliedImpulse is an estimated total applied impulse.
|
||||
///This feedback could be used to determine breaking constraints or playing sounds.
|
||||
btScalar getAppliedImpulse() const
|
||||
{
|
||||
btAssert(m_needsFeedback);
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
btTypedConstraintType getConstraintType () const
|
||||
{
|
||||
return btTypedConstraintType(m_objectType);
|
||||
}
|
||||
|
||||
void setDbgDrawSize(btScalar dbgDrawSize)
|
||||
{
|
||||
m_dbgDrawSize = dbgDrawSize;
|
||||
}
|
||||
btScalar getDbgDrawSize()
|
||||
{
|
||||
return m_dbgDrawSize;
|
||||
}
|
||||
|
||||
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
||||
///If no axis is provided, it uses the default axis for this constraint.
|
||||
virtual void setParam(int num, btScalar value, int axis = -1) = 0;
|
||||
|
||||
///return the local value of parameter
|
||||
virtual btScalar getParam(int num, int axis = -1) const = 0;
|
||||
|
||||
virtual int calculateSerializeBufferSize() const;
|
||||
|
||||
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
||||
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
||||
|
||||
};
|
||||
|
||||
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
|
||||
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
|
||||
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
|
||||
{
|
||||
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
else if(angleInRadians < angleLowerLimitInRadians)
|
||||
{
|
||||
btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
|
||||
btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
|
||||
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
|
||||
}
|
||||
else if(angleInRadians > angleUpperLimitInRadians)
|
||||
{
|
||||
btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
|
||||
btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
|
||||
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
|
||||
}
|
||||
else
|
||||
{
|
||||
return angleInRadians;
|
||||
}
|
||||
}
|
||||
|
||||
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
|
||||
struct btTypedConstraintData
|
||||
{
|
||||
btRigidBodyData *m_rbA;
|
||||
btRigidBodyData *m_rbB;
|
||||
char *m_name;
|
||||
|
||||
int m_objectType;
|
||||
int m_userConstraintType;
|
||||
int m_userConstraintId;
|
||||
int m_needsFeedback;
|
||||
|
||||
float m_appliedImpulse;
|
||||
float m_dbgDrawSize;
|
||||
|
||||
int m_disableCollisionsBetweenLinkedBodies;
|
||||
char m_pad4[4];
|
||||
|
||||
};
|
||||
|
||||
SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
|
||||
{
|
||||
return sizeof(btTypedConstraintData);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //TYPED_CONSTRAINT_H
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
|
||||
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef UNIVERSAL_CONSTRAINT_H
|
||||
#define UNIVERSAL_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "btGeneric6DofConstraint.h"
|
||||
|
||||
|
||||
|
||||
/// Constraint similar to ODE Universal Joint
|
||||
/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
|
||||
/// and Y (axis 2)
|
||||
/// Description from ODE manual :
|
||||
/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
|
||||
/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
|
||||
|
||||
class btUniversalConstraint : public btGeneric6DofConstraint
|
||||
{
|
||||
protected:
|
||||
btVector3 m_anchor;
|
||||
btVector3 m_axis1;
|
||||
btVector3 m_axis2;
|
||||
public:
|
||||
// constructor
|
||||
// anchor, axis1 and axis2 are in world coordinate system
|
||||
// axis1 must be orthogonal to axis2
|
||||
btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
|
||||
// access
|
||||
const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
|
||||
const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
|
||||
const btVector3& getAxis1() { return m_axis1; }
|
||||
const btVector3& getAxis2() { return m_axis2; }
|
||||
btScalar getAngle1() { return getAngle(2); }
|
||||
btScalar getAngle2() { return getAngle(1); }
|
||||
// limits
|
||||
void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
|
||||
void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // UNIVERSAL_CONSTRAINT_H
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue