void dCustomJoint::dDebugDisplay::DrawFrame(const dMatrix& matrix)
{
dVector o0(matrix.m_posit);
dFloat size = 0.25f;
dVector x(matrix.m_posit + matrix.RotateVector(dVector(size, 0.0f, 0.0f, 0.0f)));
SetColor(dVector (1.0f, 0.0f, 0.0f));
DrawLine (matrix.m_posit, x);
dVector y(matrix.m_posit + matrix.RotateVector(dVector(0.0f, size, 0.0f, 0.0f)));
SetColor(dVector (0.0f, 1.0f, 0.0f));
DrawLine (matrix.m_posit, y);
dVector z(matrix.m_posit + matrix.RotateVector(dVector(0.0f, 0.0f, size, 0.0f)));
SetColor(dVector (0.0f, 0.0f, 1.0f));
DrawLine (matrix.m_posit, z);
}
void dRigidbodyNodeInfo::BakeTransform (const dMatrix& transform)
{
// SetTransform (transform.Inverse4x4() * GetTransform() * transform);
dNodeInfo::BakeTransform (transform);
m_centerOfMass = transform.UnrotateVector(m_centerOfMass);
m_massMatrix = transform.UnrotateVector(m_massMatrix);
m_velocity = transform.RotateVector(m_velocity);
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
rollPin = matrix1.RotateVector(rollPin);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(euler0.m_x);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
void CustomPlayerController::Init(dFloat mass, dFloat outerRadius, dFloat innerRadius, dFloat height, dFloat stairStep, const dMatrix& localAxis)
{
dAssert (stairStep >= 0.0f);
dAssert (innerRadius >= 0.0f);
dAssert (outerRadius >= innerRadius);
dAssert (height >= stairStep);
dAssert (localAxis[0].m_w == dFloat (0.0f));
dAssert (localAxis[1].m_w == dFloat (0.0f));
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
SetRestrainingDistance (0.0f);
m_outerRadio = outerRadius;
m_innerRadio = innerRadius;
m_height = height;
m_stairStep = stairStep;
SetClimbSlope(45.0f * 3.1416f/ 180.0f);
m_upVector = localAxis[0];
m_frontVector = localAxis[1];
m_groundPlane = dVector (0.0f, 0.0f, 0.0f, 0.0f);
m_groundVelocity = dVector (0.0f, 0.0f, 0.0f, 0.0f);
const int steps = 12;
dVector convexPoints[2][steps];
// create an inner thin cylinder
dFloat shapeHigh = height;
dAssert (shapeHigh > 0.0f);
dVector p0 (0.0f, m_innerRadio, 0.0f, 0.0f);
dVector p1 (shapeHigh, m_innerRadio, 0.0f, 0.0f);
for (int i = 0; i < steps; i ++) {
dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(p0));
convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(p1));
}
NewtonCollision* const supportShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL);
// create the outer thick cylinder
dMatrix outerShapeMatrix (localAxis);
dFloat capsuleHigh = m_height - stairStep;
dAssert (capsuleHigh > 0.0f);
m_sphereCastOrigin = capsuleHigh * 0.5f + stairStep;
outerShapeMatrix.m_posit = outerShapeMatrix[0].Scale(m_sphereCastOrigin);
outerShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollision* const bodyCapsule = NewtonCreateCapsule(world, 0.25f, 0.5f, 0, &outerShapeMatrix[0][0]);
NewtonCollisionSetScale(bodyCapsule, capsuleHigh, m_outerRadio * 4.0f, m_outerRadio * 4.0f);
// compound collision player controller
NewtonCollision* const playerShape = NewtonCreateCompoundCollision(world, 0);
NewtonCompoundCollisionBeginAddRemove(playerShape);
NewtonCompoundCollisionAddSubCollision (playerShape, supportShape);
NewtonCompoundCollisionAddSubCollision (playerShape, bodyCapsule);
NewtonCompoundCollisionEndAddRemove (playerShape);
// create the kinematic body
dMatrix locationMatrix (dGetIdentityMatrix());
m_body = NewtonCreateKinematicBody(world, playerShape, &locationMatrix[0][0]);
// players must have weight, otherwise they are infinitely strong when they collide
NewtonCollision* const shape = NewtonBodyGetCollision(m_body);
NewtonBodySetMassProperties(m_body, mass, shape);
// make the body collidable with other dynamics bodies, by default
NewtonBodySetCollidable (m_body, true);
dFloat castHigh = capsuleHigh * 0.4f;
dFloat castRadio = (m_innerRadio * 0.5f > 0.05f) ? m_innerRadio * 0.5f : 0.05f;
dVector q0 (0.0f, castRadio, 0.0f, 0.0f);
dVector q1 (castHigh, castRadio, 0.0f, 0.0f);
for (int i = 0; i < steps; i ++) {
dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(q0));
convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(q1));
}
m_castingShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL);
m_supportShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 0));
m_upperBodyShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 1));
NewtonDestroyCollision (bodyCapsule);
NewtonDestroyCollision (supportShape);
NewtonDestroyCollision (playerShape);
m_isJumping = false;
}