Exemple #1
0
//////////////////////////////////////////////////////////////////////////
// Position the body precisely
//////////////////////////////////////////////////////////////////////////
void DropDownBox(NewtonBody *body)
{
	float matrix[16];
	NewtonBodyGetMatrix(body, matrix);
	Matrix4 m = Matrix4(matrix);

	Vector3 pos = m.GetPosition();
	pos.y += 1.0f;
	m.SetPosition(pos);
	Vector3 p = pos;
	p.y -= 20;

	// cast collision shape within +20 -20 y range
	NewtonWorld *world = NewtonBodyGetWorld(body);
	NewtonCollision *collision = NewtonBodyGetCollision(body);

	float param;
	NewtonWorldConvexCastReturnInfo info[16];
	m.FlattenToArray(matrix);
	NewtonWorldConvexCast(world, matrix, &p[0], collision, &param, body, DropDownConvexCastCallback, info, 16, 0);

	m = Matrix4(matrix);
	pos = m.GetPosition();
	m.SetPosition(pos + Vector3(0, (p.y - pos.y) * param, 0) );

	m.FlattenToArray(matrix);
	NewtonBodySetMatrix(body, matrix);
}
    virtual void PreUpdate (dFloat timestep)
    {
        (void)timestep;

        NewtonWorld* const world = GetWorld();
        DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
        NewtonDemos* const mainWindow = scene->GetRootWindow();
        DemoCamera* const camera = scene->GetCamera();

        m_helpKey.UpdatePushButton (mainWindow, 'H');
        if (m_selectShape.UpdateTriggerButton (mainWindow, ' ')) {
            m_stupidLevel->ChangeCastingShape();
        }

        bool buttonState = mainWindow->GetMouseKeyState(1);
        if (buttonState) {
            int mouseX;
            int mouseY;

            buttonState = false;
            mainWindow->GetMousePosition (mouseX, mouseY);

            float x = dFloat (mouseX);
            float y = dFloat (mouseY);
            dVector p0 (camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
            dVector p1 (camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));

            //p0 = dVector (-11.531384, 6.897866, -2.453451, 1.0f);
            //p1 = dVector (1720.189697, -1245.047241, 58.625248, 1.0f);

            // do the convex cast here
            dMatrix matrix (dGetIdentityMatrix());
            matrix.m_posit = p0;


            dFloat param = 1.2f;
            NewtonCollision* const shape = m_stupidLevel->GetCurrentShape();
            //int count = NewtonWorldConvexCast (world, &matrix[0][0], &p1[0], shape, ConvexCastCallBack::Filter, &filter, ConvexCastCallBack::Prefilter, &filter.m_contacts[0], 4, 0);
            NewtonWorldConvexCast (world, &matrix[0][0], &p1[0], shape, &param, NULL, Prefilter, NULL, 0, 0);

            //dTrace(("%f, %f, %f\n", p0[0], p0[1], p0[2]));
            //dTrace(("%f, %f, %f\n", p1[0], p1[1], p1[2]));

            if (param < 1.0f) {
                matrix.m_posit += (p1 - matrix.m_posit).Scale (param);
                m_stupidLevel->SetCastEntityMatrix (scene, matrix);
            }
            m_stupidLevel->SetCastingLine (p0, p1);
        }
    }
	void Convexcast::go(const OgreNewt::World* world, const OgreNewt::ConvexCollisionPtr& col, const Ogre::Vector3& startpt, const Ogre::Quaternion &colori, const Ogre::Vector3& endpt, int maxcontactscount, int threadIndex)
	{

		if( world->getDebugger().isRaycastRecording() )
		{
			world->getDebugger().addConvexRay(col, startpt, colori, endpt);
		}
		// reserve memory
		if( mReturnInfoListSize < maxcontactscount )
		{
			mReturnInfoListSize = 0;
			if( mReturnInfoList )
				delete[] mReturnInfoList;
			mReturnInfoList = new NewtonWorldConvexCastReturnInfo[maxcontactscount];
			mReturnInfoListSize = maxcontactscount;
		}

		memset(mReturnInfoList, 0, sizeof(mReturnInfoList[0])*mReturnInfoListSize);
		// perform the cast
		float matrix[16];
		OgreNewt::Converters::QuatPosToMatrix(colori, startpt, &matrix[0] );
		mFirstContactDistance = -1;


		mReturnInfoListLength = 
			NewtonWorldConvexCast( world->getNewtonWorld(), &matrix[0], (float*)&endpt, col->getNewtonCollision(),
			&mFirstContactDistance, this, OgreNewt::Convexcast::newtonConvexcastPreFilter,
			mReturnInfoList, mReturnInfoListSize, threadIndex);


		if( world->getDebugger().isRaycastRecording() && world->getDebugger().isRaycastRecordingHitBodies() )
		{
			Body* body;
			for(int i = 0; i < mReturnInfoListLength; i++)
			{
				body = (OgreNewt::Body*) NewtonBodyGetUserData(mReturnInfoList[i].m_hitBody);
				world->getDebugger().addHitBody(body);
			}
		}
	}
