func (s ssort) Dtoc(v *lin.V3) float64 {
normal := &lin.V3{X: s.x, Y: s.y, Z: s.z}
dot := v.Dot(normal)
dx := normal.X * dot
dy := normal.Y * dot
dz := normal.Z * dot
return dx*dx + dy*dy + dz*dz
}
// setupFrictionConstraint initializes contact based constraints.
// Expected to be called on solver setup for each point of contact.
func (sol *solver) setupFrictionConstraint(sc *solverConstraint, normalAxis *lin.V3, sbodA, sbodB *solverBody,
sp *solverPoint, relPosA, relPosB *lin.V3) {
bodyA, bodyB := sbodA.oBody, sbodB.oBody // either may be nil if body is static.
sc.sbodA, sc.sbodB = sbodA, sbodB
sc.normal.Set(normalAxis)
sc.friction = sp.combinedFriction
sc.oPoint = nil
sc.appliedImpulse = 0.0
sc.appliedPushImpulse = 0.0
// compute torque
ftorqueAxis := sc.relpos1CrossNormal.Cross(relPosA, sc.normal)
sc.angularComponentA.SetS(0, 0, 0)
if bodyA != nil {
sc.angularComponentA.MultMv(bodyA.iitw, ftorqueAxis)
}
{ // scratch v0
ftorqueAxis = sc.relpos2CrossNormal.Cross(relPosB, sol.v0.Neg(sc.normal))
} // scratch v0 free
sc.angularComponentB.SetS(0, 0, 0)
if bodyB != nil {
sc.angularComponentB.MultMv(bodyB.iitw, ftorqueAxis)
}
// compute sc.jacDiagABInv
denom0, denom1 := 0.0, 0.0
if bodyA != nil { // scratch v0
sol.v0.Cross(sc.angularComponentA, relPosA)
denom0 = bodyA.imass + normalAxis.Dot(sol.v0)
} // scratch v0 free
if bodyB != nil { // scratch v0, v1
sol.v0.Cross(sol.v1.Neg(sc.angularComponentB), relPosB)
denom1 = bodyB.imass + normalAxis.Dot(sol.v0)
} // scratch v0, v1 free
relaxation := 1.0
sc.jacDiagABInv = relaxation / (denom0 + denom1)
// compute limits.
vel1Dotn, vel2Dotn := 0.0, 0.0
if bodyA != nil {
vel1Dotn = sc.normal.Dot(sbodA.linearVelocity) + sc.relpos1CrossNormal.Dot(sbodA.angularVelocity)
}
if bodyB != nil { // scratch v0
vel2Dotn = sol.v0.Neg(sc.normal).Dot(sbodB.linearVelocity) + sc.relpos2CrossNormal.Dot(sbodB.angularVelocity)
} // scratch v0 free
velocityError := -(vel1Dotn + vel2Dotn) // negative relative velocity
sc.rhs = velocityError * sc.jacDiagABInv // velocity impulse
sc.cfm = 0
sc.lowerLimit = 0
sc.upperLimit = 1e10
sc.rhsPenetration = 0
}
// getVelocityInLocalPoint updates vector v to be the linear and angular
// velocity of this body at the given point. The point is expected to be
// in local coordinate space.
func (b *body) getVelocityInLocalPoint(localPoint, v *lin.V3) *lin.V3 {
return v.Cross(b.avel, localPoint).Add(v, b.lvel)
}
// applyImpulse updates the linear and angular velocity change needed to
// solve constraints.
func (sb *solverBody) applyImpulse(linearComponent, angularComponent *lin.V3, impulseMagnitude float64) {
if sb.oBody != nil {
sb.deltaLinearVelocity.Add(sb.deltaLinearVelocity, linearComponent.Scale(linearComponent, impulseMagnitude))
sb.deltaAngularVelocity.Add(sb.deltaAngularVelocity, angularComponent.Scale(angularComponent, impulseMagnitude))
}
}
// applyPushImpulse updates the push and turn velocity used to separate
// inter-penetrating bodies.
func (sb *solverBody) applyPushImpulse(linearComponent, angularComponent *lin.V3, impulseMagnitude float64) {
if sb.oBody != nil {
sb.pushVelocity.Add(sb.pushVelocity, linearComponent.Scale(linearComponent, impulseMagnitude))
sb.turnVelocity.Add(sb.turnVelocity, angularComponent.Scale(angularComponent, impulseMagnitude))
}
}
// setupContactConstraint initializes contact based constraints.
// Expected to be called on solver setup for each contact point.
func (sol *solver) setupContactConstraint(sc *solverConstraint, sbodA, sbodB *solverBody,
poc *pointOfContact, info *solverInfo, relPosA, relPosB, vel *lin.V3) (relativeVelocity float64) {
bodyA, bodyB := sbodA.oBody, sbodB.oBody // either may be nil if body is static.
{ // scratch v0, v1
torqueAxis0 := sol.v0.Cross(relPosA, poc.sp.normalWorldB)
sc.angularComponentA.SetS(0, 0, 0)
if bodyA != nil {
sc.angularComponentA.MultMv(bodyA.iitw, torqueAxis0)
}
torqueAxis1 := sol.v1.Cross(relPosB, poc.sp.normalWorldB)
sc.angularComponentB.SetS(0, 0, 0)
if bodyB != nil { // scratch v2
sc.angularComponentB.MultMv(bodyB.iitw, sol.v2.Neg(torqueAxis1))
} // scratch v2 free
denom0, denom1 := 0.0, 0.0
if bodyA != nil { // scratch v2
vec := sol.v2.Cross(sc.angularComponentA, relPosA)
denom0 = bodyA.imass + poc.sp.normalWorldB.Dot(vec)
} // scratch v2 free
if bodyB != nil { // scratch v2
sol.v2.Neg(sc.angularComponentB).Cross(sol.v2, relPosB)
denom1 = bodyB.imass + poc.sp.normalWorldB.Dot(sol.v2)
} // scratch v2 free
relaxation := 1.0
sc.jacDiagABInv = relaxation / (denom0 + denom1)
sc.normal.Set(poc.sp.normalWorldB)
sc.relpos1CrossNormal.Set(torqueAxis0)
sc.relpos2CrossNormal.Neg(torqueAxis1)
} // scratch v0, v1 free
// Calculate penetration, friction, and restitution.
penetration := poc.sp.distance + info.linearSlop
{ // scratch v0, v1
v0, v1 := sol.v0.SetS(0, 0, 0), sol.v1.SetS(0, 0, 0)
if bodyA != nil {
bodyA.getVelocityInLocalPoint(relPosA, v0)
}
if bodyB != nil {
bodyB.getVelocityInLocalPoint(relPosB, v1)
}
vel.Sub(v0, v1)
} // scratch v0, v1 free
sc.friction = poc.sp.combinedFriction
relativeVelocity = poc.sp.normalWorldB.Dot(vel)
restitution := poc.sp.combinedRestitution * -relativeVelocity
if restitution <= 0.0 {
restitution = 0.0
}
// Warm start uses the previously applied impulse as an initial guess.
sc.appliedImpulse = poc.sp.warmImpulse * info.warmstartingFactor
{ // scratch v0, v1
linc, angc := sol.v0, sol.v1
if bodyA != nil {
sbodA.applyImpulse(linc.Scale(sc.normal, bodyA.imass), angc.Set(sc.angularComponentA), sc.appliedImpulse)
}
if bodyB != nil {
sbodB.applyImpulse(linc.Scale(sc.normal, bodyB.imass), angc.Neg(sc.angularComponentB), -sc.appliedImpulse)
}
} // scratch v0, v1 free
sc.appliedPushImpulse = 0.0
velocityError := 0.0
vel1Dotn, vel2Dotn := 0.0, 0.0
if bodyA != nil {
vel1Dotn = sc.normal.Dot(sbodA.linearVelocity) + sc.relpos1CrossNormal.Dot(sbodA.angularVelocity)
}
if bodyB != nil { // scratch v0
vel2Dotn = sol.v0.Neg(sc.normal).Dot(sbodB.linearVelocity) + sc.relpos2CrossNormal.Dot(sbodB.angularVelocity)
} // scratch v0 free
velocityError = restitution - (vel1Dotn + vel2Dotn)
erp := info.erp2
if !info.splitImpulse || (penetration > info.splitImpulsePenetrationLimit) {
erp = info.erp
}
positionalError := 0.0
if penetration > 0 {
velocityError -= penetration / info.timestep
} else {
positionalError = -penetration * erp / info.timestep
}
penetrationImpulse := positionalError * sc.jacDiagABInv
velocityImpulse := velocityError * sc.jacDiagABInv
if !info.splitImpulse || penetration > info.splitImpulsePenetrationLimit {
// combine position and velocity into rhs
sc.rhs = penetrationImpulse + velocityImpulse
sc.rhsPenetration = 0.0
} else {
// split position and velocity into rhs and m_rhsPenetration
sc.rhs = velocityImpulse
sc.rhsPenetration = penetrationImpulse
}
sc.cfm = 0
sc.lowerLimit = 0
sc.upperLimit = 1e10
return relativeVelocity
}
// Implements Shape.Inertia
func (s *sphere) Inertia(mass float64, inertia *lin.V3) *lin.V3 {
elem := 0.4 * mass * s.R * s.R
inertia.SetS(elem, elem, elem)
return inertia
}
// Implements Shape.Inertia
func (b *box) Inertia(mass float64, inertia *lin.V3) *lin.V3 {
lx2, ly2, lz2 := 4.0*b.Hx*b.Hx, 4.0*b.Hy*b.Hy, 4.0*b.Hz*b.Hz
inertia.SetS(mass/12.0*(ly2+lz2), mass/12.0*(lx2+lz2), mass/12.0*(lx2+ly2))
return inertia
}