void CalculateTimeOffsets()
{
const float min = 0.0001f;
float[] offsets = new float[MotionAsset.MovingCount];
float[] offsetChanges = new float[MotionAsset.MovingCount];
for (int i = Int.Zero; i < MotionAsset.MovingCount; i++)
{
offsets[i] = Float.Zero;
}
int springs = (MotionAsset.MovingCount * MotionAsset.MovingCount - MotionAsset.MovingCount) / Int.Two;
int iteration = Int.Zero;
bool finished = false;
while (iteration < Int.OneHundred && finished == false)
{
for (int i = Int.Zero; i < MotionAsset.MovingCount; i++)
{
offsetChanges[i] = Int.Zero;
}
// Calculate offset changes
for (int i = Int.One; i < MotionAsset.MovingCount; i++)
{
for (int j = Int.Zero; j < i; j++)
{
for (int leg = Int.Zero; leg < m_controller.Legs.Length; leg++)
{
var index = MotionAsset.MovingIndexes[i];
var motion = MotionAsset.MotionData[index];
float ta = motion.Cycles[leg].StanceTime + offsets[i];
float tb = motion.Cycles[leg].StanceTime + offsets[j];
Vector2 va = new Vector2(Mathf.Cos(ta * Float.Two * Mathf.PI), Mathf.Sin(ta * Float.Two * Mathf.PI));
Vector2 vb = new Vector2(Mathf.Cos(tb * Float.Two * Mathf.PI), Mathf.Sin(tb * Float.Two * Mathf.PI));
Vector2 abVector = vb - va;
Vector2 va2 = va + abVector * Float.DotOne;
Vector2 vb2 = vb - abVector * Float.DotOne;
float ta2 = Exts.Mod(Mathf.Atan2(va2.y, va2.x) / Float.Two / Mathf.PI);
float tb2 = Exts.Mod(Mathf.Atan2(vb2.y, vb2.x) / Float.Two / Mathf.PI);
float aChange = Exts.Mod(ta2 - ta);
float bChange = Exts.Mod(tb2 - tb);
if (aChange > Float.Half)
{
aChange = aChange - Float.One;
}
if (bChange > Float.Half)
{
bChange = bChange - Float.One;
}
offsetChanges[i] += aChange * Float.Half / springs;
offsetChanges[j] += bChange * Float.Half / springs;
}
}
}
// Apply new offset changes
float maxChange = 0;
for (int i = 0; i < MotionAsset.MovingCount; i++)
{
offsets[i] += offsetChanges[i];
maxChange = Mathf.Max(maxChange, Mathf.Abs(offsetChanges[i]));
}
iteration++;
if (maxChange < min)
{
finished = true;
}
}
// Apply the offsets to the motions
for (int m = 0; m < MotionAsset.MovingCount; m++)
{
var index = MotionAsset.MovingIndexes[m];
MotionAsset.MotionData[index].CycleOffset = offsets[m];
for (int leg = Int.Zero; leg < m_controller.Legs.Length; leg++)
{
var val = MotionAsset.MotionData[index].Cycles[leg].StanceTime + offsets[m];
MotionAsset.MotionData[index].Cycles[leg].StanceTime = Exts.Mod(val);
}
}
}
public static void TransformFromMatrix(Matrix4x4 matrix, Transform trans)
{
trans.rotation = Exts.QuaternionFromMatrix(matrix);
trans.position = matrix.GetColumn(Int.Three); // uses implicit conversion from Vector4 to Vector3
}
