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); }
public void Normalize() { long sqr = _x * _x + _y * _y; if (sqr == 0L) { return; } long b = (long)LMath.Sqrt(sqr); this._x = (_x * LFloat.Precision / b); this._y = (_y * LFloat.Precision / b); }
/// <summary> /// 转换为角轴 /// </summary> /// <param name="angle"></param> /// <param name="axis"></param> public void ToAngleAxis(out LFloat angle, out LVector3 axis) { angle = 2 * LMath.Acos(w); if (angle == 0) { axis = LVector3.right; return; } LFloat div = 1 / LMath.Sqrt(1 - w * w); axis = new LVector3(x * div, y * div, z * div); angle = angle * 180 / LMath.PI; }
public LVector3 Normalize(LFloat newMagn) { long sqr = _x * _x + _y * _y + _z * _z; if (sqr == 0L) { return(LVector3.zero); } long b = LMath.Sqrt(sqr); _x = (_x * LFloat.Precision / b); _y = (_y * LFloat.Precision / b); _z = (_z * LFloat.Precision / b); return(this); }
public void Normalize() { long num = (long)(this._x * 100); long num2 = (long)(this._y * 100); long num3 = num * num + num2 * num2; if (num3 == 0L) { return; } long b = (long)LMath.Sqrt(num3); this._x = (int)(num * 1000L / b); this._y = (int)(num2 * 1000L / b); }
public LVector3 Normalize(LFloat newMagn) { long num = (long)(this._x * 100); long num2 = (long)(this._y * 100); long num3 = (long)(this._z * 100); long num4 = num * num + num2 * num2 + num3 * num3; if (num4 == 0L) { return(this); } long b = (long)LMath.Sqrt(num4); long num5 = newMagn._val; this._x = (int)(num * num5 / b); this._y = (int)(num2 * num5 / b); this._z = (int)(num3 * num5 / b); return(this); }
/// <summary> /// 线性插值(无限制) /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="t"></param> /// <returns></returns> public static LQuaternion LerpUnclamped(LQuaternion a, LQuaternion b, LFloat t) { LQuaternion tmpQuat = new LQuaternion(); if (Dot(a, b) < 0) { tmpQuat.Set(a.x + t * (-b.x - a.x), a.y + t * (-b.y - a.y), a.z + t * (-b.z - a.z), a.w + t * (-b.w - a.w)); } else { tmpQuat.Set(a.x + t * (b.x - a.x), a.y + t * (b.y - a.y), a.z + t * (b.z - a.z), a.w + t * (b.w - a.w)); } LFloat nor = LMath.Sqrt(Dot(tmpQuat, tmpQuat)); return(new LQuaternion(tmpQuat.x / nor, tmpQuat.y / nor, tmpQuat.z / nor, tmpQuat.w / nor)); }