public static LMatrix33 QuaternionToMatrix(LQuaternion quat) { LMatrix33 m = new LMatrix33(); LFloat x = quat.x * 2; LFloat y = quat.y * 2; LFloat z = quat.z * 2; LFloat xx = quat.x * x; LFloat yy = quat.y * y; LFloat zz = quat.z * z; LFloat xy = quat.x * y; LFloat xz = quat.x * z; LFloat yz = quat.y * z; LFloat wx = quat.w * x; LFloat wy = quat.w * y; LFloat wz = quat.w * z; m[0] = 1 - (yy + zz); m[1] = xy + wz; m[2] = xz - wy; m[3] = xy - wz; m[4] = 1 - (xx + zz); m[5] = yz + wx; m[6] = xz + wy; m[7] = yz - wx; m[8] = 1 - (xx + yy); return(m); }
// Transform AABB a by the matrix m and translation t, // find maximum extents, and store result into AABB b. public void UpdateAABB(LMatrix33 m, LVector3 t) { LVector3 _c = c + t; LVector3 _r = r; min = max = _c; // For all three axes for (int i = 0; i < 3; i++) { // Form extent by summing smaller and larger terms respectively for (int j = 0; j < 3; j++) { LFloat e = m[i, j] * _r[j]; if (e < LFloat.zero) { min[i] += e; max[i] -= e; } else { min[i] -= e; max[i] += e; } } } }
private static LQuaternion MatrixToQuaternion(LMatrix33 m) { LQuaternion quat = new LQuaternion(); LFloat fTrace = m[0, 0] + m[1, 1] + m[2, 2]; LFloat root; if (fTrace > 0) { root = LMath.Sqrt(fTrace + 1); quat.w = LFloat.half * root; root = LFloat.half / root; quat.x = (m[2, 1] - m[1, 2]) * root; quat.y = (m[0, 2] - m[2, 0]) * root; quat.z = (m[1, 0] - m[0, 1]) * root; } else { int[] s_iNext = new int[] { 1, 2, 0 }; int i = 0; if (m[1, 1] > m[0, 0]) { i = 1; } if (m[2, 2] > m[i, i]) { i = 2; } int j = s_iNext[i]; int k = s_iNext[j]; root = LMath.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1); if (root < 0) { throw new IndexOutOfRangeException("error!"); } quat[i] = LFloat.half * root; root = LFloat.half / root; quat.w = (m[k, j] - m[j, k]) * root; quat[j] = (m[j, i] + m[i, j]) * root; quat[k] = (m[k, i] + m[i, k]) * root; } LFloat nor = LMath.Sqrt(Dot(quat, quat)); quat = new LQuaternion(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor); return(quat); }
private LVector3 MatrixToEuler(LMatrix33 m) { LVector3 v = new LVector3(); if (m[1, 2] < 1) { if (m[1, 2] > -1) { v.x = LMath.Asin(-m[1, 2]); v.y = LMath.Atan2(m[0, 2], m[2, 2]); v.z = LMath.Atan2(m[1, 0], m[1, 1]); } else { v.x = LMath.PI * LFloat.half; v.y = LMath.Atan2(m[0, 1], m[0, 0]); v.z = (LFloat)0; } } else { v.x = -LMath.PI * LFloat.half; v.y = LMath.Atan2(-m[0, 1], m[0, 0]); v.z = (LFloat)0; } for (int i = 0; i < 3; i++) { if (v[i] < 0) { v[i] += LMath.PI2; } else if (v[i] > LMath.PI2) { v[i] -= LMath.PI2; } } return(v); }
private static LMatrix33 LookRotationToMatrix(LVector3 viewVec, LVector3 upVec) { LVector3 z = viewVec; LMatrix33 m = new LMatrix33(); LFloat mag = z.magnitude; if (mag <= 0) { m = LMatrix33.identity; } z /= mag; LVector3 x = Cross(upVec, z); mag = x.magnitude; if (mag <= 0) { m = LMatrix33.identity; } x /= mag; LVector3 y = Cross(z, x); m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x; m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y; m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z; return(m); }
/// <summary> /// 注视旋转 /// </summary> /// <param name="forward"></param> /// <param name="upwards"></param> /// <returns></returns> public static LQuaternion LookRotation(LVector3 forward, LVector3 upwards) { LMatrix33 m = LookRotationToMatrix(forward, upwards); return(MatrixToQuaternion(m)); }
public static bool TestOBBOBB(OBB a, OBB b) { LFloat ra, rb; LMatrix33 R = new LMatrix33(), AbsR = new LMatrix33(); // Compute rotation matrix expressing b in a's coordinate frame for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { R[i, j] = Dot(a.u[i], b.u[j]); } } // Compute translation LVector t LVector3 t = b.c - a.c; // Bring translation into a's coordinate frame t = new LVector3(Dot(t, a.u[0]), Dot(t, a.u[1]), Dot(t, a.u[2])); // Compute common subexpressions. Add in an epsilon term to // counteract arithmetic errors when two edges are parallel and // their cross product is (near) null (see text for details) for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { AbsR[i, j] = Abs(R[i, j]) + LFloat.EPSILON; } } // Test axes L = A0, L = A1, L = A2 for (int i = 0; i < 3; i++) { ra = a.e[i]; rb = b.e[0] * AbsR[i, 0] + b.e[1] * AbsR[i, 1] + b.e[2] * AbsR[i, 2]; if (Abs(t[i]) > ra + rb) { return(false); } } // Test axes L = B0, L = B1, L = B2 for (int i = 0; i < 3; i++) { ra = a.e[0] * AbsR[0, i] + a.e[1] * AbsR[1, i] + a.e[2] * AbsR[2, i]; rb = b.e[i]; if (Abs(t[0] * R[0, i] + t[1] * R[1, i] + t[2] * R[2, i]) > ra + rb) { return(false); } } // Test axis L = A0 x B0 ra = a.e[1] * AbsR[2, 0] + a.e[2] * AbsR[1, 0]; rb = b.e[1] * AbsR[0, 2] + b.e[2] * AbsR[0, 1]; if (Abs(t[2] * R[1, 0] - t[1] * R[2, 0]) > ra + rb) { return(false); } // Test axis L = A0 x B1 ra = a.e[1] * AbsR[2, 1] + a.e[2] * AbsR[1, 1]; rb = b.e[0] * AbsR[0, 2] + b.e[2] * AbsR[0, 0]; if (Abs(t[2] * R[1, 1] - t[1] * R[2, 1]) > ra + rb) { return(false); } // Test axis L = A0 x B2 ra = a.e[1] * AbsR[2, 2] + a.e[2] * AbsR[1, 2]; rb = b.e[0] * AbsR[0, 1] + b.e[1] * AbsR[0, 0]; if (Abs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb) { return(false); } // Test axis L = A1 x B0 ra = a.e[0] * AbsR[2, 0] + a.e[2] * AbsR[0, 0]; rb = b.e[1] * AbsR[1, 2] + b.e[2] * AbsR[1, 1]; if (Abs(t[0] * R[2, 0] - t[2] * R[0, 0]) > ra + rb) { return(false); } // Test axis L = A1 x B1 ra = a.e[0] * AbsR[2, 1] + a.e[2] * AbsR[0, 1]; rb = b.e[0] * AbsR[1, 2] + b.e[2] * AbsR[1, 0]; if (Abs(t[0] * R[2, 1] - t[2] * R[0, 1]) > ra + rb) { return(false); } // Test axis L = A1 x B2 ra = a.e[0] * AbsR[2, 2] + a.e[2] * AbsR[0, 2]; rb = b.e[0] * AbsR[1, 1] + b.e[1] * AbsR[1, 0]; if (Abs(t[0] * R[2, 2] - t[2] * R[0, 2]) > ra + rb) { return(false); } // Test axis L = A2 x B0 ra = a.e[0] * AbsR[1, 0] + a.e[1] * AbsR[0, 0]; rb = b.e[1] * AbsR[2, 2] + b.e[2] * AbsR[2, 1]; if (Abs(t[1] * R[0, 0] - t[0] * R[1, 0]) > ra + rb) { return(false); } // Test axis L = A2 x B1 ra = a.e[0] * AbsR[1, 1] + a.e[1] * AbsR[0, 1]; rb = b.e[0] * AbsR[2, 2] + b.e[2] * AbsR[2, 0]; if (Abs(t[1] * R[0, 1] - t[0] * R[1, 1]) > ra + rb) { return(false); } // Test axis L = A2 x B2 ra = a.e[0] * AbsR[1, 2] + a.e[1] * AbsR[0, 2]; rb = b.e[0] * AbsR[2, 1] + b.e[1] * AbsR[2, 0]; if (Abs(t[1] * R[0, 2] - t[0] * R[1, 2]) > ra + rb) { return(false); } // Since no separating axis found, the OBBs must be intersecting return(true); }