312 lines
8.3 KiB
C++
312 lines
8.3 KiB
C++
/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#if !defined(AFX_DGCONSTRAINT_H__F9EC24E0_6E0F_4CD5_909E_A5F5E1AC7C0B__INCLUDED_)
|
|
#define AFX_DGCONSTRAINT_H__F9EC24E0_6E0F_4CD5_909E_A5F5E1AC7C0B__INCLUDED_
|
|
|
|
#define ARRAYSIZE(x) ((int)(sizeof(x) / sizeof(x[0])))
|
|
|
|
#include "dgBodyMasterList.h"
|
|
|
|
#define DG_MAX_BOUND dgFloat32 (1.0e15f)
|
|
#define DG_MIN_BOUND (-DG_MAX_BOUND)
|
|
|
|
|
|
#define DG_BILATERAL_CONSTRAINT -1
|
|
#define DG_NORMAL_CONSTRAINT -2
|
|
#define DG_BILATERAL_FRICTION_CONSTRAINT -3
|
|
|
|
#define DG_CONSTRAINT_MAX_ROWS (3 * 16)
|
|
|
|
#define MIN_JOINT_PIN_LENGTH dgFloat32 (16.0f)
|
|
|
|
class dgBody;
|
|
class dgWorld;
|
|
class dgConstraint;
|
|
class dgBilateralBounds;
|
|
|
|
typedef void(dgApi *ConstraintsForceFeedback)(const NewtonJoint *const userJoint, dFloat timestep, int32 threadIndex);
|
|
|
|
class dgConstraintInfo {
|
|
public:
|
|
|
|
dgMatrix m_attachMatrix_0;
|
|
dgMatrix m_attachMatrix_1;
|
|
dgFloat32 m_minLinearDof[3];
|
|
dgFloat32 m_maxLinearDof[3];
|
|
dgFloat32 m_minAngularDof[3];
|
|
dgFloat32 m_maxAngularDof[3];
|
|
dgBody *m_attachBody_0;
|
|
dgBody *m_attachBody_1;
|
|
dgFloat32 m_extraParameters[16];
|
|
dgInt32 m_collideCollisionOn;
|
|
char m_discriptionType[16];
|
|
|
|
void clear() {
|
|
m_attachMatrix_0 = dgGetZeroMatrix();
|
|
m_attachMatrix_1 = dgGetZeroMatrix();
|
|
for (unsigned i = 0; i < ARRAYSIZE(m_minLinearDof); i++)
|
|
m_minLinearDof[i] = dgFloat32(0.0);
|
|
for (unsigned i = 0; i < ARRAYSIZE(m_maxLinearDof); i++)
|
|
m_maxLinearDof[i] = dgFloat32(0.0);
|
|
for (unsigned i = 0; i < ARRAYSIZE(m_minAngularDof); i++)
|
|
m_minAngularDof[i] = dgFloat32(0.0);
|
|
for (unsigned i = 0; i < ARRAYSIZE(m_maxAngularDof); i++)
|
|
m_maxAngularDof[i] = dgFloat32(0.0);
|
|
m_attachBody_0 = nullptr;
|
|
m_attachBody_1 = nullptr;
|
|
for (unsigned i = 0; i < ARRAYSIZE(m_extraParameters); i++)
|
|
m_extraParameters[i] = dgFloat32(0.0);
|
|
m_collideCollisionOn = dgInt32(0);
|
|
for (unsigned i = 0; i < ARRAYSIZE(m_discriptionType); i++)
|
|
m_discriptionType[i] = 0;
|
|
}
|
|
};
|
|
|
|
|
|
class dgJointCallBackParam {
|
|
public:
|
|
dgFloat32 m_accel;
|
|
dgFloat32 m_minFriction;
|
|
dgFloat32 m_maxFriction;
|
|
dgFloat32 m_timestep;
|
|
};
|
|
|
|
|
|
class dgBilateralBounds {
|
|
public:
|
|
dgFloat32 m_low;
|
|
dgFloat32 m_upper;
|
|
dgInt32 m_normalIndex;
|
|
dgFloat32 *m_jointForce;
|
|
};
|
|
|
|
DG_MSC_VECTOR_ALIGMENT
|
|
class dgJacobian {
|
|
public:
|
|
dgVector m_linear;
|
|
dgVector m_angular;
|
|
} DG_GCC_VECTOR_ALIGMENT;
|
|
|
|
DG_MSC_VECTOR_ALIGMENT
|
|
class dgJacobianPair {
|
|
public:
|
|
dgJacobian m_jacobian_IM0;
|
|
dgJacobian m_jacobian_IM1;
|
|
} DG_GCC_VECTOR_ALIGMENT;
|
|
|
|
class dgJointAccelerationDecriptor {
|
|
public:
|
|
dgInt32 m_rowsCount;
|
|
dgFloat32 m_timeStep;
|
|
dgFloat32 m_invTimeStep;
|
|
dgFloat32 m_firstPassCoefFlag;
|
|
// dgBody *m_body0;
|
|
// dgBody *m_body1;
|
|
dgFloat32 *m_penetration;
|
|
dgFloat32 *m_coordenateAccel;
|
|
const dgJacobianPair *m_Jt;
|
|
const dgFloat32 *m_restitution;
|
|
const dgInt32 *m_accelIsMotor;
|
|
const dgInt32 *m_normalForceIndex;
|
|
const dgFloat32 *m_externAccelaration;
|
|
const dgFloat32 *m_penetrationStiffness;
|
|
|
|
};
|
|
|
|
|
|
DG_MSC_VECTOR_ALIGMENT
|
|
class dgContraintDescritor {
|
|
public:
|
|
dgJacobianPair m_jacobian[DG_CONSTRAINT_MAX_ROWS];
|
|
dgBilateralBounds m_forceBounds[DG_CONSTRAINT_MAX_ROWS];
|
|
dgFloat32 m_jointAccel[DG_CONSTRAINT_MAX_ROWS];
|
|
dgFloat32 m_jointStiffness[DG_CONSTRAINT_MAX_ROWS];
|
|
dgFloat32 m_restitution[DG_CONSTRAINT_MAX_ROWS];
|
|
dgFloat32 m_penetration[DG_CONSTRAINT_MAX_ROWS];
|
|
dgFloat32 m_penetrationStiffness[DG_CONSTRAINT_MAX_ROWS];
|
|
dgUnsigned32 m_isMotor[DG_CONSTRAINT_MAX_ROWS];
|
|
dgWorld *m_world;
|
|
dgInt32 m_threadIndex;
|
|
dgFloat32 m_timestep;
|
|
dgFloat32 m_invTimestep;
|
|
} DG_GCC_VECTOR_ALIGMENT;
|
|
|
|
enum dgConstraintID {
|
|
dgBallConstraintId,
|
|
dgHingeConstraintId,
|
|
dgSliderConstraintId,
|
|
dgContactConstraintId,
|
|
dgUpVectorConstraintId,
|
|
dgUniversalConstraintId,
|
|
dgCorkscrewConstraintId,
|
|
dgPointToCurveConstraintId,
|
|
|
|
dgUnknownConstraintId
|
|
};
|
|
|
|
typedef void(dgApi *OnConstraintDestroy)(const NewtonJoint *const me);
|
|
|
|
DG_MSC_VECTOR_ALIGMENT
|
|
class dgConstraint {
|
|
public:
|
|
DG_CLASS_ALLOCATOR(allocator)
|
|
|
|
dgUnsigned32 GetId() const;
|
|
dgBody *GetBody0() const;
|
|
dgBody *GetBody1() const;
|
|
dgBodyMasterListRow::dgListNode *GetLink0() const;
|
|
dgBodyMasterListRow::dgListNode *GetLink1() const;
|
|
void *GetUserData() const;
|
|
bool IsCollidable() const;
|
|
|
|
|
|
dgInt32 GetMaxDOF() const;
|
|
void SetUserData(void *userData);
|
|
void SetCollidable(bool state);
|
|
virtual void SetDestructorCallback(OnConstraintDestroy destructor) = 0;
|
|
|
|
virtual dgFloat32 GetStiffness() const;
|
|
virtual void SetStiffness(dgFloat32 stiffness);
|
|
virtual void GetInfo(dgConstraintInfo *const info) const;
|
|
|
|
class dgPointParam {
|
|
public:
|
|
dgVector m_r0;
|
|
dgVector m_r1;
|
|
dgVector m_posit0;
|
|
dgVector m_posit1;
|
|
dgVector m_veloc0;
|
|
dgVector m_veloc1;
|
|
dgVector m_centripetal0;
|
|
dgVector m_centripetal1;
|
|
dgFloat32 m_stiffness;
|
|
};
|
|
|
|
|
|
protected:
|
|
dgConstraint();
|
|
virtual ~dgConstraint();
|
|
|
|
virtual bool IsBilateral() const;
|
|
|
|
|
|
virtual dgUnsigned32 JacobianDerivative(dgContraintDescritor ¶ms) = 0;
|
|
virtual void JointAccelerations(const dgJointAccelerationDecriptor ¶ms) = 0;
|
|
virtual void JointAccelerationsSimd(const dgJointAccelerationDecriptor ¶ms) = 0;
|
|
|
|
virtual void JointVelocityCorrection(const dgJointAccelerationDecriptor ¶ms) = 0;
|
|
|
|
void SetUpdateFeedbackFunction(ConstraintsForceFeedback function);
|
|
void InitPointParam(dgPointParam ¶m, dgFloat32 stiffness, const dgVector &p0Global, const dgVector &p1Global) const;
|
|
|
|
|
|
void InitInfo(dgConstraintInfo *const info) const;
|
|
|
|
void *m_userData;
|
|
dgBody *m_body0;
|
|
dgBody *m_body1;
|
|
dgBodyMasterListRow::dgListNode *m_link0;
|
|
dgBodyMasterListRow::dgListNode *m_link1;
|
|
ConstraintsForceFeedback m_updaFeedbackCallback;
|
|
dgUnsigned32 m_dynamicsLru;
|
|
|
|
dgUnsigned32 m_index : 16;
|
|
dgUnsigned32 m_maxDOF : 6;
|
|
dgUnsigned32 m_constId : 6;
|
|
dgUnsigned32 m_enableCollision : 1;
|
|
dgUnsigned32 m_isUnilateral : 1;
|
|
|
|
friend class dgWorld;
|
|
friend class dgJacobianMemory;
|
|
friend class dgBodyMasterList;
|
|
friend class dgWorldDynamicUpdate;
|
|
friend class dgParallelSolverJointAcceleration;
|
|
friend class dgParallelSolverInitFeedbackUpdate;
|
|
friend class dgParallelSolverBuildJacobianMatrix;
|
|
friend class dgBroadPhaseMaterialCallbackWorkerThread;
|
|
} DG_GCC_VECTOR_ALIGMENT;
|
|
|
|
inline dgConstraint::dgConstraint() {
|
|
NEWTON_ASSERT((((dgUnsigned64) this) & 15) == 0);
|
|
|
|
m_link0 = NULL;
|
|
m_link1 = NULL;
|
|
m_body0 = NULL;
|
|
m_body1 = NULL;
|
|
m_userData = NULL;
|
|
|
|
m_maxDOF = 6;
|
|
m_dynamicsLru = 0;
|
|
m_isUnilateral = false;
|
|
m_enableCollision = false;
|
|
m_constId = dgUnknownConstraintId;
|
|
m_updaFeedbackCallback = NULL;
|
|
}
|
|
|
|
inline dgConstraint::~dgConstraint() {
|
|
}
|
|
|
|
inline void dgConstraint::SetUpdateFeedbackFunction(ConstraintsForceFeedback function) {
|
|
m_updaFeedbackCallback = function;
|
|
}
|
|
|
|
inline bool dgConstraint::IsCollidable() const {
|
|
return m_enableCollision ? true : false;
|
|
}
|
|
|
|
inline void dgConstraint::SetCollidable(bool state) {
|
|
m_enableCollision = dgUnsigned32(state);
|
|
}
|
|
|
|
inline dgUnsigned32 dgConstraint::GetId() const {
|
|
return m_constId;
|
|
}
|
|
|
|
inline dgBody *dgConstraint::GetBody0() const {
|
|
return m_body0;
|
|
}
|
|
|
|
inline dgBody *dgConstraint::GetBody1() const {
|
|
return m_body1;
|
|
}
|
|
|
|
inline dgBodyMasterListRow::dgListNode *dgConstraint::GetLink0() const {
|
|
return m_link0;
|
|
}
|
|
inline dgBodyMasterListRow::dgListNode *dgConstraint::GetLink1() const {
|
|
return m_link1;
|
|
}
|
|
|
|
|
|
inline dgFloat32 dgConstraint::GetStiffness() const {
|
|
return dgFloat32(1.0f);
|
|
}
|
|
|
|
inline void dgConstraint::SetStiffness(dgFloat32 stiffness) {
|
|
}
|
|
|
|
inline dgInt32 dgConstraint::GetMaxDOF() const {
|
|
return dgInt32(m_maxDOF);
|
|
}
|
|
|
|
#endif // !defined(AFX_DGCONSTRAINT_H__F9EC24E0_6E0F_4CD5_909E_A5F5E1AC7C0B__INCLUDED_)
|