private static MJoint ParseJoint(string[] mos, ref int line_counter, List <MJoint> mjointList)
{
// parse lines for current joint
// Todo: Improve parser to consider empty lines and comments
string name = mos[line_counter].Split(' ')[1];
float[] off = parseFloatParameter(mos[line_counter + 2].Split(' '), 3);
MVector3 offset = new MVector3(off[0], off[1], off[2]);
float[] quat = parseFloatParameter(mos[line_counter + 3].Split(' '), 4);
MQuaternion rotation = new MQuaternion(quat[1], quat[2], quat[3], quat[0]);
string[] channels = mos[line_counter + 4].Replace("CHANNELS", "").Split(' ');
List <MChannel> mchannels = MapChannels(channels);
MJoint mjoint = new MJoint(name, MJointTypeMap[name], offset, rotation);
mjoint.Channels = mchannels;
mjointList.Add(mjoint);
line_counter += 5;
while (!mos[line_counter].Contains("}"))
{
MJoint child = ParseJoint(mos, ref line_counter, mjointList);
child.Parent = mjoint.ID;
}
line_counter += 1;
return(mjoint);
}
public static MQuaternion FromToRotation(MVector3 aFrom, MVector3 aTo)
{
MVector3 axis = aFrom.Cross(aTo);
double angle = aFrom.Angle(aTo);
return(AngleAxis(angle, axis.Normalize()));
}
/// <summary>
/// Returns a list of MVector3 based on the MPathConstraint
/// </summary>
/// <param name="pathConstraint"></param>
/// <returns></returns>
public static List <MVector3> GetMVector3List(this MPathConstraint pathConstraint)
{
//Create a list for storing the full trajectory
List <MVector3> list = new List <MVector3>();
foreach (MGeometryConstraint mg in pathConstraint.PolygonPoints)
{
MVector3 p = new MVector3();
//Use the parent to constraint if defined
if (mg.ParentToConstraint != null)
{
p = mg.ParentToConstraint.Position.Clone();
}
else
{
p = mg.TranslationConstraint.GetVector3();
}
list.Add(p);
}
return(list);
}
/// <summary>
/// Returns the values of the vector as list of doubles
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static List <double> GetValues(this MVector3 vector)
{
return(new List <double>()
{
vector.X, vector.Y, vector.Z
});
}
/// <summary>
/// Computes the rotation to rotate from one vector to the other
/// </summary>
/// <param name="from">The start direction</param>
/// <param name="to">The desired direction</param>
/// <returns></returns>
private static MQuaternion FromToRotation(MVector3 from, MVector3 to)
{
//Normalize both vectors
from = from.Normalize();
to = to.Normalize();
//Estimate the rotation axis
MVector3 axis = MVector3Extensions.Cross(from, to).Normalize();
//Compute the phi angle
double phi = Math.Acos(MVector3Extensions.Dot(from, to)) / (from.Magnitude() * to.Magnitude());
//Create a new quaternion representing the rotation
MQuaternion result = new MQuaternion()
{
X = Math.Sin(phi / 2) * axis.X,
Y = Math.Sin(phi / 2) * axis.Y,
Z = Math.Sin(phi / 2) * axis.Z,
W = Math.Cos(phi / 2)
};
//Perform is nan check and return identity quaternion
if (double.IsNaN(result.W) || double.IsNaN(result.X) || double.IsNaN(result.Y) || double.IsNaN(result.Z))
{
result = new MQuaternion(0, 0, 0, 1);
}
//Return the estimated rotation
return(result);
}
public static MQuaternion AngleAxis(double aAngle, MVector3 aAxis)
{
aAxis = aAxis.Normalize();
double rad = aAngle * Math.PI / 180 * 0.5;
aAxis = aAxis.Multiply(Math.Sin(rad));
return(new MQuaternion(aAxis.X, aAxis.Y, aAxis.Z, Math.Cos(rad)));
}
public static double Angle(this MVector3 from, MVector3 to)
{
double rad = from.Normalize().Dot(to.Normalize());
// clamp
rad = Math.Max(Math.Min(rad, 1), -1);
return(Math.Acos(rad) * 180 / Math.PI);
}
/// <summary>
/// Returns a deep copy of the MVector3
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static MVector3 Clone(this MVector3 vector)
{
if (vector != null)
{
return(new MVector3(vector.X, vector.Y, vector.Z));
}
else
{
return(null);
}
}
/// <summary>
/// Creates euler angles (in degree) form the given quaternion
/// Source code from: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
/// </summary>
/// <param name="q"></param>
/// <returns></returns>
public static MVector3 ToEuler(MQuaternion q)
{
MVector3 euler = new MVector3();
// if the input quaternion is normalized, this is exactly one. Otherwise, this acts as a correction factor for the quaternion's not-normalizedness
double unit = (q.X * q.X) + (q.Y * q.Y) + (q.Z * q.Z) + (q.W * q.W);
// this will have a magnitude of 0.5 or greater if and only if this is a singularity case
double test = q.X * q.W - q.Y * q.Z;
if (test > 0.4995f * unit) // singularity at north pole
{
euler.X = Math.PI / 2;
euler.Y = 2f * Math.Atan2(q.Y, q.X);
euler.Z = 0;
}
else if (test < -0.4995f * unit) // singularity at south pole
{
euler.X = -Math.PI / 2;
euler.Y = -2f * Math.Atan2(q.Y, q.X);
euler.Z = 0;
}
else // no singularity - this is the majority of cases
{
euler.X = Math.Asin(2f * (q.W * q.X - q.Y * q.Z));
euler.Y = Math.Atan2(2f * q.W * q.Y + 2f * q.Z * q.X, 1 - 2f * (q.X * q.X + q.Y * q.Y));
euler.Z = Math.Atan2(2f * q.W * q.Z + 2f * q.X * q.Y, 1 - 2f * (q.Z * q.Z + q.X * q.X));
}
// all the math so far has been done in radians. Before returning, we convert to degrees...
euler = euler.Multiply(Rad2Deg);
//...and then ensure the degree values are between 0 and 360
euler.X %= 360;
euler.Y %= 360;
euler.Z %= 360;
return(euler);
}
/// <summary>
/// Creates a quaternion based on the given euler angles (in degree).
/// Source code from: https://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
/// </summary>
/// <param name="euler"></param>
/// <returns></returns>
public static MQuaternion FromEuler(MVector3 euler)
{
double xOver2 = euler.X * Deg2Rad * 0.5f;
double yOver2 = euler.Y * Deg2Rad * 0.5f;
double zOver2 = euler.Z * Deg2Rad * 0.5f;
double sinXOver2 = Math.Sin(xOver2);
double cosXOver2 = Math.Cos(xOver2);
double sinYOver2 = Math.Sin(yOver2);
double cosYOver2 = Math.Cos(yOver2);
double sinZOver2 = Math.Sin(zOver2);
double cosZOver2 = Math.Cos(zOver2);
MQuaternion result = new MQuaternion
{
X = cosYOver2 * sinXOver2 * cosZOver2 + sinYOver2 * cosXOver2 * sinZOver2,
Y = sinYOver2 * cosXOver2 * cosZOver2 - cosYOver2 * sinXOver2 * sinZOver2,
Z = cosYOver2 * cosXOver2 * sinZOver2 - sinYOver2 * sinXOver2 * cosZOver2,
W = cosYOver2 * cosXOver2 * cosZOver2 + sinYOver2 * sinXOver2 * sinZOver2
};
return(result);
}
/// <summary>
/// Multiplies a vector with a quaternion
/// </summary>
/// <param name="rotation"></param>
/// <param name="vector"></param>
/// <returns></returns>
public static MVector3 Multiply(this MQuaternion rotation, MVector3 vector)
{
double rx2 = rotation.X * 2f;
double ry2 = rotation.Y * 2f;
double rz2 = rotation.Z * 2f;
double rx = rotation.X * rx2;
double ry = rotation.Y * ry2;
double rz = rotation.Z * rz2;
double rxy = rotation.X * ry2;
double rxz = rotation.X * rz2;
double ryz = rotation.Y * rz2;
double rwx = rotation.W * rx2;
double rwy = rotation.W * ry2;
double rwz = rotation.W * rz2;
//Create a new vector representing the rotated one and return it
return(new MVector3
{
X = (1f - (ry + rz)) * vector.X + (rxy - rwz) * vector.Y + (rxz + rwy) * vector.Z,
Y = (rxy + rwz) * vector.X + (1f - (rx + rz)) * vector.Y + (ryz - rwx) * vector.Z,
Z = (rxz - rwy) * vector.X + (ryz + rwx) * vector.Y + (1f - (rx + ry)) * vector.Z
});
}
/// <summary>
/// Normalizes the given vector
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static MVector3 Normalize(this MVector3 vector)
{
return(vector.Divide(vector.Magnitude()));
}
public MTransform(string ID, MVector3 Position, MQuaternion Rotation) : this()
{
this.ID = ID;
this.Position = Position;
this.Rotation = Rotation;
}
/// <summary>
/// Returns the magnitude of the vector
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static float Magnitude(this MVector3 vector)
{
return((float)Math.Sqrt(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z));
}
public void Read(TProtocol iprot)
{
iprot.IncrementRecursionDepth();
try
{
bool isset_ID = false;
bool isset_Position = false;
bool isset_Rotation = false;
TField field;
iprot.ReadStructBegin();
while (true)
{
field = iprot.ReadFieldBegin();
if (field.Type == TType.Stop)
{
break;
}
switch (field.ID)
{
case 1:
if (field.Type == TType.String)
{
ID = iprot.ReadString();
isset_ID = true;
}
else
{
TProtocolUtil.Skip(iprot, field.Type);
}
break;
case 2:
if (field.Type == TType.Struct)
{
Position = new MVector3();
Position.Read(iprot);
isset_Position = true;
}
else
{
TProtocolUtil.Skip(iprot, field.Type);
}
break;
case 3:
if (field.Type == TType.Struct)
{
Rotation = new MQuaternion();
Rotation.Read(iprot);
isset_Rotation = true;
}
else
{
TProtocolUtil.Skip(iprot, field.Type);
}
break;
case 4:
if (field.Type == TType.String)
{
Parent = iprot.ReadString();
}
else
{
TProtocolUtil.Skip(iprot, field.Type);
}
break;
default:
TProtocolUtil.Skip(iprot, field.Type);
break;
}
iprot.ReadFieldEnd();
}
iprot.ReadStructEnd();
if (!isset_ID)
{
throw new TProtocolException(TProtocolException.INVALID_DATA, "required field ID not set");
}
if (!isset_Position)
{
throw new TProtocolException(TProtocolException.INVALID_DATA, "required field Position not set");
}
if (!isset_Rotation)
{
throw new TProtocolException(TProtocolException.INVALID_DATA, "required field Rotation not set");
}
}
finally
{
iprot.DecrementRecursionDepth();
}
}
/// <summary>
/// Performs a division
/// </summary>
/// <param name="vector"></param>
/// <param name="scalar"></param>
/// <returns></returns>
public static MVector3 Divide(this MVector3 vector, float scalar)
{
return(new MVector3(vector.X / scalar, vector.Y / scalar, vector.Z / scalar));
}
public static double Dot(this MVector3 v1, MVector3 v2)
{
return(v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z);
}
public static MVector3 Cross(this MVector3 v1, MVector3 v2)
{
return(new MVector3(v1.Y * v2.Z - v1.Z * v2.Y, v1.Z * v2.X - v1.X * v2.Z, v1.X * v2.Y - v1.Y * v2.X));
}
/// <summary>
/// Adds a vector on top of the current one and returns the result as new vector
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static MVector3 Add(this MVector3 v1, MVector3 v2)
{
return(new MVector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z));
}
/// <summary>
/// Returns a new vector which is the result of the Multiplication of the specified vector and a scalar value
/// </summary>
/// <param name="vector"></param>
/// <param name="scalar"></param>
/// <returns></returns>
public static MVector3 Multiply(this MVector3 vector, double scalar)
{
return(new MVector3(vector.X * scalar, vector.Y * scalar, vector.Z * scalar));
}
/// <summary>
/// Lerps the vector
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="t"></param>
/// <returns></returns>
public static MVector3 Lerp(this MVector3 from, MVector3 to, float t)
{
return(from.Add(to.Subtract(from).Multiply(t)));
}
/// <summary>
/// The euclidean distance between two vectors
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
public static double Distance(MVector3 vector1, MVector3 vector2)
{
return((vector1.Subtract(vector2)).Magnitude());
}
/// <summary>
/// Converts the vector to an interval
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static MInterval3 ToMInterval3(this MVector3 vector)
{
return(new MInterval3(new MInterval(vector.X, vector.X), new MInterval(vector.Y, vector.Y), new MInterval(vector.Z, vector.Z)));
}
/// <summary>
/// Performs a subtraction
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static MVector3 Subtract(this MVector3 v1, MVector3 v2)
{
return(new MVector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z));
}
/// <summary>
/// Converts the vector to an interval
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static MInterval3 ToMInterval3(this MVector3 vector, float deviation)
{
return(new MInterval3(new MInterval(vector.X - deviation / 2f, vector.X + deviation / 2f), new MInterval(vector.Y - deviation / 2f, vector.Y + deviation / 2f), new MInterval(vector.Z - deviation / 2f, vector.Z + deviation / 2f)));
}