Roket_PhysicsJointUniversal::Roket_PhysicsJointUniversal(
NewtonWorld* worldd,
irr::core::vector3df point,
irr::core::vector3df pin0,
irr::core::vector3df pin1,
Roket_PhysicsBody* child,
Roket_PhysicsBody* parent,
RPhysicsJoint* rjoint)
{
world = worldd;
float pointD[3];
pointD[0] = point.X;
pointD[1] = point.Y;
pointD[2] = point.Z;
float pointP0[3];
pointP0[0] = pin0.X;
pointP0[1] = pin0.Y;
pointP0[2] = pin0.Z;
float pointP1[3];
pointP1[0] = pin1.X;
pointP1[1] = pin1.Y;
pointP1[2] = pin1.Z;
if (parent == NULL)
joint = NewtonConstraintCreateUniversal(world, &pointD[0], &pointP0[0], &pointP1[0], child->body, NULL);
else
joint = NewtonConstraintCreateUniversal(world, &pointD[0], &pointP0[0], &pointP1[0], child->body, parent->body);
NewtonJointSetUserData(joint, &rjoint);
NewtonJointSetDestructor(joint, &JointDestructor);
}
void PhyFixed::createPrivateJoint()
{
#ifdef WORLDSIM_USE_NEWTON
m_priv->joint = NewtonConstraintCreateUserJoint( m_worldPriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, m_priv->child, m_priv->parent );
NewtonJointSetUserData( m_priv->joint, this );
#endif
}
NewtonCustomJoint::NewtonCustomJoint (int maxDOF, NewtonBody* body0, NewtonBody* body1)
{
m_joint = NULL;
m_body0 = body0;
m_body1 = body1;
m_maxDox = maxDOF;
m_world = NewtonBodyGetWorld (body0);
m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstrainst, m_body0, m_body1);
NewtonJointSetUserData (m_joint, this);
NewtonJointSetDestructor (m_joint, Destructor);
}
void NewtonCustomJoint::Destructor (const NewtonJoint* me)
{
NewtonCustomJoint* joint;
// get the pointer to the joint class
joint = (NewtonCustomJoint*) NewtonJointGetUserData (me);
// set the joint call to NULL to prevent infinite recursion
NewtonJointSetDestructor (me, NULL);
NewtonJointSetUserData (me, NULL);
// delete the joint class
delete joint;
}
void CustomJoint::Init (int maxDOF, NewtonBody* const body0, NewtonBody* const body1)
{
m_autoDestroy = 0;
m_joint = NULL;
m_body0 = body0;
m_body1 = body1;
m_maxDof = maxDOF;
m_world = NewtonBodyGetWorld (body0);
m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstraints, GetInfo, m_body0, m_body1);
NewtonJointSetUserData (m_joint, this);
NewtonJointSetDestructor (m_joint, Destructor);
m_userData = NULL;
m_userDestructor = NULL;
m_userConstrationCallback = NULL;
}
void dCustomJoint::Init (int maxDOF, NewtonBody* const body0, NewtonBody* const body1)
{
m_joint = NULL;
m_body0 = body0;
m_body1 = body1;
m_maxDof = maxDOF;
m_autoDestroy = 0;
m_stiffness = 1.0f;
m_world = body0 ? NewtonBodyGetWorld (body0) : NewtonBodyGetWorld (body1);
m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstraints, m_body0, m_body1);
NewtonJointSetUserData (m_joint, this);
NewtonJointSetDestructor (m_joint, Destructor);
m_userData = NULL;
m_userDestructor = NULL;
}
cPhysicsJointScrewNewton::cPhysicsJointScrewNewton(const tString &asName,
iPhysicsBody *apParentBody, iPhysicsBody *apChildBody,
iPhysicsWorld *apWorld,const cVector3f &avPivotPoint, const cVector3f avPinDir)
: iPhysicsJointNewton<iPhysicsJointScrew>(asName,apParentBody,apChildBody,apWorld,avPivotPoint)
{
mvPin = avPinDir;
mvPin.Normalise();
mpNewtonJoint = NewtonConstraintCreateCorkscrew(mpNewtonWorld, avPivotPoint.v, avPinDir.v, mpNewtonChildBody,
mpNewtonParentBody);
NewtonJointSetUserData(mpNewtonJoint, (void*) this);
NewtonCorkscrewSetUserCallback(mpNewtonJoint,LimitCallback);
mfMaxDistance =0;
mfMinDistance =0;
mvPinDir = avPinDir;
mvPivotPoint = avPivotPoint;
}
cPhysicsJointHingeNewton::cPhysicsJointHingeNewton(const tString &asName,
iPhysicsBody *apParentBody, iPhysicsBody *apChildBody,
iPhysicsWorld *apWorld,const cVector3f &avPivotPoint, const cVector3f avPinDir)
: iPhysicsJointNewton<iPhysicsJointHinge>(asName,apParentBody,apChildBody,apWorld,avPivotPoint)
{
mpNewtonJoint = NewtonConstraintCreateHinge(mpNewtonWorld, avPivotPoint.v, avPinDir.v,
mpNewtonChildBody, mpNewtonParentBody);
//Add callback and user data.
NewtonJointSetUserData(mpNewtonJoint, (void*) this);
NewtonHingeSetUserCallback(mpNewtonJoint, LimitCallback);
mvPinDir = avPinDir;
mvPivotPoint = avPivotPoint;
mfMaxAngle = 0;
mfMinAngle = 0;
mfPreviousAngle =0;
}
dCustomJoint::dCustomJoint(NewtonInverseDynamics* const invDynSolver, void* const invDynNode)
:dCustomAlloc()
,m_localMatrix0(dGetIdentityMatrix())
,m_localMatrix1(dGetIdentityMatrix())
{
m_joint = NULL;
m_body1 = NULL;
m_maxDof = 6;
m_autoDestroy = 0;
m_stiffness = 1.0f;
m_body0 = NewtonInverseDynamicsGetBody (invDynSolver, invDynNode);
m_world = NewtonBodyGetWorld(m_body0);
m_joint = NewtonInverseDynamicsCreateEffector(invDynSolver, invDynNode, SubmitConstraints);
NewtonJointSetUserData(m_joint, this);
NewtonJointSetDestructor(m_joint, Destructor);
m_userData = NULL;
m_userDestructor = NULL;
}
iPhysicsJoint* iPhysics::createJoint(iPhysicsBody* body0, iPhysicsBody* body1, int dof)
{
iPhysicsJoint* result = nullptr;
con_assert(body0 != nullptr, "zero pointer");
if (body0 != nullptr)
{
NewtonWaitForUpdateToFinish(static_cast<const NewtonWorld*>(_defaultWorld));
NewtonJoint * joint = NewtonConstraintCreateUserJoint(static_cast<NewtonWorld*>(_defaultWorld), dof, reinterpret_cast<NewtonUserBilateralCallback>(SubmitConstraints), nullptr, static_cast<NewtonBody*>(body0->getNewtonBody()), body1 != nullptr ? static_cast<NewtonBody*>(body1->getNewtonBody()) : nullptr);
result = new iPhysicsJoint(joint, body0->getID(), body1 != nullptr ? body1->getID() : 0);
_jointListMutex.lock();
_joints[result->getID()] = result;
_jointListMutex.unlock();
NewtonJointSetUserData(joint, result);
}
return result;
}
CustomJoint::~CustomJoint()
{
//dAssert (m_joint);
//if the joint has user data it means the application is destroying the joint
// if there is a C destructor call it form here
CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (m_joint);
if (joint->m_userDestructor) {
joint->m_userDestructor ((const NewtonUserJoint*) joint);
}
// if (NewtonJointGetUserData (m_joint)) {
if (!m_autoDestroy) {
// set the joint call to NULL to prevent infinite recursion
NewtonJointSetUserData (m_joint, NULL);
NewtonJointSetDestructor (m_joint, NULL);
// destroy this joint
NewtonDestroyJoint(m_world, m_joint);
}
}
Roket_PhysicsJointCorkscrew::Roket_PhysicsJointCorkscrew(
NewtonWorld* worldd,
irr::core::vector3df point,
irr::core::vector3df pin,
Roket_PhysicsBody* child,
Roket_PhysicsBody* parent,
RPhysicsJoint* rjoint)
{
world = worldd;
float pointD[3];
pointD[0] = point.X;
pointD[1] = point.Y;
pointD[2] = point.Z;
float pointP[3];
pointP[0] = pin.X;
pointP[1] = pin.Y;
pointP[2] = pin.Z;
if (parent == NULL)
joint = NewtonConstraintCreateCorkscrew(world, &pointD[0], &pointP[0], child->body, NULL);
else
joint = NewtonConstraintCreateCorkscrew(world, &pointD[0], &pointP[0], child->body, parent->body);
NewtonJointSetUserData(joint, &rjoint);
NewtonJointSetDestructor(joint, &JointDestructor);
}