void dComplentaritySolver::dBodyState::IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque)
{
dVector accel (force.Scale (m_invMass));
dVector alpha (m_invInertia.RotateVector(torque));
m_veloc += accel.Scale (timestep);
m_omega += alpha.Scale (timestep);
}
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 InitCamera (const dVector& eyePoint, const dVector& dir)
{
gCameraEyepoint = eyePoint;
gCurrCameraDir = dir.Scale (1.0f / sqrt (dir % dir));
gRollAngle = dAsin (gCurrCameraDir.m_y);
gPrevRollAngle = gYawAngle;
gYawAngle = dAtan2 (-gCurrCameraDir.m_z, gCurrCameraDir.m_x);
gPrevYawAngle = gYawAngle;
}
dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
{
// this is correct
dQuaternion rotation (*this);
dFloat omegaMag2 = omega % omega;
const dFloat errAngle = 0.0125f * 3.141592f / 180.0f;
const dFloat errAngle2 = errAngle * errAngle;
if (omegaMag2 > errAngle2) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion deltaRotation (omegaAxis, omegaAngle);
rotation = rotation * deltaRotation;
rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
}
return rotation;
}
static void PhysicsApplyPickForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dVector com;
dVector veloc;
dVector omega;
dMatrix matrix;
// apply the thew body forces
if (chainForceCallback) {
chainForceCallback (body, timestep, threadIndex);
}
// add the mouse pick penalty force and torque
NewtonBodyGetVelocity(body, &veloc[0]);
NewtonBodyGetOmega(body, &omega[0]);
NewtonBodyGetVelocity(body, &veloc[0]);
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector force (pickedForce.Scale (mass * MOUSE_PICK_STIFFNESS));
dVector dampForce (veloc.Scale (MOUSE_PICK_DAMP * mass));
force -= dampForce;
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetCentreOfMass (body, &com[0]);
// calculate local point relative to center of mass
dVector point (matrix.RotateVector (attachmentPoint - com));
dVector torque (point * force);
dVector torqueDamp (omega.Scale (mass * 0.1f));
NewtonBodyAddForce (body, &force.m_x);
NewtonBodyAddTorque (body, &torque.m_x);
// make sure the body is unfrozen, if it is picked
NewtonBodySetFreezeState (body, 0);
}
void CustomPathFollow::SetPathTarget (const dVector& posit, const dVector& tangent)
{
m_pointOnPath = posit;
m_pathTangent = tangent.Scale (1.0f / dSqrt (m_pathTangent % m_pathTangent));
}
// This function read KeyBoard and Mouse control to control this scene
// The control are read at the simulation rate, and two states are kept
void InputControl(NewtonWorld* world)
{
// read the mouse position and set the camera direction
static dVector mouse0 (GetMousePos());
dVector mouse1 (GetMousePos());
gPrevYawAngle = gYawAngle;
gPrevRollAngle = gRollAngle;
if (!MousePick (world, mouse1)) {
int leftKetDown;
leftKetDown = IsKeyDown (KeyCode_L_BUTTON);
// we are not in mouse pick mode, then we are in camera tracking mode
if (leftKetDown) {
// when click left mouse button the first time, we reset the camera
// convert the mouse x position to delta yaw angle
if (mouse1.m_x > (mouse0.m_x + 1)) {
gYawAngle += 1.0f * 3.1416f / 180.0f;
if (gYawAngle > (360.0f * 3.1416f / 180.0f)) {
gYawAngle -= (360.0f * 3.1416f / 180.0f);
}
} else if (mouse1.m_x < (mouse0.m_x - 1)) {
gYawAngle -= 1.0f * 3.1416f / 180.0f;
if (gYawAngle < 0.0f) {
gYawAngle += (360.0f * 3.1416f / 180.0f);
}
}
if (mouse1.m_y > (mouse0.m_y + 1)) {
gRollAngle += 1.0f * 3.1416f / 180.0f;
if (gRollAngle > (80.0f * 3.1416f / 180.0f)) {
gRollAngle = 80.0f * 3.1416f / 180.0f;
}
} else if (mouse1.m_y < (mouse0.m_y - 1)) {
gRollAngle -= 1.0f * 3.1416f / 180.0f;
if (gRollAngle < -(80.0f * 3.1416f / 180.0f)) {
gRollAngle = -80.0f * 3.1416f / 180.0f;
}
}
dMatrix cameraDirMat (dRollMatrix(gRollAngle) * dYawMatrix(gYawAngle));
gCurrCameraDir = cameraDirMat.m_front;
}
}
// save mouse position and left mouse key state for next frame
mouse0 = mouse1;
// camera control
gPrevCameraEyepoint = gCameraEyepoint;
if (IsKeyDown ('W')) {
gCameraEyepoint += gCurrCameraDir.Scale (CAMERA_SPEED / 60.0f);
} else if (IsKeyDown ('S')) {
gCameraEyepoint -= gCurrCameraDir.Scale (CAMERA_SPEED / 60.0f);
}
if (IsKeyDown ('D')) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (gCurrCameraDir * up);
gCameraEyepoint += right.Scale (CAMERA_SPEED / 60.0f);
} else if (IsKeyDown ('A')) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (gCurrCameraDir * up);
gCameraEyepoint -= right.Scale (CAMERA_SPEED / 60.0f);
}
}
bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
witdh;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
// save point local to th body matrix
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// save the transform call back
chainForceCallback = NewtonBodyGetForceAndTorqueCallback (pickedBody);
dAssert (chainForceCallback != PhysicsApplyPickForce);
// set a new call back
NewtonBodySetForceAndTorqueCallback (pickedBody, PhysicsApplyPickForce);
}
}
if (mousePickMode) {
// init pick mode
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p2 (matrix.TransformVector (attachmentPoint));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
pickedForce = p - p2;
dFloat mag2 = pickedForce % pickedForce;
if (mag2 > dFloat (20 * 20)) {
pickedForce = pickedForce.Scale (20.0f / dSqrt (pickedForce % pickedForce));
}
// rotate normal to global space
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
// rayWorldOrigin = p2;
// show the pick points
//ShowMousePicking (p, p2, witdh);
ShowMousePicking (p, p2);
//ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length));
}
} else {
mousePickMode = false;
if (pickedBody) {
//dAssert (chainForceCallback != NewtonBodyGetForceAndTorqueCallback (pickedBody));
dAssert (chainForceCallback != PhysicsApplyPickForce);
NewtonBodySetForceAndTorqueCallback (pickedBody, chainForceCallback);
pickedBody = NULL;
chainForceCallback = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
static NewtonUserJoint* bodyPickController;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// Create PickBody Joint
NewtonBodyGetMassMatrix (pickedBody, &mass, &Ixx, &Iyy, &Izz);
if (mass) {
// bodyPickController = new CustomPickBody (pickedBody, p);
// bodyPickController->SetMaxLinearFriction (MAX_PICK_ACCEL);
// bodyPickController->SetMaxAngularFriction (MAX_PICK_ACCEL * 5.0f);
bodyPickController = CreateCustomKinematicController (pickedBody, &p[0]);
CustomKinematicControllerSetMaxLinearFriction (bodyPickController, MAX_PICK_ACCEL);
CustomKinematicControllerSetMaxAngularFriction (bodyPickController, MAX_PICK_ACCEL * 5.0f);
}
}
}
if (mousePickMode) {
// init pick mode
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
if (bodyPickController) {
//bodyPickController->SetTargetPosit (p);
CustomKinematicControllerSetTargetPosit (bodyPickController, &p[0]);
}
// rotate normal to global space
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
dVector p2 (matrix.TransformVector (attachmentPoint));
ShowMousePicking (p, p2, witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
}
} else {
mousePickMode = false;
if (pickedBody) {
if (bodyPickController) {
//delete bodyPickController;
CustomDestroyJoint (bodyPickController);
}
bodyPickController = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
// Keyboard handler.
void Keyboard()
{
// check for termination
if (dGetKeyState (VK_ESCAPE) & 0x8000) {
exit(0);
}
// read the mouse position and set the camera direction
static MOUSE_POINT mouse0;
static dFloat yawAngle = 90.0f * 3.1416 / 180.0f;
static dFloat rollAngle = 0.0f;
MOUSE_POINT mouse1;
GetCursorPos(mouse1);
if (dGetKeyState (VK_LBUTTON) & 0x8000) {
if (mouse1.x > (mouse0.x + 1)) {
yawAngle += 1.0f * 3.1416 / 180.0f;
if (yawAngle > (360.0f * 3.1416 / 180.0f)) {
yawAngle -= (360.0f * 3.1416 / 180.0f);
}
} else if (mouse1.x < (mouse0.x - 1)) {
yawAngle -= 1.0f * 3.1416 / 180.0f;
if (yawAngle < 0.0f) {
yawAngle += (360.0f * 3.1416 / 180.0f);
}
}
if (mouse1.y > (mouse0.y + 1)) {
rollAngle += 1.0f * 3.1416 / 180.0f;
if (rollAngle > (80.0f * 3.1416 / 180.0f)) {
rollAngle = 80.0f * 3.1416 / 180.0f;
}
} else if (mouse1.y < (mouse0.y - 1)) {
rollAngle -= 1.0f * 3.1416 / 180.0f;
if (rollAngle < -(80.0f * 3.1416 / 180.0f)) {
rollAngle = -80.0f * 3.1416 / 180.0f;
}
}
dMatrix cameraDirMat (dgRollMatrix(rollAngle) * dgYawMatrix(yawAngle));
cameraDir = cameraDirMat.m_front;
}
mouse0 = mouse1;
// camera control
if (dGetKeyState ('W') & 0x8000) {
cameraEyepoint += cameraDir.Scale (CAMERA_SPEED / 60.0f);
} else if (dGetKeyState ('S') & 0x8000) {
cameraEyepoint -= cameraDir.Scale (CAMERA_SPEED / 60.0f);
}
if (dGetKeyState ('D') & 0x8000) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (cameraDir * up);
cameraEyepoint += right.Scale (CAMERA_SPEED / 60.0f);
} else if (dGetKeyState ('A') & 0x8000) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (cameraDir * up);
cameraEyepoint -= right.Scale (CAMERA_SPEED / 60.0f);
}
}