public override void ApplyPositionCorrection(float deltaTime)
{
if ((objA.linearVelocity - objB.linearVelocity).sqrMagnitude <= 0)
{
LVector2 ca = objA.GetWorldCenterOfMass();
LVector2 ra = anchorA - ca;
LVector2 cb = objB.GetWorldCenterOfMass();
LVector2 rb = anchorB - cb;
LFloat pa = objA.mass / (objA.mass + objB.mass);
LVector3 dva = -(LVector3)normal * penetration * pa * ERP;
LVector3 dvb = (LVector3)normal * penetration * (1 - pa) * ERP;
LFloat dwa = LVector3.Cross(ra, dva * objA.mass).z *objA.inertiaInverse *deltaTime;
LFloat dwb = LVector3.Cross(rb, dvb * objB.mass).z *objB.inertiaInverse *deltaTime;
if (objA.rigidBody != null && !objA.rigidBody.isFixed)
{
objA.transform.position += LParser.Parse(dva);
objA.transform.eulerAngles += new Vector3(0, 0, (dwa * Mathf.Rad2Deg).ToFloat());
}
if (objB.rigidBody != null && !objB.rigidBody.isFixed)
{
objB.transform.position += LParser.Parse(dvb);
objB.transform.eulerAngles += new Vector3(0, 0, (dwb * Mathf.Rad2Deg).ToFloat());
}
}
}
public LVector2 GetWorldCenterOfMass()
{
return(transform.TransformPoint(LParser.Parse(mCenterOfMass)));
}
public LVector2 GetWorldCenterOfMass()
{
return(rigidBody != null?rigidBody.GetWorldCenterOfMass() : (LVector2)LParser.Parse(transform.position));
}