func RotationBetweenNormals(n1, n2 glm.Vec4d) glm.Mat4d {
axis := Cross3D(n1, n2)
dot := n1.Dot(n2)
if !NearZero(axis) {
angle := math.Acos(dot)
return glm.HomogRotate3Dd(angle, axis.Normalize())
} else if dot < 0 {
for e := 0; e < 3; e++ {
v := glm.Vec4d{}
v[e] = 1
cross := Cross3D(n1, v)
if !NearZero(cross) {
return glm.HomogRotate3Dd(math.Pi, cross.Normalize())
}
}
panic(fmt.Sprintln("no orthogonal axis found for normal", n1))
}
return glm.Ident4d()
}
func NearZero(v glm.Vec4d) bool {
return v.ApproxEqual(glm.Vec4d{})
}
func (car *Car) Simulate(controls Controls, timestep float64) {
p := car.Center
v := car.Velocity
u := car.Direction
at := car.TransientAcceleration
rwav := car.RearWheelAngularVelocity
m := car.Profile.Mass
d := car.Profile.Drag
rr := car.Profile.RollingResistance
h := car.Profile.CenterOfGravityHeight
fl := car.Profile.FrontAxelDisplacement
rl := car.Profile.RearAxelDisplacement
xg := car.Profile.GearRatio
xd := car.Profile.DifferentialRatio
te := car.Profile.TransmissionEfficiency
wr := car.Profile.WheelRadius
wm := car.Profile.WheelMass
bmax := car.Profile.BreakingPower
bmaxToruqe := bmax / wr
mu := car.Profile.TyreFrictionMu
tc := car.Profile.TyreTractionConstant
g := Gravity
dt := timestep
vmag := v.Len()
staticWeight := g * m
axelDisplacement := fl + rl
// maxFrontTyreTraction := rl / axelDisplacement * weight - (h-wr)/axelLength * m * at.Dot(u)
dynamicWeight := (h - wr) / axelDisplacement * m * at.Dot(u)
weight := fl/axelDisplacement*staticWeight + dynamicWeight
maxRearTyreTraction := mu * weight
freeRollingAV := v.Dot(u) / wr
rwav = freeRollingAV
slipRatio := 0.0
if !IsZero(v) {
slipRatio = (rwav*wr - v.Dot(u)) / vmag
}
// else {
// // slipRatio = rwav * wr //-rwav * wr
// }
tractionForce := math.Min(tc*slipRatio, maxRearTyreTraction)
tractionTorque := tractionForce * wr
// fmt.Println("tyre traction force", tractionForce)
vn := glm.Vec4d{}
if !IsZero(v) {
vn = v.Normalize()
}
brakeTorque := bmaxToruqe * vn.Dot(u) * controls.BreakPedal
rpm := freeRollingAV * xg * xd * 60 / (2 * math.Pi)
engineTorque := car.Profile.Engine.Torque(rpm)
appliedTorque := engineTorque * controls.FuelPedal
driveTorque := appliedTorque * xg * xd * te
driveForce := driveTorque / wr
totalTorque := driveTorque - 2*tractionTorque - brakeTorque
// fmt.Println("wrav", rwav)
// fmt.Println("driveTorque", driveTorque)
// fmt.Println("tractionTorque", tractionTorque)
// fmt.Println("brakeTorque", brakeTorque)
// fmt.Println("totalTorque", totalTorque)
b := bmax * controls.BreakPedal
rearForceTraction := math.Copysign(math.Min(math.Abs(driveForce), maxRearTyreTraction), driveForce)
forceTraction := u.Mul(rearForceTraction)
forceBreaking := u.Mul(-b * vn.Dot(u))
forceDrag := v.Mul(-d * vmag)
forceRollingResistance := v.Mul(-rr)
force := forceTraction.Add(forceDrag).Add(forceRollingResistance).Add(forceBreaking)
rearWheelInertia := wm * wr * wr / 2
wheelAcceleration := totalTorque / rearWheelInertia
drwav := wheelAcceleration * dt
a := force.Mul(1.0 / m)
dv := a.Mul(dt)
dp := v.Mul(dt).Add(a.Mul(dt * dt * 0.5))
if v.Len() < forceBreaking.Len()*dt/m {
dv = v.Mul(-1)
}
// fmt.Println(rwav, dv)
p = p.Add(dp)
v = v.Add(dv)
rwav = rwav + drwav
rwav = v.Dot(u) / wr
car.FrontWheelO = car.FrontWheelO + controls.WheelAngularVelocity*dt
car.FrontWheelO = math.Copysign(math.Min(math.Abs(car.FrontWheelO), car.Profile.MaxStreeringAngle), car.FrontWheelO)
if math.Abs(car.FrontWheelO) > 0.0001 {
turningRadius := axelDisplacement / math.Sin(car.FrontWheelO)
angularVelocity := v.Dot(u) / turningRadius
rotation := dt * angularVelocity
m := glm.HomogRotate3DZd(-rotation * 180 / math.Pi)
u = m.Mul4x1(u)
v = m.Mul4x1(v)
}
car.Center = p
car.Velocity = v
car.TransientAcceleration = a
car.RearWheelAngularVelocity = rwav
car.Direction = u
car.FrontWheelAngularDeviation += freeRollingAV * dt
car.RearWheelAngularDeviation += car.RearWheelAngularVelocity * dt
}
