In Part 2 we made a basic physics simulation of a car.

The car model was made of 5 points(4 wheels and the car center).

We included gravity in the simulation and we were able to drive over a terrain and fall from ledges.

In this part we will add suspensions and the car will be able to jump over ramps and drive smoothly over small bumps.

The Models

The model of the track remains the same as in part 1 and 2(a set of 3D triangles).

The car is made out of 5 points(4 for the wheels and 1 for the center) like in Part 2.

We will also save the gravity velocity of each of these 5 points from the previous frame and also save the car’s orientation from the previous frame.

Unlike part 2 we will also add suspensions to the wheels which will be modeled with springs.

The resting position of the wheels will be lower than the car’s center(a lower wheel base).

Our wheel base will be 1.8 m wide, 4 m long and 1 m (which is also the spring length) bellow the car’s center.

Steering

Like in part 2 we calculate the new Look and Right vectors(the orientation) of the car by rotating the Look and Right vectors from the previous frame on the Look X Right plane.

However, this time we will only do so if the front wheels were touching the terrain in the previous frame.

double DeltaAngle = WheelFactor*t*AngleSpeed;
if (!LastFrontWheelsTouch)
DeltaAngle = 0.0;
Look = LastRight*sin(DeltaAngle)+ LastLook*cos(DeltaAngle);
Right = LastRight*cos(DeltaAngle)- LastLook*sin(DeltaAngle);

The Suspensions(Springs)

In the real world, if cars didn’t have suspensions every small bump in the road would rattle the car. The suspensions allow the car to drive smoothly over uneven road and obstacles.

In part 2 whenever the car drove over a small bump it would move quite violently, or even worse, it would make the car jump.

With the steering we use now, which only function if the front wheels touch the surface, these small bumps would make the steering very difficult.

We model the car’s suspensions using a spring equation. One spring for each one of the 4 wheels.

The basic ODE(Ordinary Differential Equation) of a spring is:

m*x” = -k*x

An ODE is a certain way to express one or several equations implicitly(if they even exist).

In the ODE above m is the car’s mass, x” is the acceleration, k is the spring’s strength constant and x is the distance of the edge of the spring(or the wheel attached to the spring) from it’s resting position.

A spring is a device that apply force whenever it is not in it’s resting form. Like when it’s stretched or compressed.

m*x” is force expressed as mass multiplied by acceleration.

So we can see that the force the spring applies is the opposite direction to the distance of the spring’s free edge from it’s resting position(multiplied by a factor k).

How does this equation help us?

We could try to apply the force to our wheels every frame but that won’t be accurate.

Since this equation is non linear calculating it in the frame’s delta time granularity will give us bad results.

What we actually want is to analytically extract the motion equation from the ODE.

The movement of the spring depending on the time t and with the initial conditions c1 and c2 is:

Our initial conditions c1 and c2 are found at time t=0.

c1 = x(0)

x2 = v(0)/sqrt(k/mass)

Ok we got all the initial conditions and we can now assign t to the equations to get the spring’s length and veolocity.

But how does this spring interact with the world?

Well what we actually do is recalculate the initial condition every frame.

The spring’s current length and velocity are adjusted from the interaction with the track and are fed again into the spring equations in the next frame.

Since the equation is agnostic to the initial time we can assume that time starts from t=0 every frame and only use the frame’s time delta as the time we want to calculate the new motion of the spring.

The code for updating the spring motion is the following:

There is a problem with the springs we used above.

They do not lose energy.

It means that if the car’s springs are not in the resting position they will oscillate forever even if the car stays in the same place.

We model springs with energy loss with the following ODE:

m*x” = -k*x -c*x’

The springs force is now affected by the spring’s velocity.

The energy loss constant is c and x’ is the spring’s velocity.

Much like in the previous ODE I used an online ODE solver and found the equation for the motion in dependency of the time t.

I then derived the equation to get the velocity equation.