void CustomPlayerController::UpdateGroundPlane (dMatrix& matrix, const dMatrix& castMatrix, const dVector& dst, int threadIndex)
{
	CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
	NewtonWorld* const world = manager->GetWorld();
	NewtonWorldConvexCastReturnInfo info;
	CustomControllerConvexRayFilter filter(m_body);

	dFloat param = 10.0f;
	int count = NewtonWorldConvexCast (world, &castMatrix[0][0], &dst[0], m_castingShape, &param, &filter, CustomControllerConvexCastPreFilter::Prefilter, &info, 1, threadIndex);

	m_groundPlane = dVector (0.0f);
	m_groundVelocity = dVector (0.0f);

	if (count && (param <= 1.0f)) {
		m_isJumping = false;
		dVector supportPoint (castMatrix.m_posit + (dst - castMatrix.m_posit).Scale (param));
		m_groundPlane = dVector (info.m_normal[0], info.m_normal[1], info.m_normal[2], 0.0f);
		m_groundPlane.m_w = - supportPoint.DotProduct3(m_groundPlane);
		NewtonBodyGetPointVelocity (info.m_hitBody, &supportPoint.m_x, &m_groundVelocity[0]);
		matrix.m_posit = supportPoint;
		matrix.m_posit.m_w = 1.0f;
	}
}
void ConvexCastPlacement (NewtonBody* body)
{
	dFloat param;
	dMatrix matrix;
	NewtonWorld* world;
	NewtonCollision* collision;
	NewtonWorldConvexCastReturnInfo info[16];


	NewtonBodyGetMatrix (body, &matrix[0][0]);

	matrix.m_posit.m_y += 40.0f;
	dVector p (matrix.m_posit);
	p.m_y -= 80.0f;

	world = NewtonBodyGetWorld(body);
	collision = NewtonBodyGetCollision(body);
	NewtonWorldConvexCast (world, &matrix[0][0], &p[0], collision, &param, body, ConvexCastCallback, info, 16, 0);
	dAssert (param < 1.0f);

	matrix.m_posit.m_y += (p.m_y - matrix.m_posit.m_y) * param;

	NewtonBodySetMatrix(body, &matrix[0][0]);
}
void CustomDGRayCastCar::CalculateTireCollision (Tire& tire, const dMatrix& suspensionMatrix, int threadIndex) const
{
	int floorcontact = 0;
	tire.m_HitBody = NULL;

	tire.m_posit = tire.m_suspensionLenght;
	if ( tire.m_tireUseConvexCastMode ) {
		dFloat hitParam;
		NewtonWorldConvexCastReturnInfo info;

//		tire.m_rayDestination = tire.m_suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght ));   
		dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght)));   
		if ( NewtonWorldConvexCast ( m_world, &suspensionMatrix[0][0], &rayDestination[0], tire.m_shape, &hitParam, (void*)m_body0, ConvexCastPrefilter, &info, 1, threadIndex ) ){
			tire.m_posit = hitParam * tire.m_suspensionLenght;
			tire.m_contactPoint = info.m_point;
			tire.m_contactNormal = info.m_normal;
			tire.m_HitBody = (NewtonBody*)info.m_hitBody; 
			floorcontact = 1;
		}
	} else {
		struct RayCastInfo
		{
			RayCastInfo (const NewtonBody* body)
			{
				m_param = 1.0f;
				m_me = body;
				m_hitBody = NULL;
				m_contactID = 0;
				m_normal = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			}
			static dFloat RayCast (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
			{
				RayCastInfo& caster = *( (RayCastInfo*) userData ); 
				// if this body is not the vehicle, see if a close hit
				if ( body != caster.m_me ) {
					if ( intersetParam < caster.m_param) {
						// this is a close hit, record the information. 
						caster.m_param = intersetParam;
						caster.m_hitBody = body;
						caster.m_contactID = collisionID;
						caster.m_normal = dVector (normal[0], normal[1], normal[2], 1.0f);
					} 
				}
				return intersetParam;
			}
			dFloat m_param;
			dVector m_normal;
			const NewtonBody* m_me;
			const NewtonBody* m_hitBody;
			int m_contactID;
		};
		RayCastInfo info (m_body0);

_ASSERTE (0);
		// extend the ray by the radius of the tire
		dFloat dist ( tire.m_suspensionLenght + tire.m_radius );
		dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -dist )));	
		// cast a ray to the world ConvexCastPrefilter
		NewtonWorldRayCast( m_world, &suspensionMatrix.m_posit[0], &rayDestination[0], RayCastInfo::RayCast, &info, &ConvexCastPrefilter );
		// if the ray hit something, it means the tire has some traction
		if ( info.m_hitBody ) {
			dFloat intesectionDist;
			tire.m_HitBody = (NewtonBody*)info.m_hitBody; 
			tire.m_contactPoint = suspensionMatrix.m_posit + (rayDestination - suspensionMatrix.m_posit ).Scale ( info.m_param ); 
			tire.m_contactNormal = info.m_normal;  

			// TO DO: get the material properties for tire frictions on different roads 
			intesectionDist = ( dist * info.m_param - tire.m_radius );
			if ( intesectionDist < 0.0f ) {
				intesectionDist = 0.0f;
			} else if ( intesectionDist > tire.m_suspensionLenght ) {
				intesectionDist = tire.m_suspensionLenght;
			}
			tire.m_posit = intesectionDist;
		}
	}
}
		void tPlayerController::_upateState(float timeStep)
		{
			Vec3 nextStep = Vec3::Zero;

			if (mState == eOnLand)
			{
				nextStep += mForwordVelocity * mNode->GetDirection();
				nextStep += mSideVelocity * mNode->GetRight();
			}
			else if (mState == eOnFall)
			{
				nextStep += Vec3::UnitY * mGravity;
			}
			else if (mState == eOnJump)
			{
				nextStep += mForwordVelocity * mNode->GetDirection();
				nextStep += mSideVelocity * mNode->GetRight();
				nextStep += mInternalJumpVel * Vec3::UnitY;
			}

			nextStep *= timeStep;

			const Vec3 & worldPos = mNode->GetPosition();
			NewtonWorld * ntWorld = tWorld::Instance()->_getNewtonWorld();

			NewtonWorldConvexCastReturnInfo info[_NT_MAX_CONTACTS];

			Mat4 worldTm;

			worldTm.MakeTranslate(worldPos.x, worldPos.y, worldPos.z);
			 
			Vec3 dest = worldPos + nextStep;
			float hitParam;

			// check on land
			if (mState == eOnJump)
			{
				int contacts = NewtonWorldConvexCast(ntWorld, worldTm[0], &dest.x, mShape->_getNewtonCollision(), &hitParam, this,
													 _convexCastBodyFilter, info, _NT_MAX_CONTACTS, 0);

				if (contacts)
				{
					float bestValue = info[0].m_point[1];
					float lowestValue = info[0].m_point[1];

					for (int i = 1; i < contacts; i ++)
					{
						float value = info[i].m_point[1];

						if (value > bestValue)
							bestValue = value;

						if (value < lowestValue)
							lowestValue = value;
					}

					if (mInternalJumpVel >= 0 && bestValue > mPlayerHeight * mJumpHighLimit)
						mInternalJumpVel = 0;

					/*if (mInternalJumpVel <= 0 && lowestValue < mPlayerHeight * mLandLimit)
					{
						mState = eOnLand;
					}
					*/

					Vec3 Normal, Position;
					if (mInternalJumpVel <= 0 && _checkOnLand(dest, &Normal, &Position))
					{
						mNode->SetPosition(Position);
						mState = eOnLand;
					}
				}

				mNode->SetPosition(dest);

				return ;
			}
			
			Vec3 Normal, Position;
			if (_checkOnLand(dest, &Normal, &Position))
			{
				int contacts = NewtonWorldConvexCast(ntWorld, worldTm[0], &dest.x, mShape->_getNewtonCollision(), &hitParam, this,
													 _convexCastBodyFilter, info, _NT_MAX_CONTACTS, 0);

				float bestValue = info[0].m_point[1];
				float lowestValue = info[0].m_point[1];

				for (int i = 1; i < contacts; i ++)
				{
					float value = info[i].m_point[1];

					if (value > bestValue)
						bestValue = value;

					if (value < lowestValue)
						lowestValue = value;
				}

				if (bestValue < dest.y + mPlayerHeight * mLandLimit)
					mState = eOnLand;
				else
					mState = eOnIlligalRamp;

				/*if (Normal.y < mMaxSlopeCos)
				{
				mState = eOnIlligalRamp;
				return ;
				}*/

				if (mState == eOnLand)
				{
					mNode->SetPosition(Position);
				}

				return ;
			}

			mNode->SetPosition(dest);
			
			mState = eOnFall;
		}
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex)
{
	dMatrix matrix; 
	dQuaternion bodyRotation;
	dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); 
	dVector omega(0.0f, 0.0f, 0.0f, 0.0f);  

	CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
	NewtonWorld* const world = manager->GetWorld();

	// apply the player motion, by calculation the desired plane linear and angular velocity
	manager->ApplyPlayerMove (this, timestep);

	// get the body motion state 
	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	NewtonBodyGetVelocity(m_body, &veloc[0]);
	NewtonBodyGetOmega(m_body, &omega[0]);

	// integrate body angular velocity
	NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); 
	bodyRotation = bodyRotation.IntegrateOmega(omega, timestep);
	matrix = dMatrix (bodyRotation, matrix.m_posit);

	// integrate linear velocity
	dFloat normalizedTimeLeft = 1.0f; 
	dFloat step = timestep * dSqrt (veloc % veloc) ;
	dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS);
	int prevContactCount = 0;
	CustomControllerConvexCastPreFilter castFilterData (m_body);
	NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS];

	dVector updir (matrix.RotateVector(m_upVector));

	dVector scale;
	NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z);
	//const dFloat radio = m_outerRadio * 4.0f;
	const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f;
	NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio);


	NewtonWorldConvexCastReturnInfo upConstratint;
	memset (&upConstratint, 0, sizeof (upConstratint));
	upConstratint.m_normal[0] = m_upVector.m_x;
	upConstratint.m_normal[1] = m_upVector.m_y;
	upConstratint.m_normal[2] = m_upVector.m_z;
	upConstratint.m_normal[3] = m_upVector.m_w;

	for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) {
		if ((veloc % veloc) < 1.0e-6f) {
			break;
		}

		dFloat timetoImpact;
		NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS];
		dVector destPosit (matrix.m_posit + veloc.Scale (timestep));
		int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
		if (contactCount) {
			contactCount = manager->ProcessContacts (this, info, contactCount);
		}

		if (contactCount) {
			matrix.m_posit += veloc.Scale (timetoImpact * timestep);
			if (timetoImpact > 0.0f) {
				matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; 
			}

			normalizedTimeLeft -= timetoImpact;

			dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
			dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
			dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2];

			for (int i = 1; i < contactCount; i ++) {
				dVector n0 (info[i-1].m_normal);
				for (int j = 0; j < i; j ++) {
					dVector n1 (info[j].m_normal);
					if ((n0 % n1) > 0.9999f) {
						info[i] = info[contactCount - 1];
						i --;
						contactCount --;
						break;
					}
				}
			}

			int count = 0;
			if (!m_isJumping) {
				upConstratint.m_point[0] = matrix.m_posit.m_x;
				upConstratint.m_point[1] = matrix.m_posit.m_y;
				upConstratint.m_point[2] = matrix.m_posit.m_z;
				upConstratint.m_point[3] = matrix.m_posit.m_w;

				speed[count] = 0.0f;
				bounceNormal[count] = dVector (upConstratint.m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint);
				count ++;
			}

			for (int i = 0; i < contactCount; i ++) {
				speed[count] = 0.0f;
				bounceNormal[count] = dVector (info[i].m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]);
				count ++;
			}

			for (int i = 0; i < prevContactCount; i ++) {
				speed[count] = 0.0f;
				bounceNormal[count] = dVector (prevInfo[i].m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]);
				count ++;
			}

			dFloat residual = 10.0f;
			dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);
			for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) {
				residual = 0.0f;
				for (int k = 0; k < count; k ++) {
					dVector normal (bounceNormal[k]);
					dFloat v = bounceSpeed[k] - normal % auxBounceVeloc;
					dFloat x = speed[k] + v;
					if (x < 0.0f) {
						v = 0.0f;
						x = 0.0f;
					}

					if (dAbs (v) > residual) {
						residual = dAbs (v);
					}

					auxBounceVeloc += normal.Scale (x - speed[k]);
					speed[k] = x;
				}
			}

			dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f);
			for (int i = 0; i < count; i ++) {
				dVector normal (bounceNormal[i]);
				velocStep += normal.Scale (speed[i]);
			}
			veloc += velocStep;

			dFloat velocMag2 = velocStep % velocStep;
			if (velocMag2 < 1.0e-6f) {
				dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep);
				matrix.m_posit += veloc.Scale (advanceTime);
				normalizedTimeLeft -= advanceTime / timestep;
			}

			prevContactCount = contactCount;
			memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));

		} else {
			matrix.m_posit = destPosit;
			matrix.m_posit.m_w = 1.0f;
			break;
		}
	}
	NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z);

	// determine if player is standing on some plane
	dMatrix supportMatrix (matrix);
	supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin);
	if (m_isJumping) {
		dVector dst (matrix.m_posit);
		UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
	} else {
		step = dAbs (updir % veloc.Scale (timestep));
		dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step;
		dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f));
		UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
	}

	// set player velocity, position and orientation
	NewtonBodySetVelocity(m_body, &veloc[0]);
	NewtonBodySetMatrix (m_body, &matrix[0][0]);
}