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); } }