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 }