CarPhysics.cpp

void ACar::update(const GameUpdateSets& updateSets)
{
	if(updateSets.isGamePaused())
		return;

	m_traitCamera.update(this, updateSets);
	m_traitCamera.rotateWorkingDirectionTowardsFlat((-getTransformMtx().data[0].xyz().normalized0() + 2.5f*vec3f::getAxis(1)).normalized(), updateSets.dt);
	//m_traitCamera.rotateWorkingDirectionTowardsFlat((-getTransformMtx().data[0].xyz().normalized0() + 0.5f*vec3f::getAxis(1)).normalized(), updateSets.dt);

	struct Tyre
	{
		Tyre(vec3f localOffset, float radius, bool isSteering, bool isHandbrakeable)
			: localOffset(localOffset)
			, radius(radius)
			, isSteering(isSteering)
			, isHandbrakeable(isHandbrakeable)
		{}

		// Parameters.
		vec3f localOffset;
		float radius;
		bool isSteering;
		bool isHandbrakeable;

		// Per-frame parameters:
		bool hasContact = false;
		vec3f hitNormalWS = vec3f(0.f, 1.f, 0.f);
		vec3f steeringDir = vec3f::getAxis(0);
		vec3f forceToApply = vec3f(0.f);
		vec3f centralTorque = vec3f(0.f);
		btVector3 forcePos;
	};

	Tyre tyres[4] =
	{
		// Front (left, right)
		Tyre(vec3f(0.65f, 0.2f, -0.70f), 0.7f, true, false),
		Tyre(vec3f(0.65f, 0.2f, 0.70f), 0.7f, true, false),

		// Back (left, right)
		Tyre(vec3f(-0.85f, 0.2f, -0.70f), 0.7f, false, true),
		Tyre(vec3f(-0.85f, 0.2f, 0.70f), 0.7f, false, true),
	};

	vec2f  sterringInput = updateSets.is.GetArrowKeysDir(false, true);
	bool isBreakBtnDown = updateSets.is.IsKeyDown(Key::Key_Space);
	bool isNitroBtnDown = updateSets.is.IsKeyDown(Key::Key_LShift);
	bool isRandomForceBtnDown = updateSets.is.IsKeyReleased(Key::Key_C);
	bool isJumpBtnDown = updateSets.is.IsKeyDown(Key::Key_V);
	bool isJumpBtnReleased = updateSets.is.IsKeyReleased(Key::Key_V);

	if(updateSets.is.getXInputDevice(0).hooked)
	{
		if(sterringInput.lengthSqr() < 1e-2f)
		{
			sterringInput.x = updateSets.is.getXInputDevice(0).getInputDir(true).x;
		}

		if(updateSets.is.getXInputDevice(0).btnX & 0x1) {
			sterringInput.y = +1;
		}

		if(updateSets.is.getXInputDevice(0).getInputDir(true).y < -1e-3f){
			sterringInput.y = updateSets.is.getXInputDevice(0).getInputDir(true).y;
		}

		isJumpBtnDown |= (updateSets.is.getXInputDevice(0).btnA & 0x3) != 0;
		isBreakBtnDown |= (updateSets.is.getXInputDevice(0).btnB & 0x3) == 1;
		isNitroBtnDown |= (updateSets.is.getXInputDevice(0).btnShoulderL & 0x3) == 1;

		isJumpBtnReleased |= (updateSets.is.getXInputDevice(0).btnA & 0x3) == 2;
	}

	float const antiFlip = 0.4f; // [0; 1]
	float const nitroAccelMult = isNitroBtnDown ? 2.f : 1.f;
	float const nitroTopSpeedMult = isNitroBtnDown ? 1.2f : 1.f;
	float const topSpeed = 60.f;

	m_targetSteering = -sterringInput.x;
	if(sterringInput.x == 0) m_workingSteering = 0.f;
	else m_workingSteering = m_workingSteering + (m_targetSteering - m_workingSteering)*updateSets.dt * 8.f;
	//m_workingSteering = m_targetSteering;


	const mat4f worldTransform = getTransformMtx();
	btRigidBody* rb = m_traitRB.getRigidBody()->getBulletRigidBody();
	const btTransform worldPhys = rb->getWorldTransform();

	//
	auto& grp = getCore()->getDebugDraw2().getGroup(getDisplayName().c_str());
	grp.clear(false);

	auto velocityAtPointWS = [&](vec3f ptWS) -> vec3f {
		vec3f linVel = fromBullet(rb->getLinearVelocity());
		vec3f angVel = fromBullet(rb->getAngularVelocity());
		vec3f result = linVel + angVel.cross(ptWS - fromBullet(rb->getCenterOfMassPosition()));
		return result;
	};

	vec3f const localUp = worldTransform.data[1].xyz().normalized();

	if(isRandomForceBtnDown)
	{
		vec3f pos((rand() % 100) / 100.f, (rand() % 100) / 100.f, (rand() % 100) / 100.f);
		pos = pos * 2.f - vec3f(1.f);
		rb->applyImpulse(toBullet(vec3f::getAxis(1) * 10.f), toBullet(pos));
	}

	const float jumpImpulsePowerMin = 5.f;
	const float jumpImpulsePowerMax = 30.f;
	const float jumpFullChargeTime = 0.5f;

	if(isJumpBtnDown) {
		m_jumpAnticipationTime += updateSets.dt;
	}
	
	if(isJumpBtnReleased)
	{
		float k =  clamp01(m_jumpAnticipationTime / jumpFullChargeTime);
		float power = lerp(jumpImpulsePowerMin, jumpImpulsePowerMax, k);
		rb->applyCentralImpulse(btVector3(0.f, power, 0.f));
		m_jumpAnticipationTime = 0.f;
	}

	if(!isJumpBtnDown) {
		m_jumpAnticipationTime = 0;
	}

	//grp.getWiered().line(worldTransform.data[3].xyz(), worldTransform.data[3].xyz() + fromBullet(rb->getLinearVelocity()), 0xffffffff);

	// Performe the raycast and apply comute the force that is going to be applied.
	RayResultActor rayResults[SGE_ARRSZ(tyres)];
	for(int t = 0; t < SGE_ARRSZ(tyres); ++t)
	{
		tyres[t].forceToApply = vec3f(0.f);
		vec3f const rayPosWS = mat_mul_pos(worldTransform, tyres[t].localOffset);
		vec3f const rayTarWS = rayPosWS - localUp * tyres[t].radius;

		btVector3 const rayPosBulletWS = toBullet(rayPosWS);
		btVector3 const rayTarBulletWS = toBullet(rayTarWS);

		rayResults[t].setup(this, rayPosBulletWS, rayTarBulletWS);
		getWorld()->physicsWorld.dynamicsWorld->rayTest(rayPosBulletWS, rayTarBulletWS, rayResults[t]);

		// No hit, so no force.
		if(rayResults[t].hasHit() == false) {
			continue;
		}

		btVector3 const hitToTyre = rayPosBulletWS - rayResults[t].m_hitPointWorld;
		float const hitDistance = hitToTyre.length();
		vec3f const hitPointWorld = fromBullet(rayResults[t].m_hitPointWorld);
		vec3f const hitPointRel = hitPointWorld - fromBullet(rb->getCenterOfMassPosition());

		tyres[t].hasContact = true;
		tyres[t].hitNormalWS = fromBullet(rayResults[t].m_hitNormalWorld);
		tyres[t].forcePos = toBullet(hitPointRel);

		Plane const groundPlaneApprox(tyres[t].hitNormalWS, 0.f);
		vec3f const velocity = velocityAtPointWS(fromBullet(rayResults[t].m_hitPointWorld));
		vec3f const velocityOnGround = groundPlaneApprox.Project(velocity);

		// Suspension.
		{
			float const compression = tyres[t].radius - hitDistance;//1.f - hitDistance / tyres[t].radius;
			vec3f suspensionForce = tyres[t].hitNormalWS * compression * 66.f;
			vec3f const dragForce = -tyres[t].hitNormalWS * dot(tyres[t].hitNormalWS, velocity);

			tyres[t].forceToApply = suspensionForce + dragForce;

			// Update tyre visual position.
			m_tyreTransform[t] = mat4f::getTranslation(hitPointWorld + vec3f(0.f, tyres->radius, 0.f)) * mat4f::getRotationQuat(m_logicTransform.r);

			// Debug draw.
			//grp.getWiered().sphere(mat4f::getTranslation(hitPointWorld), 0xffff00ff, 1.f);
			//grp.getWiered().line(hitPointWorld, hitPointWorld + suspensionForce, 0xff00ff00);
			//grp.getWiered().line(hitPointWorld, hitPointWorld + dragForce, 0xff00ffff);
		}

		// Acceleration + Steering.
		{
			tyres[t].steeringDir = groundPlaneApprox.Project(worldTransform.data[0].xyz()).normalized();

			float const maxVelocity = topSpeed * nitroTopSpeedMult;
			vec3f const velocityAlongSteeringDir = tyres[t].steeringDir * dot(tyres[t].steeringDir, velocityOnGround);
			float const currentVelocity = velocityAlongSteeringDir.length();
			float speedRatio = 1.f - clamp(currentVelocity / maxVelocity, 0.f, 1.f);

			if(tyres[t].isSteering)
			{
				mat4f const rot = mat4f::getAxisRotation(tyres[t].hitNormalWS, m_workingSteering * deg2rad(30.f));
				tyres[t].steeringDir = mat_mul_dir(rot, tyres[t].steeringDir);
				m_tyreTransform[t] = m_tyreTransform[t] * rot;

				
				vec3f totalForce = vec3f(0.f);
				if(sterringInput.y != 0.f) {
					//if(sign(sterringInput.y) == sign(currentVelocity) || currentVelocity == 0.f)
					{
						vec3f const engineForce = tyres[t].steeringDir * (sterringInput.y * speedRatio * (10.0f * nitroAccelMult));
						totalForce = engineForce;
					}
					//else
					//{
					//	// Changing forwards to backwards (or vice versa).
					//	vec3f const engineForce = tyres[t].steeringDir * (sterringInput.y * speedRatio * (20.0f * nitroAccelMult));
					//	//vec3f const fakeFrictionForce = -velocityAlongSteeringDir * 0.6f;
					//	//totalForce = engineForce + fakeFrictionForce;
					//}
				}
				tyres[t].forceToApply += totalForce;
				//grp.getWiered().line(fromBullet(rayResults[t].m_hitPointWorld), fromBullet(rayResults[t].m_hitPointWorld) + totalForce, 0xff00ffff);
			}
		}

		// Handbreak and road friction. Depends on steering
		{
			vec3f const tyreRightWS = cross(tyres[t].hitNormalWS, tyres[t].steeringDir).normalized();

			if(isBreakBtnDown && tyres[t].isHandbrakeable) {
				// Breaks.
				tyres[t].forceToApply += tyres[t].steeringDir * dot(tyres[t].steeringDir, -velocityOnGround.normalized0() * topSpeed * 0.3f) * 0.8f;
				tyres[t].forceToApply += tyreRightWS * dot(tyreRightWS, -velocityOnGround) * 1.f;
			}
			else
			{
				// Friction with the road.
				
				float scale = dot(tyreRightWS, velocity);
				const float speed = velocity.length();

				//float f0 = 3.f;
				//float f1 = 0.7f;
				//float f1Speed = 40.f;

				//float sidewaysFriction = lerp(f0, f1, sqr(sqr(clamp(speed / f1Speed, 0.f, 1.f))));
				//


				const float lowSpeedFriction = 7.f;
				const float highSpeedFriction = 0.8f;
				const float lowSpeed = 5.f;
				const float highSpeed = 15.f;

				float sidewaysFriction = 0.f;
				if(speed < lowSpeed) sidewaysFriction = lowSpeedFriction;
				if(speed > highSpeed) sidewaysFriction = highSpeedFriction;
				else sidewaysFriction = lerp(lowSpeedFriction, highSpeedFriction, (speed - lowSpeed) / (highSpeed - lowSpeed));

				vec3f force = -tyreRightWS * scale * sidewaysFriction;// *sqrt(1.f - sqr(speedRatio - 1.f));
				//force += -tyres[t].steeringDir * dot(tyres[t].steeringDir, velocity) * sidewaysFriction * 0.1f;
				//vec3f force = (tyres[t].steeringDir * dot(tyres[t].steeringDir, velocityOnGround) - velocityOnGround) * 1.f;

				if(sterringInput.y == 0.f)
				{
					force += -tyres[t].steeringDir * minOf(dot(tyres[t].steeringDir, velocityOnGround) * 0.1f, topSpeed * 0.1f);
				}

				tyres[t].forceToApply += force;
				tyres[t].centralTorque += -hitPointRel.cross(force) * antiFlip;

				// Debug draw.
				//grp.getWiered().line(fromBullet(rayResults[t].m_hitPointWorld), fromBullet(rayResults[t].m_hitPointWorld) + velocityOnGround, 0x00ffffff);
				//grp.getWiered().line(fromBullet(rayResults[t].m_hitPointWorld), fromBullet(rayResults[t].m_hitPointWorld) + force, 0xff0000ff);
				//grp.getWiered().line(fromBullet(rayResults[t].m_hitPointWorld), fromBullet(rayResults[t].m_hitPointWorld) + tyres[t].steeringDir, 0xf00f00ff);
			}
		}

	}

	//getWorld()->inspector->showNotification(string_format("%f", rb->getLinearVelocity().length()).c_str());

	// Wind
	if(0)
	{
		float const windScale = clamp(fromBullet(rb->getLinearVelocity()).length() / topSpeed, 0.f, 1.f);
		rb->applyCentralForce(toBullet(windScale * -localUp * 10.f));
	}

	// Apply the forces
	for(int t = 0; t < SGE_ARRSZ(tyres); ++t)
	{
		if(tyres[t].hasContact)
		{
			rb->applyImpulse(toBullet(tyres[t].forceToApply) * updateSets.dt, tyres[t].forcePos);
			rb->applyTorque(toBullet(tyres[t].centralTorque));
		}
	}
}