public void InitVelocityConstraint(float dt)
{
var body = GetComponent <PhysicsBody>();
float pbc;
ConstraintUtil.VelocityConstraintBias(body.Mass, ConstraintParams, dt, out pbc, out m_sbc);
Vector3 r = transform.rotation * GetLocalAnchor();
Vector3 cPos = (transform.position + r) - GetTarget();
Vector3 cVel = body.LinearVelocity + Vector3.Cross(body.AngularVelocity, r);
m_cross = Matrix3x3.PostCross(r);
Matrix3x3 k = body.InverseMass * Matrix3x3.Identity;
if (EnableRotation)
{
k += m_cross * body.InverseInertiaWs * m_cross.Transposed;
}
k += m_sbc * Matrix3x3.Identity;
m_positionErrorBias = pbc * cPos;
m_effectiveMass = k.Inverted;
// TODO: warm starting
m_lambda = Vector3.zero;
}
public void InitVelocityConstraint(float dt)
{
var body = GetComponent <PhysicsBody>();
float pbc;
ConstraintUtil.VelocityConstraintBias(body.Mass, ConstraintParams, dt, out pbc, out sbc);
m_n = (Plane != null) ? Plane.transform.up : Vector3.up;
m_p = (Plane != null) ? Plane.transform.position : transform.position;
m_d = Vector3.Dot(transform.position - m_p, m_n) - Offset;
if (m_d > 0.0f)
{
return;
}
Vector3 cPos = m_d * m_n;
m_positionErrorBias = pbc * cPos + Restitution * Vector3.Project(-body.LinearVelocity, m_n);
m_effectiveMass = 1.0f / (body.InverseMass + sbc);
// TODO: warm starting
m_impulse = Vector3.zero;
}