Example #1
0
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()
}
Example #2
0
func NearZero(v glm.Vec4d) bool {
	return v.ApproxEqual(glm.Vec4d{})
}
Example #3
0
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
}