After finding the two equations I assigned t=0 to the two equations to find the initial conditions.

The last step was to calculate an energy loss constant that made sense.

If the energy loss constant is 0 then you will get a square root of a negative number which is an imaginary number.

This actually makes sense because this imaginary value is the power of two exponents.

An imaginary power of an exponent is also an imaginary number but when you add two exponents with symmetric imaginary numbers the imaginary part “disappears” and you get the same equation we had with the previous ODE.

The code for updating the spring motion based on these equations is:

The first step is the steering(in case the front wheels were on track in the previous frame).

The second step is updating the springs.

The third step is adding the thrust of the car(only if the front wheels were on the track in the previous frame). We also apply gravity and update the car and wheel’s position with the current velocity.

The fourth step is testing whether the wheels penetrate the track and by how much. If they do we adjust the springs length and velocity and also the gravity velocity of the wheels and the center of the car.

The fifth and last step is recalculating the orientation of the now deformed wheel base.

In the third step we basically add the thrust according to the wheel base orientation and we make sure that we don’t add thrust when the car’s velocity is equal or greater to the car’s maximum speed.

Testing for wheel penetration is done similar to what we did in part 2.

We cast a ray from slightly above the wheel’s position down to each triangle in the terrain.

If the ray intersects a triangle we can tell if the wheel penetrates that triangle and by how much according to the where the ray intersect the triangle.

(We presented an optimization to this test in part 2 so that we wouldn’t need to test against all the triangles for each wheel).

This time, as opposed to in part 2, we do not adjust the wheel’s position directly if there is a penetration.

Instead, we adjust the wheel’s spring length and adjust the wheels velocity.

(Notice the wheels velocity and the spring’s speed are kept in separate variables).

If the spring’s speed pushes the spring velocity downwards into the triangle, we zero this speed and we add it to the wheel’s velocity and also to the car’s center velocity.

(This part needs more work though, as the result are not good in all the scenarios).

This is where the springs interact with the terrain and the initial conditions of the spring equations are modified.

The final part is restoring the wheel base from it’s deformed state and calculate the new car orientation.

(Quote from part 2)

We will restore the original form of the wheel base by treating the 4 wheels as if they have springs among themselves(a total of 6 springs).

This will make the 4 wheel points simulate the wheels base as a if it was a rigid body.

In order to restore the original form of the wheels base we go over all the 6 springs and adjust them to be closer to their original length.

For the sake of completion here is the update code:

