//--------------------------------------------------
// Constructors
//--------------------------------------------------
/// <summary>
/// Creates a new generator with support for the provided number of legs, and peak heights for each of those legs
/// </summary>
/// <param name="supportedLegs">The number of legs to support</param>
/// <param name="peakHeight">An array of equal size to the number of supported legs, which the peak stride height of each</param>
public GaitGenerator(int supportedLegs, float[] peakHeights)
{
_anyChannelsStriding = false;
_channels = new GaitChannel[supportedLegs];
for (int i = 0; i < _channels.Length; i++)
{
_channels[i] = new GaitChannel(peakHeights[i]);
}
_maxOfAllStances = 1.0f;
_fOverrun = 0.0f;
_cycleTime = 0.0f;
}
/// <summary>
///
/// </summary>
/// <param name="channel"></param>
/// <param name="point"></param>
/// <param name="motion"></param>
/// <param name="pathTime"></param>
/// <param name="pointSpeed"></param>
/// <param name="transform"></param>
/// <returns></returns>
private static WalkController.Point ComputePointPosition(GaitChannel channel, MotionPoint point, Motion motion, float pathTime, float pointSpeed, Transform transform)
{
Vector3 pointPosition = point.Position;
float pointAngle = point.Angle;
//TODO This is wrong as it returns a gait position at zero when in fact it should be elsewhere
if (!float.IsPositiveInfinity(pathTime))
{
//Below finds the local ideal position of the stride point, as in where on the line/arc it is at
if (motion.HasRotation())
{
Vector3 origin = motion.RotationOrigin;
float pathRadius = (pointPosition - origin).magnitude;
if (pathRadius > 0.0f)
{
float angleRange = (pointSpeed * pathTime) / (pathRadius * Mathf.Deg2Rad);
Debug.DrawRay(transform.TransformPoint(origin + Vector3.up * 0.05f), transform.TransformDirection(Util.Rotate(pointPosition - origin, -angleRange)), Color.white);
Debug.DrawRay(transform.TransformPoint(origin + Vector3.up * 0.05f), transform.TransformDirection(Util.Rotate(pointPosition - origin, angleRange)), Color.white);
Util.DrawArc(transform.TransformPoint(origin + Vector3.up * 0.05f), transform.TransformDirection(pointPosition - origin), transform.TransformDirection(Vector3.up), pathRadius, pathRadius, 32, Color.white, -angleRange, angleRange * 2.0f);
//float fLength = pathRadius * (angleRange * Mathf.Deg2Rad);
//Debug.DrawRay(transform.TransformPoint(pointPosition + Vector3.up * 0.1f), transform.TransformDirection(Vector3.forward * fLength / 2), regions[i].DbgCol);
//Debug.DrawRay(transform.TransformPoint(pointPosition + Vector3.up * 0.1f), transform.TransformDirection(Vector3.back * fLength / 2), regions[i].DbgCol);
pointPosition = Util.Rotate(pointPosition - origin, angleRange * Mathf.Sign(motion.AngularVelocity) * channel.Displacement) + origin;
pointAngle += angleRange * Mathf.Sign(motion.AngularVelocity) * channel.Displacement;
}
}
else
{
float pathLength = pointSpeed * pathTime;
//TODO see if this is necessary any more now we do the infinity check
if (!float.IsNaN(pathLength))
{
Vector3 linVelocity = Util.Rotate(motion.LinearVelocity, point.Angle);
Vector3 endPos1 = pointPosition + (linVelocity.normalized * -pathLength) + new Vector3(linVelocity.normalized.z, 0.0f, -linVelocity.normalized.x) * pathLength * 0.5f;
Vector3 endPos2 = pointPosition + (linVelocity.normalized * -pathLength) - new Vector3(linVelocity.normalized.z, 0.0f, -linVelocity.normalized.x) * pathLength * 0.5f;
Debug.DrawLine(transform.TransformPoint(endPos1 + Vector3.up * 0.05f), transform.TransformPoint(endPos2 + Vector3.up * 0.05f), Color.white);
Vector3 endPos3 = pointPosition - (linVelocity.normalized * -pathLength) + new Vector3(linVelocity.normalized.z, 0.0f, -linVelocity.normalized.x) * pathLength * 0.5f;
Vector3 endPos4 = pointPosition - (linVelocity.normalized * -pathLength) - new Vector3(linVelocity.normalized.z, 0.0f, -linVelocity.normalized.x) * pathLength * 0.5f;
Debug.DrawLine(transform.TransformPoint(endPos3 + Vector3.up * 0.05f), transform.TransformPoint(endPos4 + Vector3.up * 0.05f), Color.white);
Debug.DrawRay(transform.TransformPoint(pointPosition + Vector3.up * 0.05f), transform.TransformDirection(linVelocity.normalized * pathLength), Color.white);
Debug.DrawRay(transform.TransformPoint(pointPosition + Vector3.up * 0.05f), transform.TransformDirection(linVelocity.normalized * -pathLength), Color.white);
pointPosition += linVelocity.normalized * pathLength * channel.Displacement;
//Debug.DrawRay(transform.TransformPoint(pointPosition + Vector3.up * 0.1f), transform.TransformDirection(Vector3.forward * pathLength / 2), regions[i].DbgCol);
//Debug.DrawRay(transform.TransformPoint(pointPosition + Vector3.up * 0.1f), transform.TransformDirection(Vector3.back * pathLength / 2), regions[i].DbgCol);
}
}
}
pointPosition.y += channel.Height;
return(new WalkController.Point(pointPosition, pointAngle));
}
