Beispiel #1
0
func (s ssort) Dtoc(v *lin.V3) float64 {
	normal := &lin.V3{s.x, s.y, 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
}
Beispiel #2
0
// 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
}