void Update (double t)
{
if (Input.GetLeft())
WheelFactor -= t/AngleLatency;
else if (Input.GetRight())
WheelFactor += t/AngleLatency;
else
WheelFactor = std::max(std::fabs(WheelFactor)-(2.0*t/AngleLatency), 0.0)*(WheelFactor>0.0?1.0:-1.0);
WheelFactor = std::max(std::min(WheelFactor, 1.0), -1.0);
double CurrentFlatSpeed = 0.0;
if (Input.GetThrust())
CurrentFlatSpeed = MaxFlatSpeed;
// Step 1: Steering.
double DeltaAngle = WheelFactor*t*AngleSpeed;
if (!LastFrontWheelsTouch)
DeltaAngle = 0.0;
Graphics2D::Position Look = LastRight*sin(DeltaAngle)+ LastLook*cos(DeltaAngle);
Graphics2D::Position Right = LastRight*cos(DeltaAngle)- LastLook*sin(DeltaAngle);
Graphics2D::Position Up = Look.Cross(Right).Normalize();
Graphics2D::Position YAxis = Graphics2D::Position (0, 1, 0);
// Step 2: Update springs.
for (unsigned int i=0; i<WheelsSpring.size(); i++)
{
double a1 = (WheelsSpring[i]*SpringParaboleNegative-WheelsSpringSpeed[i])/SpringParabole;
double a2 = (WheelsSpring[i]*SpringParabolePositive+WheelsSpringSpeed[i])/SpringParabole;
WheelsSpringSpeed[i] = -a1*SpringParabolePositive*exp(-t*SpringParabolePositive)+a2*SpringParaboleNegative*exp(t*SpringParaboleNegative);
WheelsSpring[i] = a1*exp(-t*SpringParabolePositive)+a2*exp(t*SpringParaboleNegative);
}
// Step 3: Thrust, gravity and position update.
if (LastFrontWheelsTouch)
{
double ThrustSpeed = CurrentVelocity.Dot(Look);
if (!Input.GetThrust())
CurrentVelocity = CurrentVelocity-Look*std::max(std::min(ThrustSpeed, 0.25*MaxFlatSpeed*t/AccelLatency), -0.25*MaxFlatSpeed*t/AccelLatency);
double ForwardSpeed = std::max(CurrentVelocity.Dot(Look), 0.0);
double DownSpeed = std::min(CurrentVelocity.Dot(Up), 0.0);
double RightSpeed = CurrentVelocity.Dot(Right);
RightSpeed = std::min(fabs(RightSpeed), MaxFlatSpeed*t/AccelLatency)*(RightSpeed>0.0?1.0:-1.0);
CurrentVelocity = CurrentVelocity+Look*(std::min(std::max(MaxFlatSpeed-ForwardSpeed, 0.0), CurrentFlatSpeed*t/AccelLatency))-Right*RightSpeed-Up*(LastCenterTouch?DownSpeed:0.0);
}
CurrentVelocity=CurrentVelocity+Gravity*t;
for (unsigned int i=0; i<WheelsGravityVelocity.size(); i++)
WheelsGravityVelocity[i]=WheelsGravityVelocity[i]+Gravity*t+(LastCenterTouch?YAxis*Look.Dot(YAxis)*CurrentFlatSpeed*t:Graphics2D::Position(0, 0, 0));
CarParms->SetLook (Look, Graphics2D::Position(0, 1, 0));
std::vector<Graphics2D::Position> Wheels;
Wheels.resize(WheelsDelta.size());
for (unsigned int i=0; i<Wheels.size(); i++)
Wheels[i] = Right*WheelsDelta[i].x+Look*WheelsDelta[i].z+Up*WheelsDelta[i].y+Pos+(Look*CurrentFlatSpeed+WheelsGravityVelocity[i])*t;
LastSteerTouch = false;
LastCenterTouch = false;
Pos = Pos+CurrentVelocity*t;
double TouchDistance = 1.*(std::max(-WheelsSpring[0], std::max(-WheelsSpring[1], std::max(-WheelsSpring[2], std::max(-WheelsSpring[3], 0.0))))+SpringLength);
LastFrontWheelsTouch = false;
Pos.y = std::max(0.0, Pos.y);
std::list<unsigned int>::iterator q;
CarParms->SetPosition (Pos);
// Step 4: Penetration test and handling
std::vector<bool> isTouch;
isTouch.resize(4);
std::list<Graphics2D::Position> Normals;
Graphics2D::Position PreviousVelocity = CurrentVelocity;
for (unsigned int i=0; i<Wheels.size(); i++)
{
if (Up.y<0.00001)
continue;
Graphics2D::Position p = Wheels[i]+Up*SpringLength;
unsigned int StartX = std::min((unsigned int)(std::max((p.x-Min.x)/(Max.x-Min.x), 0.0)), TrackGrid[0].size()-1);
unsigned int StartZ = std::min((unsigned int)(std::max((p.z-Min.z)/(Max.z-Min.z), 0.0)), TrackGrid.size()-1);
p = Wheels[i];
Graphics2D::Position p2 = p-Up*SpringLength;
unsigned int EndX = std::min((unsigned int)(std::max((p2.x-Min.x)/(Max.x-Min.x), 0.0)), TrackGrid[0].size()-1);
unsigned int EndZ = std::min((unsigned int)(std::max((p2.z-Min.z)/(Max.z-Min.z), 0.0)), TrackGrid.size()-1);
if (EndX<StartX)
{
unsigned int KeepX = StartX;
StartX = EndX;
EndX = KeepX;
}
if (EndZ<StartZ)
{
unsigned int KeepZ = StartZ;
StartZ = EndZ;
EndZ = KeepZ;
}
double ElasticEnergyLoss = 0.9;
bool WheelTouch = false;
for (unsigned int CountZ = StartZ; CountZ<=EndZ; CountZ++)
for (unsigned int CountX = StartX; CountX<=EndX; CountX++)
{
std::list<unsigned int>::iterator q;
for (q = TrackGrid[CountZ][CountX].begin(); q != TrackGrid[CountZ][CountX].end(); q++)
{
const math::Ray r(float3(p.x, p.y, p.z)+float3(Up.x, Up.y, Up.z)*10.0*SpringLength, -float3(Up.x, Up.y, Up.z));
float d = 0;
math::float3 Point;
if (TrackGeometry[*q].Intersects(r, &d, &Point))
{
double UpLength = r.pos.y/Up.y;
double UpWheelHeight = Wheels[i].y/Up.y;
if (UpLength-d>=UpWheelHeight)
{
double Penetrate = std::max(UpLength-d, 0.0)-UpWheelHeight;
if (Penetrate>WheelsSpring[i])
{
WheelsSpring[i] = std::max(WheelsSpring[i], Penetrate);//std::min(Penetrate, SpringLength));
double DownSpringSpeed = -std::min(WheelsSpringSpeed[i], 0.0);
CurrentVelocity = CurrentVelocity+Up*std::max(DownSpringSpeed+std::min(WheelsGravityVelocity[i].y, 0.0), 0.0)/(double)WheelsGravityVelocity.size();//*EnergyLossFactor;
if (Penetrate>SpringLength)
WheelsGravityVelocity[i].y = std::max(0., WheelsGravityVelocity[i].y);//-DownSpringSpeed;
else if (WheelsGravityVelocity[i].y<0.0)
{
WheelsGravityVelocity[i].y+=DownSpringSpeed*Up.y;
WheelsGravityVelocity[i].y = std::min(WheelsGravityVelocity[i].y, 0.0);
}
}
if (i<2)
LastFrontWheelsTouch = true;
WheelTouch = true;
isTouch[i] = true;
}
}
}
}
if (WheelTouch)
continue;
if (FloorHeight>=Wheels[i].y)
{
double Penetrate = FloorHeight-Wheels[i].y/Up.y;
if (Penetrate>WheelsSpring[i])
{
WheelsSpring[i] = std::max(WheelsSpring[i], Penetrate);//std::min(Penetrate, SpringLength));
double DownSpringSpeed = -std::min(WheelsSpringSpeed[i], 0.0);
CurrentVelocity = CurrentVelocity+Up*std::max(DownSpringSpeed+std::min(WheelsGravityVelocity[i].y, 0.0), 0.0)/(double)WheelsGravityVelocity.size();//*EnergyLossFactor;
if (Penetrate>SpringLength)
WheelsGravityVelocity[i].y = std::max(0., WheelsGravityVelocity[i].y);//-DownSpringSpeed;
if (WheelsGravityVelocity[i].y<0.0)
{
WheelsGravityVelocity[i].y+=DownSpringSpeed*Up.y;
WheelsGravityVelocity[i].y = std::min(WheelsGravityVelocity[i].y, 0.0);
}
}
if (i<2)
LastFrontWheelsTouch = true;
isTouch[i] = true;
}
else
{
}
}
// Step 5: Wheel base correction and orientation calculation
for (unsigned int k=0; k<10; k++)
{
for (unsigned int i=0; i<Wheels.size(); i++)
for (unsigned int j=i+1; j<Wheels.size(); j++)
{
Graphics2D::Position v = Wheels[i]-Wheels[j];
Graphics2D::Position center = (Wheels[i]+Wheels[j])*0.5;
double l = (WheelsDelta[i]-WheelsDelta[j]).Length();
double radius = (0.1*l+0.9*v.Length())/2.0;
Wheels[i] = (Wheels[i]-center).Normalize()*radius+center;
Wheels[j] = (Wheels[j]-center).Normalize()*radius+center;
}
}
Look = (Wheels[0]-Wheels[3]).Normalize();
Look.y = std::min(fabs(Look.y), 0.65)*(Look.y>0.0?1.0:-1.0);
Look = Look.Normalize();
Right = (Wheels[1]-Wheels[0]).Normalize();
Right.y = std::min(fabs(Right.y), 0.65)*(Right.y>0.0?1.0:-1.0);
Right = Right.Normalize();
LastLook = Look;
LastRight = Right;
CarParms->SetLook(Look, Look.Cross(Right));
for (unsigned int i=0; i<Wheels.size(); i++)
{
TireParm[i]->SetPosition(Right*WheelsDelta[i].x+Look*WheelsDelta[i].z+Look.Cross(Right)*(WheelsSpring[i]+WheelsDelta[i].y)+Pos);
TireParm[i]->SetLook(Look, Look.Cross(Right));
}
}

