/// <summary> /// 球形插值(无限制) /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="t"></param> /// <returns></returns> public static LQuaternion SlerpUnclamped(LQuaternion q1, LQuaternion q2, LFloat t) { LFloat dot = Dot(q1, q2); LQuaternion tmpQuat = new LQuaternion(); if (dot < 0) { dot = -dot; tmpQuat.Set(-q2.x, -q2.y, -q2.z, -q2.w); } else { tmpQuat = q2; } if (dot < 1) { LFloat angle = LMath.Acos(dot); LFloat sinadiv, sinat, sinaomt; sinadiv = 1 / LMath.Sin(angle); sinat = LMath.Sin(angle * t); sinaomt = LMath.Sin(angle * (1 - t)); tmpQuat.Set((q1.x * sinaomt + tmpQuat.x * sinat) * sinadiv, (q1.y * sinaomt + tmpQuat.y * sinat) * sinadiv, (q1.z * sinaomt + tmpQuat.z * sinat) * sinadiv, (q1.w * sinaomt + tmpQuat.w * sinat) * sinadiv); return(tmpQuat); } else { return(Lerp(q1, tmpQuat, t)); } }
/// <summary> /// <para>Clamps the Vector2Int to the bounds given by min and max.</para> /// </summary> /// <param name="min"></param> /// <param name="max"></param> public void Clamp(LVector2Int min, LVector2Int max) { this.x = LMath.Max(min.x, this.x); this.x = LMath.Min(max.x, this.x); this.y = LMath.Max(min.y, this.y); this.y = LMath.Min(max.y, this.y); }
/// <summary> /// /// </summary> /// <param name="deg"> 弧度</param> /// <returns></returns> public LVector2 Rotate(LFloat deg) { var rad = LMath.Deg2Rad * deg; LFloat cos, sin; LMath.SinCos(out cos, out sin, rad); return(new LVector2(x * cos - y * sin, x * sin + y * cos)); }
public static int Sqrt(int a) { if (a <= 0) { return(0); } return((int)LMath.Sqrt32((uint)a)); }
public static int Sqrt(long a) { if (a <= 0L) { return(0); } if (a <= (long)(0xffffffffu)) { return((int)LMath.Sqrt32((uint)a)); } return((int)LMath.Sqrt64((ulong)a)); }
/// <summary> /// 3*3 矩阵转化为四元数 /// </summary> /// <param name="m"></param> /// <returns></returns> 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 LVector3 RotateY(LFloat degree) { LFloat s; LFloat c; LMath.SinCos(out s, out c, new LFloat(true, degree._val * 31416L / 1800000L)); LVector3 vInt; vInt._x = (int)(((long)this._x * s._val + (long)this._z * c._val) / LFloat.Precision); vInt._z = (int)(((long)this._x * -c._val + (long)this._z * s._val) / LFloat.Precision); vInt._y = 0; return(vInt.normalized); }
/// <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 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); }
/// <summary> /// 向目标角度旋转 /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <param name="maxDegreesDelta"></param> /// <returns></returns> public static LQuaternion RotateTowards(LQuaternion from, LQuaternion to, LFloat maxDegreesDelta) { LFloat num = LQuaternion.Angle(from, to); LQuaternion result = new LQuaternion(); if (num == 0) { result = to; } else { LFloat t = LMath.Min(LFloat.one, maxDegreesDelta / num); result = LQuaternion.SlerpUnclamped(from, to, t); } return(result); }
/// <summary> /// 轴向旋转 /// </summary> /// <param name="angle"></param> /// <param name="axis"></param> /// <returns></returns> public static LQuaternion AngleAxis(LFloat angle, LVector3 axis) { axis = axis.normalized; angle = angle * LMath.Deg2Rad; LQuaternion q = new LQuaternion(); LFloat halfAngle = angle * LFloat.half; LFloat s = LMath.Sin(halfAngle); q.w = LMath.Cos(halfAngle); q.x = s * axis.x; q.y = s * axis.y; q.z = s * axis.z; return(q); }
/// <summary> /// 欧拉角转四元数 /// </summary> /// <param name="x"></param> /// <param name="y"></param> /// <param name="z"></param> /// <returns></returns> public static LQuaternion Euler(LFloat x, LFloat y, LFloat z) { LFloat cX = LMath.Cos(x * LMath.PI / 360); LFloat sX = LMath.Sin(x * LMath.PI / 360); LFloat cY = LMath.Cos(y * LMath.PI / 360); LFloat sY = LMath.Sin(y * LMath.PI / 360); LFloat cZ = LMath.Cos(z * LMath.PI / 360); LFloat sZ = LMath.Sin(z * LMath.PI / 360); LQuaternion qX = new LQuaternion(sX, LFloat.zero, LFloat.zero, cX); LQuaternion qY = new LQuaternion(LFloat.zero, sY, LFloat.zero, cY); LQuaternion qZ = new LQuaternion(LFloat.zero, LFloat.zero, sZ, cZ); LQuaternion q = (qY * qX) * qZ; return(q); }
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); }
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); }
/// <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)); }
public static LFloat Sqrt(LFloat val) { return(LMath.Sqrt(val)); }
/// <summary> /// 夹角大小 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <returns></returns> public static LFloat Angle(LQuaternion a, LQuaternion b) { LFloat single = Dot(a, b); return(LMath.Acos(LMath.Min(LMath.Abs(single), LFloat.one)) * 2 * (180 / LMath.PI)); }
public static LVector2 Max(LVector2 a, LVector2 b) { return(new LVector2(true, LMath.Max(a._x, b._x), LMath.Max(a._y, b._y))); }
public void Max(ref LVector2 r) { this._x = LMath.Max(this._x, r._x); this._y = LMath.Max(this._y, r._y); }