Golang lin.V3类代码示例

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
}

// 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
}

// 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
}

// 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
//.........这里部分代码省略.........

// 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)
}