public static LQuaternion FromToRotation(LVector3 fromDirection, LVector3 toDirection) { LFloat angle = LVector3.Angle(fromDirection, toDirection); LVector3 axis = fromDirection.Cross(toDirection); return(AngleAxis(angle, axis)); }
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 override void ApplyConstraintImpluse(LFloat deltaTime) { LVector2 ca = objA.GetWorldCenterOfMass(); LVector2 ra = anchorA - ca; LVector2 cb = objB.GetWorldCenterOfMass(); LVector2 rb = anchorB - cb; // 1 X 6 LMatrix jacobian = new LMatrix(new LFloat[] { -normal.x, -normal.y, -LVector3.Cross(ra, normal).z, normal.x, normal.y, LVector3.Cross(rb, normal).z }, 1); // 6 X 1 LMatrix jacobian_t = jacobian.Transpose(); // 6 X 6 LMatrix massMatrixInverse = new LMatrix(6, 6); massMatrixInverse[0][0] = objA.massInverse; massMatrixInverse[1][1] = objA.massInverse; massMatrixInverse[2][2] = objA.inertiaInverse; massMatrixInverse[3][3] = objB.massInverse; massMatrixInverse[4][4] = objB.massInverse; massMatrixInverse[5][5] = objB.inertiaInverse; // 6 X 1 LMatrix v = new LMatrix(new LFloat[] { objA.linearVelocity.x, objA.linearVelocity.y, objA.angularVelocity, objB.linearVelocity.x, objB.linearVelocity.y, objB.angularVelocity, }, 6); //GeneralMatrix m1 = jacobian * massMatrixInverse; //GeneralMatrix m2 = jacobian_t; //double[][] m1s = m1.Array; //Debug.Log(m1s[0][0] + " " + m1s[0][1] + " " + m1s[0][2] + " " + m1s[0][3] + " " + m1s[0][4] + " " + m1s[0][5]); //double[][] m2s = m1.Array; //Debug.Log(m2s[0][0] + " " + m2s[1][0] + " " + m2s[2][0] + " " + m2s[3][0] + " " + m2s[4][0] + " " + m2s[5][0]); LMatrix lamdaMatrix = (jacobian * v * -1) * (jacobian * massMatrixInverse * jacobian_t).Inverse(); LFloat lamda = lamdaMatrix[0][0]; // 6 X 1 LMatrix dv = massMatrixInverse * jacobian_t * lamda; objA.linearVelocity += new LVector2(dv[0][0], dv[1][0]); objA.angularVelocity += dv[2][0]; objB.linearVelocity += new LVector2(dv[3][0], dv[4][0]); objB.angularVelocity += dv[5][0]; }
public void AddForceAtPosition(LVector2 force, LVector2 position, LForceMode2D forceMode) { AddForce(force, forceMode); switch (forceMode) { case LForceMode2D.Force: { mUserTorque += LVector3.Cross(position - GetWorldCenterOfMass(), force).z; break; } case LForceMode2D.Impulse: { mAngularVelocity += LVector3.Cross(position - GetWorldCenterOfMass(), force).z *mInertiaInverse; break; } } }
private void TestLVector3() { LVector3 v1 = new LVector3(3.3d, 1.2d, 1.5d); LVector3 v2 = new LVector3(1.2d, 3.3d, 1.5d); AssertTrue(v1 != v2); AssertFalse(v1 == v2); AssertTrue(v1 + v2 == new LVector3(4.5d, 4.5d, 3)); AssertTrue(v1 - v2 == new LVector3(2.1d, -2.1d, 0)); AssertTrue(v1 * 2 == new LVector3(6.6d, 2.4d, 3)); AssertTrue(v1 / 3 == new LVector3(1.1d, 0.4d, 0.5d)); AssertTrue(LVector3.zero.normalized == LVector3.zero); AssertTrue(LVector3.Dot(v1, v2) == 10.17d); AssertTrue(LVector3.Dot(LVector3.Cross(v1, v2), v2) == 0); Debug.Log(v1.sqrMagnitude); Debug.Log(v1.magnitude); Debug.Log(v1.normalized); Debug.Log("----------------------"); }
public LVector3 Cross(LVector3 v) { return(LVector3.Cross(this, v)); }
public static LQuaternion LookRotation(LVector3 forward, LVector3 upwards) { return(LookRotation(forward.Cross(upwards).Cross(upwards))); }