Conclusion

In this part we have improved our car model by adding suspensions to the wheels.

The suspensions are modeled with springs.

We used analytic equations derived from ODEs to update the springs’ motion.

The springs were affected by the terrain by adjusting the initial conditions of the springs equations.

This simulation is more realistic but is still lacking in certain scenarios.

Notice we are now testing wheel/triangle intersection with rays that do not necessarily align to the y axis but rather align to the car’s Up vector.

We have also forced the car’s orientation to a certain cone so the car won’t be able to flip upside down. This pose all sort of issues.

In the next part we will try to improve on this.

On the mean time you can see the results of this simulation in the following video:

In part 1 we have shown how to make a car(truck) glide on a track made out of 3D triangles.

We were able to steer the car and it was “glued” to the track to make it seem like there are physics involved.

In this part we are going to have an actual physics simulation(to some degree).

The Models

The model of the track remains the same as in part 1(a set of 3D triangles).

The car model is made of 5 points: 4 for the wheels and 1 for the bottom center. Just like in part 1.

However, we will also maintain variables to keep the orientation of the car and the gravity velocity of each of the 4 wheels(in addition to the car’s gravity velocity).

Steering

Since we now maintain the orientation of the car from the previous frame we can’t just assume the steering vector is calculated aligned to the xz plane.

