car.cs

Introduction to 3d game engine design一书的源代码！

						} 
						break;
					case GearState.Drive:
						if ( shift_down ) 
						{
							auto_gear = GearState.ThirdGear;
						} 
						break;
					case GearState.FirstGear:
						if ( shift_up ) 
						{
							auto_gear = GearState.SecondGear;
						} 
						else if ( shift_down ) 
						{
							auto_gear = GearState.Neutral;
						} 
						break;
					case GearState.SecondGear:
						if ( shift_up ) 
						{
							auto_gear = GearState.ThirdGear;
						} 
						else if ( shift_down ) 
						{
							auto_gear = GearState.FirstGear;
						} 
						break;
					case GearState.ThirdGear:
						if ( shift_up ) 
						{
							auto_gear = GearState.Drive;
						} 
						else if ( shift_down ) 
						{
							auto_gear = GearState.SecondGear;
						} 
						break;
				}
					break;
				case GearState.FirstGear:
					auto_gear = GearState.FirstGear;
					break;
				case GearState.SecondGear:
					auto_gear = GearState.SecondGear;
					break;
				case GearState.ThirdGear:
					auto_gear = GearState.ThirdGear;
					break;
			}
			current_gear_ratio = gear_ratio[(int)auto_gear];

			if ( running && target_rpm < idle_rpm ) 
			{
				target_rpm = idle_rpm;
			} 
			else if ( !running ) 
			{
				target_rpm = 0.0f;
			}
			else 
			{
				target_rpm = idle_rpm + throttle * ( max_rpm - idle_rpm);
			}
			delta_rpm = target_rpm - engine_rpm;
			if ( delta_rpm > 3000.0f ) 
			{
				delta_rpm = 3000.0f;
			} 
			else if ( delta_rpm < -3000.0f ) 
			{
				delta_rpm = -3000.0f;
			}
			if ( delta_rpm < 1.0f && delta_rpm > -1.0f ) 
			{
				delta_rpm = 0.0f;
			}
			engine_rpm += (delta_rpm * delta_t );
			if ( auto_gear == GearState.Neutral || gear == GearState.Park ) 
			{
				eng_torque = 0.0;
			} 
			else 
			{
				eng_torque = torque_curve.Interpolate(percent_rpm * 100.0) * max_engine_torque;
			}

			engine_loss = Math.Max(((engine_rpm/20) * (engine_rpm/20) + 45), 0.0);

			brake_torque = brake_force * mass; 

			temp = (eng_torque - engine_loss);

			if ( temp < 0.0 && Math.Abs(mph) < 0.1 ) 
			{
				temp = 0.0;
			}

			if ( current_gear_ratio != 0.0 ) 
			{
				wheel_force = temp;
			} 
			else 
			{
				wheel_force = 0.0f;
			}

			if ( (drag + rolling_resistance) > wheel_force )
			{
				wheel_force = drag + rolling_resistance;
			}
			net_force = wheel_force - drag - rolling_resistance;
