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);
}
