//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles public FixQuaternion(FixVec3 e) { w = FixMath.Cos(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Cos(e._x / 2) + FixMath.Sin(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Sin(e._x / 2); x = FixMath.Cos(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Sin(e._x / 2) - FixMath.Sin(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Cos(e._x / 2); y = FixMath.Sin(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Sin(e._x / 2) + FixMath.Cos(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Cos(e._x / 2); z = FixMath.Sin(e._z / 2) * FixMath.Cos(e._y / 2) * FixMath.Cos(e._x / 2) - FixMath.Cos(e._z / 2) * FixMath.Sin(e._y / 2) * FixMath.Sin(e._x / 2); }
//https://math.stackexchange.com/questions/2975109/how-to-convert-euler-angles-to-quaternions-and-get-the-same-euler-angles-back-fr public FixQuaternion(FixVec3 e) { x = FixMath.Cos(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Sin(e.x / 2) - FixMath.Sin(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Cos(e.x / 2); y = FixMath.Sin(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Sin(e.x / 2) + FixMath.Cos(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Cos(e.x / 2); z = FixMath.Sin(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Cos(e.x / 2) - FixMath.Cos(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Sin(e.x / 2); w = FixMath.Cos(e.z / 2) * FixMath.Cos(e.y / 2) * FixMath.Cos(e.x / 2) + FixMath.Sin(e.z / 2) * FixMath.Sin(e.y / 2) * FixMath.Sin(e.x / 2); }
public FixTrans2(FixVec2 position, FixVec2 scale, Fix rotation) { Fix cos = FixMath.Cos(rotation); Fix sin = FixMath.Sin(rotation); _m11 = cos * scale.X; _m12 = -sin * scale.X; _m13 = position.X; _m21 = sin * scale.Y; _m22 = cos * scale.Y; _m23 = position.Y; }
public static FixTrans2 MakeRotation(Fix degrees) { Fix cos = FixMath.Cos(degrees); Fix sin = FixMath.Sin(degrees); return(new FixTrans2( cos, -sin, 0, sin, cos, 0 )); }
public static FixTrans3 MakeRotationX(Fix degrees) { Fix cos = FixMath.Cos(degrees); Fix sin = FixMath.Sin(degrees); return(new FixTrans3( 1, 0, 0, 0, 0, cos, -sin, 0, 0, sin, cos, 0, 0, 0, 0, 1 )); }
//https://gamedev.stackexchange.com/questions/50963/how-to-extract-euler-angles-from-transformation-matrix public FixVec3 EulerAngle() { FixVec3 ea = new FixVec3(); ea.x = FixMath.Atan2(-m[1, 2], m[2, 2]); Fix cosYangle = FixMath.Sqrt(FixMath.Pow(m[0, 0], 2) + FixMath.Pow(m[0, 1], 2)); ea.y = FixMath.Atan2(m[0, 2], cosYangle); Fix sinXangle = FixMath.Sin(ea.x); Fix cosXangle = FixMath.Cos(ea.x); ea.z = FixMath.Atan2((cosXangle * m[1, 0]) + (sinXangle * m[2, 0]), (cosXangle * m[1, 1]) + (sinXangle * m[2, 1])); return(ea); }
/// <summary> /// Build a quaternion from the given axis and angle /// </summary> /// <param name="axis">The axis to rotate about</param> /// <param name="angle">The rotation angle in radians</param> /// <returns></returns> public static FixQuaternion FromAxisAngle(FixVec3 axis, Fix angle) { if (axis.GetSqrMagnitude() == Fix.Zero) { return(Identity); } FixQuaternion result = Identity; angle *= Fix.One / 2; axis.Normalize(); result.Xyz = axis * FixMath.Sin(angle); result.W = FixMath.Cos(angle); return(Normalize(result)); }
/// <summary> /// Do Spherical linear interpolation between two quaternions /// </summary> /// <param name="q1">The first quaternion</param> /// <param name="q2">The second quaternion</param> /// <param name="blend">The blend factor</param> /// <returns>A smooth blend between the given quaternions</returns> public static FixQuaternion Slerp(FixQuaternion q1, FixQuaternion q2, Fix blend) { // if either input is zero, return the other. if (q1.LengthSquared == Fix.Zero) { if (q2.LengthSquared == Fix.Zero) { return(Identity); } return(q2); } else if (q2.LengthSquared == Fix.Zero) { return(q1); } Fix cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz); if (cosHalfAngle >= Fix.One || cosHalfAngle <= -Fix.One) { // angle = 0.0f, so just return one input. return(q1); } else if (cosHalfAngle < Fix.Zero) { q2.Xyz = -q2.Xyz; q2.W = -q2.W; cosHalfAngle = -cosHalfAngle; } Fix blendA; Fix blendB; if (cosHalfAngle < (Fix.One / 100) * 99) { // do proper slerp for big angles Fix halfAngle = FixMath.Acos(cosHalfAngle); Fix sinHalfAngle = FixMath.Sin(halfAngle); Fix oneOverSinHalfAngle = Fix.One / sinHalfAngle; blendA = FixMath.Sin(halfAngle * (Fix.One - blend)) * oneOverSinHalfAngle; blendB = FixMath.Sin(halfAngle * blend) * oneOverSinHalfAngle; } else { // do lerp if angle is really small. blendA = Fix.One - blend; blendB = blend; } FixQuaternion result = new FixQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W); if (result.LengthSquared > Fix.Zero) { return(Normalize(result)); } else { return(Identity); } }