public static CQuaternion angle2Quat(double r1, double r2, double r3, int type) { double[] ang = { r1, r2, r3 }; double[] cang = { System.Math.Cos(ang[0] / 2), System.Math.Cos(ang[1] / 2), System.Math.Cos(ang[2] / 2) }; double[] sang = { System.Math.Sin(ang[0] / 2), System.Math.Sin(ang[1] / 2), System.Math.Sin(ang[2] / 2) }; CQuaternion q = new CQuaternion(); switch (type) { case 1: //'zyx' q.m_q0 = cang[0] * cang[1] * cang[2] + sang[0] * sang[1] * sang[2]; q.m_q1 = cang[0] * cang[1] * sang[2] - sang[0] * sang[1] * cang[2]; q.m_q2 = cang[0] * sang[1] * cang[2] + sang[0] * cang[1] * sang[2]; q.m_q3 = sang[0] * cang[1] * cang[2] + cang[0] * sang[1] * sang[2]; break; case 2: //'zxy' q.m_q0 = cang[0] * cang[1] * cang[2] - sang[0] * sang[1] * sang[2]; q.m_q1 = cang[0] * sang[1] * cang[2] - sang[0] * cang[1] * sang[2]; q.m_q2 = cang[0] * cang[1] * sang[2] + sang[0] * sang[1] * cang[2]; q.m_q3 = cang[0] * sang[1] * sang[2] + sang[0] * cang[1] * cang[2]; break; case 3: //'yxz' q.m_q0 = cang[0] * cang[1] * cang[2] + sang[0] * sang[1] * sang[2]; q.m_q1 = cang[0] * sang[1] * cang[2] + sang[0] * cang[1] * sang[2]; q.m_q2 = sang[0] * cang[1] * cang[2] - cang[0] * sang[1] * sang[2]; q.m_q3 = cang[0] * cang[1] * sang[2] - sang[0] * sang[1] * cang[2]; break; case 4: //'yzx' q.m_q0 = cang[0] * cang[1] * cang[2] - sang[0] * sang[1] * sang[2]; q.m_q1 = cang[0] * cang[1] * sang[2] + sang[0] * sang[1] * cang[2]; q.m_q2 = cang[0] * sang[1] * sang[2] + sang[0] * cang[1] * cang[2]; q.m_q3 = cang[0] * sang[1] * cang[2] - sang[0] * cang[1] * sang[2]; break; case 5: //'xyz' q.m_q0 = cang[0] * cang[1] * cang[2] - sang[0] * sang[1] * sang[2]; q.m_q1 = cang[0] * sang[1] * sang[2] + sang[0] * cang[1] * cang[2]; q.m_q2 = cang[0] * sang[1] * cang[2] - sang[0] * cang[1] * sang[2]; q.m_q3 = cang[0] * cang[1] * sang[2] + sang[0] * sang[1] * cang[2]; break; case 6: //'xzy' q.m_q0 = cang[0] * cang[1] * cang[2] + sang[0] * sang[1] * sang[2]; q.m_q1 = sang[0] * cang[1] * cang[2] - cang[0] * sang[1] * sang[2]; q.m_q2 = cang[0] * cang[1] * sang[2] - sang[0] * sang[1] * cang[2]; q.m_q3 = cang[0] * sang[1] * cang[2] + sang[0] * cang[1] * sang[2]; break; } return q; }
public static CQuaternion operator *(CQuaternion quaternion1, CQuaternion quaternion2) { CQuaternion temp = new CQuaternion(); temp.m_q0 = quaternion1.m_q0 * quaternion2.m_q0 - quaternion1.m_q1 * quaternion2.m_q1 - quaternion1.m_q2 * quaternion2.m_q2 - quaternion1.m_q3 * quaternion2.m_q3; temp.m_q1 = quaternion1.m_q0 * quaternion2.m_q1 + quaternion1.m_q1 * quaternion2.m_q0 + quaternion1.m_q2 * quaternion2.m_q3 - quaternion1.m_q3 * quaternion2.m_q2; temp.m_q2 = quaternion1.m_q0 * quaternion2.m_q2 + quaternion1.m_q2 * quaternion2.m_q0 + quaternion1.m_q3 * quaternion2.m_q1 - quaternion1.m_q1 * quaternion2.m_q3; temp.m_q3 = quaternion1.m_q0 * quaternion2.m_q3 + quaternion1.m_q3 * quaternion2.m_q0 + quaternion1.m_q1 * quaternion2.m_q2 - quaternion1.m_q2 * quaternion2.m_q1; return temp; }
public object ReadYaml(IParser parser, Type xtype) { IType result; parser.ReadMappingStart(); // read name property var typeName = parser.SafeReadScalarProperty(s_typeName); var type = Serialization.GetTypeFromRedTypeStr(typeName); if (type == null) { throw new InvalidDataException(); } if (IsArray(type)) { var innertype = type.GetGenericArguments()[0]; var list = (IList)Activator.CreateInstance( typeof(List <>).MakeGenericType( new Type[] { innertype }), BindingFlags.Instance | BindingFlags.Public, binder: null, args: null, culture: null); if (list is null) { throw new InvalidDataException(); } // read values parser.SafeReadScalarValue(s_valueName); if (parser.Current.GetType() != _sequenceStartType) { throw new InvalidDataException("Invalid YAML content."); } parser.MoveNext(); // skip the sequence start do { var x = ReadYaml(parser, innertype); list.Add(x); } while (parser.Current.GetType() != _sequenceEndType); parser.MoveNext(); // skip the mapping end (or crash) var array = (IArray)Activator.CreateInstance( typeof(CArray <>).MakeGenericType( new Type[] { innertype }), BindingFlags.Instance | BindingFlags.Public, binder: null, args: null, culture: null); if (array is null) { throw new InvalidDataException(); } array.SetItems(list); result = array is IType o ? o : throw new JsonException(); } else { if (typeof(IPrimitive).IsAssignableFrom(type)) { result = Parse(parser.SafeReadScalarProperty(s_valueName), type); } else { parser.SafeReadScalarValue(s_valueName); parser.ReadMappingStart(); switch (Serialization.GetEnumFromType(type)) { case ETweakType.CColor: { result = new CColor { Red = byte.Parse(parser.SafeReadScalarProperty(nameof(CColor.Red))), Green = byte.Parse(parser.SafeReadScalarProperty(nameof(CColor.Green))), Blue = byte.Parse(parser.SafeReadScalarProperty(nameof(CColor.Blue))), Alpha = byte.Parse(parser.SafeReadScalarProperty(nameof(CColor.Alpha))), }; break; } case ETweakType.CEulerAngles: { result = new CEulerAngles { Pitch = float.Parse(parser.SafeReadScalarProperty(nameof(CEulerAngles.Pitch))), Yaw = float.Parse(parser.SafeReadScalarProperty(nameof(CEulerAngles.Yaw))), Roll = float.Parse(parser.SafeReadScalarProperty(nameof(CEulerAngles.Roll))), }; break; } case ETweakType.CQuaternion: { result = new CQuaternion { I = float.Parse(parser.SafeReadScalarProperty(nameof(CQuaternion.I))), J = float.Parse(parser.SafeReadScalarProperty(nameof(CQuaternion.J))), K = float.Parse(parser.SafeReadScalarProperty(nameof(CQuaternion.K))), R = float.Parse(parser.SafeReadScalarProperty(nameof(CQuaternion.R))), }; break; } case ETweakType.CVector2: { result = new CVector2 { X = float.Parse(parser.SafeReadScalarProperty(nameof(CVector2.X))), Y = float.Parse(parser.SafeReadScalarProperty(nameof(CVector2.Y))) }; break; } case ETweakType.CVector3: { result = new CVector3 { X = float.Parse(parser.SafeReadScalarProperty(nameof(CVector3.X))), Y = float.Parse(parser.SafeReadScalarProperty(nameof(CVector3.Y))), Z = float.Parse(parser.SafeReadScalarProperty(nameof(CVector3.Z))) }; break; } default: throw new ArgumentOutOfRangeException(); } parser.MoveNext(); // skip the mapping end (or crash) } } parser.MoveNext(); // skip the mapping end (or crash) return(result); }