To calculate the new steering vector we rotate the Look and Right vectors by a delta of an angle on the Look x Right plane:

Look = LastRight*sin(DeltaAngle)+ LastLook*cos(DeltaAngle);
Right = LastRight*cos(DeltaAngle)- LastLook*sin(DeltaAngle);

The delta of the angle is the angular speed multiplied by the frame’s time delta.

The forward velocity is calculated the same as in part 1 but notice the Look vector might not be on the xz plane this time.

Gravity

This time we are also going to take gravity into account.

Our gravity is the following vector (0, -9.8, 0).

It is a vector pointing downwards with an acceleration of 9.8 meters per square second.

We compose the car’s velocity from two components. The steering velocity from above and the Gravity velocity.

The reason we separate the two is because it would be easier to set the gravity velocity to 0 whenever a point in the model hits the ground or track.

We also need to maintain a separate gravity velocity for each of the 5 points in the car model to make the physics simulation of the car work as if we had an object with volume.

The Simulation

After we calculated the Look and Right orientation vectors and the steering velocity vector we need to apply gravity.

For each of the 5 gravity velocity vectors we add the Gravity vector multiplied by the frame’s time delta.

CurrentGravityVelocity=CurrentGravityVelocity+Gravity*t;
for (unsigned int i=0; i<WheelsGravityVelocity.size(); i++)
WheelsGravityVelocity[i]=WheelsGravityVelocity[i]+Gravity*t;