//Debug.WriteLine("wheel force=" + wheel_force.ToString() + " drag=" + drag + " rolling resist=" + rolling_resistance);

			if ( gear == GearState.Reverse ) 
			{
				net_force *= -1.0f;      // force in reverse is in opposite direction
			}

			ground_slope.RotateEtoA( gravity );
			body_gravity = gravity;

			if ( gear != GearState.Park ) 
			{
				double net_accel = net_force/mass;  // covert to an accel
				net_accel -= brake_force;
				acceleration.X = net_accel;// + body_gravity.X;
//				Debug.WriteLine("Accel X=" + acceleration.X.ToString() + " body grav X=" + body_gravity.X.ToString()+
//					" net force= " + net_force.ToString());
//				Debug.WriteLine("ground slope=" + ground_slope.ThetaAsDegrees().ToString());
			}
			acceleration.Z -= body_gravity.Z;

			if ( velocity.X < ( delta_t * brake_force ) && velocity.X > ( delta_t * -brake_force ) ) 
			{
				mph = 0.0f;
				velocity.X = 0.0;
				acceleration.X =0.0;
				brake_force = 0.0;
			}

			if ( auto_gear == GearState.Neutral || gear == GearState.Park ) 
			{
				temp = idle_rpm + (max_rpm-idle_rpm) * throttle;
			} 
			else 
			{
				temp = velocity.X * current_gear_ratio;
			}

			if ( temp >= (idle_rpm * 0.9f) ) 
			{
				if ( temp > max_rpm ) 
				{
					target_rpm = max_rpm;
				} 
				else 
				{
					target_rpm = temp;
				}
			} 
			else 
			{
				target_rpm = idle_rpm;
			}

			CalcWeightTransfer();

			ProcessWheels( delta_t );

			ProcessAttitude( delta_t );

			IntegratePosition( delta_t );
		}

		void SetAttitude(float roll, float pitch, float heading)
		{
			attitude.Phi = roll;
			attitude.Theta = pitch;
			attitude.Psi = heading;
		}

		void SetPosition(float north, float east, float height)
		{
			position.X = north;
			position.Y = east;
			position.Z = height;
		}

		void SetVelocity(float north, float east, float vertical)
		{
			velocity.X = north;
			velocity.Y = east;
			velocity.Z = vertical;
		}

		void SetGroundHeight(WhichWheel the_wheel, float height)
		{
			wheel[(int)the_wheel].ground_height = height;
		}

		void SetAllGroundHeights(float left_front, float right_front, float left_rear, float right_rear )
		{
			wheel[(int)WhichWheel.LeftFront].ground_height = left_front;
			wheel[(int)WhichWheel.RightFront].ground_height = right_front;
			wheel[(int)WhichWheel.LeftRear].ground_height = left_rear;
			wheel[(int)WhichWheel.RightRear].ground_height = right_rear;
		}

		double WheelAngle( bool in_degrees )
		{
			double result;

			if ( in_degrees ) 
			{
				result = (wheel_angle * 180.0 / Math.PI);
			} 
			else 
			{
				result = wheel_angle;
			}

			return result;
		}

		double GetPitch(bool in_degrees)
		{
			double result;

			if ( in_degrees ) 
			{
				result = attitude.ThetaAsDegrees();
			} 
			else 
			{
				result = attitude.Theta;
			}

			return result;
		}

		double GetRoll(bool in_degrees)
		{
			double result;

			if ( in_degrees ) 
			{
				result = attitude.PhiAsDegrees();
			} 
			else 
			{
				result = attitude.Phi;
			}

			return result;
		}

		bool IsTireSquealing(WhichWheel the_wheel)
		{
			return wheel[(int)the_wheel].squealing;
		}

		bool IsTireLoose(WhichWheel the_wheel)
		{
			return wheel[(int)the_wheel].sliding;
		}

		void SetTireStiction(float new_value)
		{
			wheel[(int)WhichWheel.LeftFront].Stiction = new_value;
			wheel[(int)WhichWheel.RightFront].Stiction = new_value;
			wheel[(int)WhichWheel.LeftRear].Stiction = new_value;
			wheel[(int)WhichWheel.RightRear].Stiction = new_value;

		}
		void ProcessWheels(float delta_t)
		{
			int		i;
			double	accel;
			double	total_upwards_force = 0.0;
			bool	bottomed_out = false;
			bool	on_ground = false;
			int		sliding = 0;
			double	avg_suspension = 0.0;
			double	delta_force;
			double	avg_ground_height = 0.0;

			acceleration.SetY(-centripedal_accel);

			for ( i=0; i<4; i++ ) 
			{
				wheel[i].Process( delta_t, attitude, acceleration, velocity, position );
				total_upwards_force += wheel[i].UpwardsForce;
				avg_suspension += wheel[i].suspension_offset;
				avg_ground_height += wheel[i].ground_height;
				if ( wheel[i].bottomed_out ) 
				{
					bottomed_out = true;
				}
				if ( wheel[i].touching_ground ) 
				{
					on_ground = true;
				}
				if ( wheel[i].sliding ) 
				{
					sliding++;
				}
			}
			avg_suspension /= 4.0f;
			avg_ground_height /= 4.0f;

			if ( Math.Abs(avg_suspension) < 0.1f ) 
			{
				velocity.Z = velocity.Z * 0.2;

			}

			accel = total_upwards_force / mass;
//			Debug.WriteLine("upwards force " + accel.ToString());
			acceleration.Z = accel - body_gravity.Z;
//			Debug.WriteLine("Accel Z="+acceleration.Z.ToString());

			if ( on_ground ) 
			{
				velocity.Z = velocity.Z * 0.75;
//				Debug.WriteLine("on ground");
			}

			if ( bottomed_out ) 
			{
//				position.Z = avg_ground_height + wheel[0].offset.X + wheel[0].radius;
//				Debug.WriteLine("bottomed out");
			}

			if ( bottomed_out && velocity.Z < 0.0f ) 
			{
				velocity.Z = 0.0;
//				Debug.WriteLine("bottomed out & velocity cleared");
			}

			if ( sliding > 2 && !was_sliding ) 
			{
				was_sliding = true;
//				velocity.Y = acceleration.Y;
			}
			if ( sliding > 2 && velocity.Y > 0.0 ) 
			{
				acceleration.Y = -32.0;
//				Debug.WriteLine("sliding");
			} 
			else if ( sliding > 2 && velocity.Y < 0.0 ) 
			{
				acceleration.Y = 32.0;
//				Debug.WriteLine("sliding");
			} 
			else 
			{
				velocity.Y = 0.0;
				acceleration.Y = 0.0;
				was_sliding = false;
			}
		}

		void ProcessAttitude(float delta_t)
		{
			double avg_front;
			double avg_rear;
			double pitch;
			double avg_left;
			double avg_right;
			double roll;

			// first do ground slope
			avg_front = (wheel[(int)WhichWheel.LeftFront].ground_height + wheel[(int)WhichWheel.RightFront].ground_height) / 2.0;
			avg_rear  = (wheel[(int)WhichWheel.LeftRear].ground_height + wheel[(int)WhichWheel.RightRear].ground_height) / 2.0;
			pitch = Math.Asin((avg_front - avg_rear) / wheel_base);

			ground_slope.Theta = pitch;

			avg_left = (wheel[(int)WhichWheel.LeftFront].ground_height + wheel[(int)WhichWheel.LeftRear].ground_height) / 2.0;
			avg_right  = (wheel[(int)WhichWheel.RightFront].ground_height + wheel[(int)WhichWheel.RightRear].ground_height) / 2.0;
			roll = Math.Asin((avg_left - avg_right) / wheel_track);

			ground_slope.Phi = roll;

			// now do vehicle attitude
			avg_front = (wheel[(int)WhichWheel.LeftFront].suspension_offset + wheel[(int)WhichWheel.RightFront].suspension_offset) / 2.0f;
			avg_rear  = (wheel[(int)WhichWheel.LeftRear].suspension_offset + wheel[(int)WhichWheel.RightRear].suspension_offset) / 2.0f;
			pitch = Math.Asin((avg_front - avg_rear) / wheel_base);
			pitch = 0.0;

//			attitude_rate.Theta = (ground_slope.Theta+pitch-attitude.Theta) * 0.025;
//			attitude.Theta = ground_slope.Theta;

			avg_left = (wheel[(int)WhichWheel.LeftFront].suspension_offset + wheel[(int)WhichWheel.LeftRear].suspension_offset) / 2.0f;
			avg_right  = (wheel[(int)WhichWheel.RightFront].suspension_offset + wheel[(int)WhichWheel.RightRear].suspension_offset) / 2.0f;
			roll = Math.Asin((avg_left - avg_right) / wheel_track);
			roll = 0.0;

//			attitude_rate.Phi = (ground_slope.Phi-roll-attitude.Phi) * 0.05;
//			attitude.Phi = ground_slope.Phi;

		}

		public void MinorCollision()
		{
			velocity = velocity * 0.9f;
		}

		public void MajorCollision( float delta_x_velocity, 
			float delta_y_velocity, 
			float delta_z_velocity, 
			float delta_x_position, 
			float delta_y_position, 
			float delta_z_position )
		{
			Vector RelativeVelocity = new Vector( delta_x_velocity, delta_y_velocity, delta_z_velocity );
			Vector CollisionNormal = new Vector( delta_x_position, delta_y_position, delta_z_position );
			CollisionNormal.Normalize();

			double collisionSpeed = RelativeVelocity * CollisionNormal;

			float impulse = (float)(( 2.50 * collisionSpeed ) / 
				(( CollisionNormal * CollisionNormal) * 
				( inverse_mass )));

			Debug.WriteLine("major collision");
			Debug.WriteLine("velocity prior to crash x="+velocity.X+" y="+velocity.Y+" z="+velocity.Z);
			velocity = ( CollisionNormal * impulse ) * (float)inverse_mass;
			Debug.WriteLine("velocity after crash x="+velocity.X+" y="+velocity.Y+" z="+velocity.Z);
			engine_rpm = idle_rpm;
		}
	};
}