dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
dgVector lateralDir (matrix0.m_front * matrix1.m_front);
dgInt32 ret = 0;
dgFloat32 mag = lateralDir % lateralDir;
if (mag > dgFloat32 (1.0e-6f)) {
mag = dgSqrt (mag);
lateralDir = lateralDir.Scale3 (dgFloat32 (1.0f) / mag);
dgFloat32 angle = dgAsin (mag);
CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]);
dgVector frontDir (lateralDir * matrix1.m_front);
CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
ret = 2;
} else {
CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]);
CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
ret = 2;
}
return dgUnsigned32 (ret);
}
void dCustomUpVector::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// if the body ha rotated by some amount, the there will be a plane of rotation
dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
dFloat mag = lateralDir.DotProduct3(lateralDir);
if (mag > 1.0e-6f) {
// if the side vector is not zero, it means the body has rotated
mag = dSqrt (mag);
lateralDir = lateralDir.Scale (1.0f / mag);
dFloat angle = dAsin (mag);
// add an angular constraint to correct the error angle
NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);
// in theory only one correction is needed, but this produces instability as the body may move sideway.
// a lateral correction prevent this from happening.
dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
} else {
// if the angle error is very small then two angular correction along the plane axis do the trick
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
}
}
void OnEndUpdate (dFloat timestepInSecunds)
{
DemoCamera* const camera = m_scene->GetCamera();
dMatrix camMatrix(camera->GetNextMatrix());
dMatrix playerMatrix (m_player->GetNextMatrix());
dVector frontDir (camMatrix[0]);
CustomPlayerController* const controller = m_player->m_controller;
dFloat height = controller->GetHigh();
dVector upDir (controller->GetUpDir());
dVector camOrigin(0.0f);
if (m_player->m_inputs.m_cameraMode) {
// set third person view camera
camOrigin = playerMatrix.TransformVector (upDir.Scale(height));
camOrigin -= frontDir.Scale (PLAYER_THIRD_PERSON_VIEW_DIST);
} else {
// set first person view camera
camMatrix = camMatrix * playerMatrix;
camOrigin = playerMatrix.TransformVector (upDir.Scale(height));
}
camera->SetNextMatrix (*m_scene, camMatrix, camOrigin);
// update the shot button
if (m_shootState) {
SpawnRandomProp (camera->GetNextMatrix());
}
}
dVector CustomPlayerController::CalculateDesiredVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, const dVector& gravity, dFloat timestep) const
{
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector updir (matrix.RotateVector(m_upVector));
dVector frontDir (matrix.RotateVector(m_frontVector));
dVector rightDir (frontDir * updir);
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
if ((verticalSpeed <= 0.0f) && (m_groundPlane % m_groundPlane) > 0.0f) {
// plane is supported by a ground plane, apply the player input velocity
if ((m_groundPlane % updir) >= m_maxSlope) {
// player is in a legal slope, he is in full control of his movement
dVector bodyVeloc;
NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
veloc = updir.Scale(bodyVeloc % updir) + gravity.Scale (timestep) + frontDir.Scale (forwardSpeed) + rightDir.Scale (lateralSpeed) + updir.Scale(verticalSpeed);
veloc += (m_groundVelocity - updir.Scale (updir % m_groundVelocity));
dFloat speedLimitMag2 = forwardSpeed * forwardSpeed + lateralSpeed * lateralSpeed + verticalSpeed * verticalSpeed + m_groundVelocity % m_groundVelocity + 0.1f;
dFloat speedMag2 = veloc % veloc;
if (speedMag2 > speedLimitMag2) {
veloc = veloc.Scale (dSqrt (speedLimitMag2 / speedMag2));
}
dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
if (normalVeloc < 0.0f) {
veloc -= m_groundPlane.Scale (normalVeloc);
}
} else {
// player is in an illegal ramp, he slides down hill an loses control of his movement
NewtonBodyGetVelocity(m_body, &veloc[0]);
veloc += updir.Scale(verticalSpeed);
veloc += gravity.Scale (timestep);
dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
if (normalVeloc < 0.0f) {
veloc -= m_groundPlane.Scale (normalVeloc);
}
}
} else {
// player is on free fall, only apply the gravity
NewtonBodyGetVelocity(m_body, &veloc[0]);
veloc += updir.Scale(verticalSpeed);
veloc += gravity.Scale (timestep);
}
return veloc;
}
void SetCamera()
{
if (m_player) {
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(GetWorld());
DemoCamera* const camera = scene->GetCamera();
dMatrix camMatrix(camera->GetNextMatrix());
DemoEntity* player = (DemoEntity*)NewtonBodyGetUserData(m_player->GetBody());
dMatrix playerMatrix(player->GetNextMatrix());
dFloat height = 2.0f;
dVector frontDir(camMatrix[0]);
dVector upDir(0.0f, 1.0f, 0.0f, 0.0f);
dVector camOrigin = playerMatrix.TransformVector(upDir.Scale(height));
camOrigin -= frontDir.Scale(PLAYER_THIRD_PERSON_VIEW_DIST);
camera->SetNextMatrix(*scene, camMatrix, camOrigin);
}
}
FrictionTrankThreaSuspention(const dMatrix& pinsAndPivoFrame, const NewtonBody* child, const NewtonBody* parent)
: CustomSlidingContact (pinsAndPivoFrame, child, parent)
{
dMatrix childMatrix;
dMatrix parentMatrix;
dVector maxPointFront;
dVector minPointFront;
NewtonCollision* collision;
collision = NewtonBodyGetCollision(child);
NewtonBodyGetMatrix(child, &childMatrix[0][0]);
NewtonBodyGetMatrix(parent, &parentMatrix[0][0]);
// find the the extreme front and rear point, this is used to calculate the position of the suspension points
dVector frontDir (childMatrix.UnrotateVector(parentMatrix.m_front));
NewtonCollisionSupportVertex(collision, &frontDir[0], &maxPointFront[0]);
dVector rearDir (frontDir.Scale (-1.0f));
NewtonCollisionSupportVertex(collision, &rearDir[0], &minPointFront[0]);
// calculate the front suspension points
dVector frontHardPoint (childMatrix.m_posit + childMatrix.RotateVector(frontDir.Scale (maxPointFront % frontDir)));
m_frontHarpointOnParent = parentMatrix.UntransformVector(frontHardPoint);
m_frontHarpointOnThread = childMatrix.UntransformVector(frontHardPoint);
// calculate the front rear suspension points
dVector rearHardPoint (childMatrix.m_posit + childMatrix.RotateVector(rearDir.Scale (minPointFront % rearDir)));
m_rearHarpointOnParent = parentMatrix.UntransformVector(rearHardPoint);
m_rearHarpointOnThread = childMatrix.UntransformVector(rearHardPoint);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass0;
dFloat mass1;
NewtonBodyGetMassMatrix(child, &mass0, &Ixx, &Iyy, &Izz);
NewtonBodyGetMassMatrix(parent, &mass1, &Ixx, &Iyy, &Izz);
m_massScale = (mass0 * mass1) / (mass0 + mass1);
}
void UpdateCamera (dFloat timestep)
{
if (m_player) {
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(GetWorld());
DemoCamera* const camera = scene->GetCamera();
dMatrix camMatrix (camera->GetNextMatrix ());
dMatrix playerMatrix (m_player->GetNextMatrix());
dVector frontDir (camMatrix[0]);
dVector camOrigin;
if (m_externalView) {
camOrigin = playerMatrix.m_posit + dVector(0.0f, VEHICLE_THIRD_PERSON_VIEW_HIGHT, 0.0f, 0.0f);
camOrigin -= frontDir.Scale (VEHICLE_THIRD_PERSON_VIEW_DIST);
} else {
dAssert (0);
// camMatrix = camMatrix * playerMatrix;
// camOrigin = playerMatrix.TransformVector(dVector(-0.8f, ARTICULATED_VEHICLE_CAMERA_EYEPOINT, 0.0f, 0.0f));
}
camera->SetNextMatrix (*scene, camMatrix, camOrigin);
}
}
void CustomLimitBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
matrix1 = m_rotationOffset * matrix1;
// handle special case of the joint being a hinge
if (m_coneAngleCos > 0.9999f) {
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
} else {
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngle = coneDir0 % coneDir1;
if (cosAngle <= m_coneAngleCos) {
dVector lateralDir(coneDir0 * coneDir1);
dFloat mag2 = lateralDir % lateralDir;
dAssert(mag2 > 1.0e-4f);
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
dQuaternion rot(m_coneAngleHalfCos, lateralDir.m_x * m_coneAngleHalfSin, lateralDir.m_y * m_coneAngleHalfSin, lateralDir.m_z * m_coneAngleHalfSin);
dVector frontDir(rot.UnrotateVector(coneDir1));
dVector upDir(lateralDir * frontDir);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(coneDir0, frontDir, lateralDir), &lateralDir[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
//handle twist angle
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
}
}
void dCustomPlayerController::ResolveCollision()
{
dMatrix matrix;
NewtonWorldConvexCastReturnInfo info[D_MAX_ROWS];
NewtonWorld* const world = m_manager->GetWorld();
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
int contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
if (!contactCount) {
return;
}
dFloat maxPenetration = 0.0f;
for (int i = 0; i < contactCount; i ++) {
maxPenetration = dMax (info[i].m_penetration, maxPenetration);
}
if (maxPenetration > D_MAX_COLLISION_PENETRATION) {
ResolveInterpenetrations(contactCount, info);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
}
int rowCount = 0;
dVector zero(0.0f);
dMatrix invInertia;
dVector com(0.0f);
dVector veloc(0.0f);
dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
dFloat rhs[D_MAX_ROWS];
dFloat low[D_MAX_ROWS];
dFloat high[D_MAX_ROWS];
dFloat impulseMag[D_MAX_ROWS];
int normalIndex[D_MAX_ROWS];
NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]);
// const dMatrix localFrame (dPitchMatrix(m_headingAngle) * m_localFrame * matrix);
const dMatrix localFrame (m_localFrame * matrix);
com = matrix.TransformVector(com);
com.m_w = 0.0f;
for (int i = 0; i < contactCount; i ++) {
NewtonWorldConvexCastReturnInfo& contact = info[i];
dVector point (contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
dVector normal (contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);
jt[rowCount].m_linear = normal;
jt[rowCount].m_angular = (point - com).CrossProduct(normal);
low[rowCount] = 0.0f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
dVector tmp (veloc * jt[rowCount].m_linear.Scale (1.001f));
rhs[rowCount] = - (tmp.m_x + tmp.m_y + tmp.m_z);
rowCount ++;
dAssert (rowCount < (D_MAX_ROWS - 3));
//dFloat updir = localFrame.m_front.DotProduct3(normal);
dFloat friction = m_manager->ContactFriction(this, point, normal, contact.m_hitBody);
if (friction > 0.0f)
{
// add lateral traction friction
dVector sideDir (localFrame.m_up.CrossProduct(normal).Normalize());
jt[rowCount].m_linear = sideDir;
jt[rowCount].m_angular = (point - com).CrossProduct(sideDir);
low[rowCount] = -friction;
high[rowCount] = friction;
normalIndex[rowCount] = -1;
dVector tmp1 (veloc * jt[rowCount].m_linear);
rhs[rowCount] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);
rowCount++;
dAssert (rowCount < (D_MAX_ROWS - 3));
// add longitudinal traction friction
dVector frontDir (normal.CrossProduct(sideDir));
jt[rowCount].m_linear = frontDir;
jt[rowCount].m_angular = (point - com).CrossProduct(frontDir);
low[rowCount] = -friction;
high[rowCount] = friction;
normalIndex[rowCount] = -2;
dVector tmp2 (veloc * jt[rowCount].m_linear);
rhs[rowCount] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
rowCount++;
dAssert(rowCount < (D_MAX_ROWS - 3));
}
}
for (int i = 0; i < 3; i++) {
jt[rowCount].m_linear = zero;
jt[rowCount].m_angular = zero;
jt[rowCount].m_angular[i] = dFloat(1.0f);
rhs[rowCount] = 0.0f;
impulseMag[rowCount] = 0;
low[rowCount] = -1.0e12f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
rowCount ++;
dAssert (rowCount < D_MAX_ROWS);
}
dVector impulse (veloc.Scale (m_mass) + CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
veloc = impulse.Scale(m_invMass);
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
}