The next step is to calculate the current position of the wheels and the position of the car center.

For the car center position, we add the current steering velocity vector and the current gravity velocity vector multiplied by the frame’s time delta.

For each of the 4 wheels we calculate the wheel’s position relative to the car’s center using the wheels base dimensions and the Look and Right orientation vectors.

We then add the car’s center position to each of the 4 wheels positions.

The last step is to add the steering velocity and the gravity velocity multiplied by the time delta. The steering velocity is the same as it was for the car’s center but notice the gravity velocity might be unique for each wheel point(We saved them in their own variables).

for (unsigned int i=0; i<Wheels.size(); i++)
Wheels[i] = Right*WheelsDelta[i].x+Look*WheelsDelta[i].z+Pos+(Look*CurrentFlatSpeed+WheelsGravityVelocity[i])*t;

In a similar fashion to part 1 we now check which triangles each one of the 5 points of the model intersect with.

The difference is that instead of setting the points to the intersection height of the respective triangles, we only set them to the intersection height in case their own height is lower.

In addition we set to zero the Gravity velocity of each point only if it’s height was adjusted by a triangle.

Now we have 5 points of the car displaced into new heights. The car’s center point will be used for the new position, but like in part 1, we need to calculate the new orientation from the 4 wheel points.

The current car model simulates a rigid body. However while adjusting the wheels’ heights the car’s wheel base is now deformed.

We will restore the original form of the wheel base by treating the 4 wheels as if they have springs among themselves(a total of 6 springs).

This will make the 4 wheel points simulate the wheels base as a if it was a rigid body.

In order to restore the original form of the wheels base we go over all the 6 springs and adjust them to be closer to their original length.

We iterate over this process for ten times and at the end we would get something closer to the original form.

for (unsigned int k=0; k<10; k++)
{
for (unsigned int i=0; i<Wheels.size(); i++)
for (unsigned int j=i+1; j<Wheels.size(); j++)
{
Graphics2D::Position v = Wheels[i]-Wheels[j];
Graphics2D::Position center = (Wheels[i]+Wheels[j])*0.5;
double l = (WheelsDelta[i]-WheelsDelta[j]).Length();
double radius = (0.1*l+0.9*v.Length())/2.0;
Wheels[i] = (Wheels[i]-center).Normalize()*radius+center;
Wheels[j] = (Wheels[j]-center).Normalize()*radius+center;
}
}

Now that we have the wheel points placed on the wheels base frame we can calculate the new Look and Right orientation vectors in a similar fashion we did in part 1.

Look = (Wheels[0]-Wheels[3]).Normalize();
Right = (Wheels[1]-Wheels[0]).Normalize();

Conclusion

We now have a more physically based simulation that also support falling off from edges.

The result of this simulation can be seen in this video:

For the sake of completion I am adding the entire code for the update function.

