Esempio n. 1
0
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;
}
Esempio n. 4
0
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;
}
Esempio n. 5
0
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);
}
Esempio n. 6
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);
	}
} 
Esempio n. 8
0
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;
}
Esempio n. 9
0
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;
}
Esempio n. 10
0
//	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);
	}
}