def solveIsolatedImpulseLambda(self,
velocityJacobian,
angJacobian,
bias=0.0):
em = Vector.dot(velocityJacobian, velocityJacobian) / self.mass
em += self.rotationVectorDotProduct(
angJacobian, (self.inverseInertia * angJacobian))
em = 1.0 / em
deviation = Vector.dot(velocityJacobian, self.velocity)
rot = self.angMomToRotMat(self.R, self.inverseInertia) * self.L
deviation += self.rotationVectorDotProduct(angJacobian, rot)
return -em * (deviation + bias)
def orthogonalize(m):
"""
Orthogonalize a matrix by applying Gram-Shmidt to
its columns in ascending order
"""
c1 = m.column(0)
c2 = m.column(1)
c3 = m.column(2)
c1.normalize()
c2 += c1 * (-Vector.dot(c2, c1))
c2.normalize()
c3 += c1 * (-Vector.dot(c3, c1))
c3 += c2 * (-Vector.dot(c3, c2))
c3.normalize()
m._e = c1._e + c2._e + c3._e