void Update (double t)
{
double WheelFactor = 0;
if (Input.GetLeft())
WheelFactor = -1;
else if (Input.GetRight())
WheelFactor = 1;
if (Input.GetThrust())
CurrentFlatSpeed += MaxFlatSpeed*t/AccelLatency;
CurrentFlatSpeed = std::max(std::min(CurrentFlatSpeed, MaxFlatSpeed), 0.0);
double DeltaAngle = WheelFactor*t;
Graphics2D::Position Look = LastRight*sin(DeltaAngle)+ LastLook*cos(DeltaAngle);
Graphics2D::Position Right = LastRight*cos(DeltaAngle)- LastLook*sin(DeltaAngle);
CurrentGravityVelocity=CurrentGravityVelocity+Gravity*t;
for (unsigned int i=0; i<WheelsGravityVelocity.size(); i++)
WheelsGravityVelocity[i]=WheelsGravityVelocity[i]+Gravity*t;
CurrentVelocity = Look*CurrentFlatSpeed+CurrentGravityVelocity;
CarParms->SetLook (Look, Graphics2D::Position(0, 1, 0));
std::vector<Graphics2D::Position> Wheels;
Wheels.resize(WheelsDelta.size());
for (unsigned int i=0; i<Wheels.size(); i++)
Wheels[i] = Right*WheelsDelta[i].x+Look*WheelsDelta[i].z+Pos+(Look*CurrentFlatSpeed+WheelsGravityVelocity[i])*t;
Pos = Pos+CurrentVelocity*t;
if (Pos.y<0.0)
CurrentGravityVelocity.y = std::max(0., CurrentGravityVelocity.y);
Pos.y = std::max(0.0, Pos.y);
unsigned int StartX = std::min((unsigned int)(std::max((Pos.x-Min.x)/(Max.x-Min.x), 0.0)), TrackGrid[0].size()-1);
unsigned int StartZ = std::min((unsigned int)(std::max((Pos.z-Min.z)/(Max.z-Min.z), 0.0)), TrackGrid.size()-1);
std::list<unsigned int>::iterator q;
for (q = TrackGrid[StartZ][StartX].begin(); q != TrackGrid[StartZ][StartX].end(); q++)
{
const math::Ray r(float3(Pos.x, 100.0, Pos.z), float3(0, -1, 0));
float d = 0;
math::float3 Point;
if (TrackGeometry[*q].Intersects(r, &d, &Point))
{
if (r.pos.y-d>=Pos.y)
{
CurrentGravityVelocity.y = std::max(0., CurrentGravityVelocity.y);
Pos.y = r.pos.y-d;
}
}
}
CarParms->SetPosition (Pos);
// std::vector<bool> IsWheelContact;
// IsWheelContact.resize(4, false);
for (unsigned int i=0; i<Wheels.size(); i++)
{
Graphics2D::Position p = Wheels[i];
unsigned int StartX = std::min((unsigned int)(std::max((p.x-Min.x)/(Max.x-Min.x), 0.0)), TrackGrid[0].size()-1);
unsigned int StartZ = std::min((unsigned int)(std::max((p.z-Min.z)/(Max.z-Min.z), 0.0)), TrackGrid.size()-1);
std::list<unsigned int>::iterator q;
for (q = TrackGrid[StartZ][StartX].begin(); q != TrackGrid[StartZ][StartX].end(); q++)
{
const math::Ray r(float3(p.x, 100.0, p.z), float3(0, -1, 0));
float d = 0;
math::float3 Point;
if (TrackGeometry[*q].Intersects(r, &d, &Point))
{
if (r.pos.y-d>=Wheels[i].y)
{
WheelsGravityVelocity[i].y = std::max(0., WheelsGravityVelocity[i].y);
// IsWheelContact[i] = true;
Wheels[i].y = r.pos.y-d;
}
}
}
if (FloorHeight>=Wheels[i].y)
{
WheelsGravityVelocity[i].y = std::max(0., WheelsGravityVelocity[i].y);
// IsWheelContact[i] = true;
Wheels[i].y = FloorHeight;
}
}
for (unsigned int k=0; k<10; k++)
{
for (unsigned int i=0; i<Wheels.size(); i++)
for (unsigned int j=i+1; j<Wheels.size(); j++)
{
Graphics2D::Position v = Wheels[i]-Wheels[j];
Graphics2D::Position center = (Wheels[i]+Wheels[j])*0.5;
double l = (WheelsDelta[i]-WheelsDelta[j]).Length();
double radius = (0.1*l+0.9*v.Length())/2.0;
Wheels[i] = (Wheels[i]-center).Normalize()*radius+center;
Wheels[j] = (Wheels[j]-center).Normalize()*radius+center;
}
}
Look = (Wheels[0]-Wheels[3]).Normalize();
Right = (Wheels[1]-Wheels[0]).Normalize();
LastLook = Look;
LastRight = Right;
Graphics2D::Position Up = Look.Cross(Right);
CarParms->SetLook(Look, Up);
}