private void UpdateFacingDirection()
{
// Calculate which way character should be facing
float facingWeight = desiredFacingDirection.magnitude;
Vector3 combinedFacingDirection = (
transform.rotation * desiredMovementDirection * (1 - facingWeight)
+ desiredFacingDirection * facingWeight
);
combinedFacingDirection = LocomotionUtil.ProjectOntoPlane(combinedFacingDirection, transform.up);
combinedFacingDirection = alignCorrection * combinedFacingDirection;
if (combinedFacingDirection.sqrMagnitude > 0.01f)
{
Vector3 newForward = LocomotionUtil.ConstantSlerp(
transform.forward,
combinedFacingDirection,
maxRotationSpeed * Time.deltaTime
);
newForward = LocomotionUtil.ProjectOntoPlane(newForward, transform.up);
//Debug.DrawLine(transform.position, transform.position+newForward, Color.yellow);
Quaternion q = new Quaternion();
q.SetLookRotation(newForward, transform.up);
transform.rotation = q;
}
}
public void InitFootData(int leg)
{
// Make sure character is in grounded pose before analyzing
gameObject.SampleAnimation(groundedPose, 0);
// Give each leg a color
Vector3 colorVect = Quaternion.AngleAxis(leg * 360.0f / legs.Length, Vector3.one) * Vector3.right;
legs[leg].debugColor = new Color(colorVect.x, colorVect.y, colorVect.z);
// Calculate heel and toetip positions and alignments
// (The vector from the ankle to the ankle projected onto the ground at the stance pose
// in local coordinates relative to the ankle transform.
// This essentially is the ankle moved to the bottom of the foot, approximating the heel.)
// Get ankle position projected down onto the ground
Matrix4x4 ankleMatrix = LocomotionUtil.RelativeMatrix(legs[leg].ankle, gameObject.transform);
Vector3 anklePosition = ankleMatrix.MultiplyPoint(Vector3.zero);
Vector3 heelPosition = anklePosition;
heelPosition.y = groundPlaneHeight;
// Get toe position projected down onto the ground
Matrix4x4 toeMatrix = LocomotionUtil.RelativeMatrix(legs[leg].toe, gameObject.transform);
Vector3 toePosition = toeMatrix.MultiplyPoint(Vector3.zero);
Vector3 toetipPosition = toePosition;
toetipPosition.y = groundPlaneHeight;
// Calculate foot middle and vector
Vector3 footMiddle = (heelPosition + toetipPosition) / 2;
Vector3 footVector;
if (toePosition == anklePosition)
{
footVector = ankleMatrix.MultiplyVector(legs[leg].ankle.localPosition);
footVector.y = 0;
footVector = footVector.normalized;
}
else
{
footVector = (toetipPosition - heelPosition).normalized;
}
Vector3 footSideVector = Vector3.Cross(Vector3.up, footVector);
legs[leg].ankleHeelVector = (
footMiddle
+ (-legs[leg].footLength / 2 + legs[leg].footOffset.y) * footVector
+ legs[leg].footOffset.x * footSideVector
);
legs[leg].ankleHeelVector = ankleMatrix.inverse.MultiplyVector(legs[leg].ankleHeelVector - anklePosition);
legs[leg].toeToetipVector = (
footMiddle
+ (legs[leg].footLength / 2 + legs[leg].footOffset.y) * footVector
+ legs[leg].footOffset.x * footSideVector
);
legs[leg].toeToetipVector = toeMatrix.inverse.MultiplyVector(legs[leg].toeToetipVector - toePosition);
}
public void SetDesiredOrientation(Vector3 target)
{
Vector3 difference =
LocomotionUtil.ProjectOntoPlane(
target - transform.position,
Vector3.up);
this.desiredOrientation = Quaternion.LookRotation(difference);
}
private void UpdateVelocity()
{
CharacterController controller = GetComponent(typeof(CharacterController)) as CharacterController;
Vector3 velocity = controller.velocity;
if (firstframe)
{
velocity = Vector3.zero;
firstframe = false;
}
if (grounded)
{
velocity = LocomotionUtil.ProjectOntoPlane(velocity, transform.up);
}
// Calculate how fast we should be moving
Vector3 movement = velocity;
//bool hasJumped = false;
jumping = false;
if (grounded)
{
// Apply a force that attempts to reach our target velocity
Vector3 velocityChange = (desiredVelocity - velocity);
if (velocityChange.magnitude > maxVelocityChange)
{
velocityChange = velocityChange.normalized * maxVelocityChange;
}
movement += velocityChange;
// Jump
if (canJump && Input.GetButton("Jump"))
{
movement += transform.up * Mathf.Sqrt(2 * jumpHeight * gravity);
//hasJumped = true;
jumping = true;
}
}
// Apply downwards gravity
movement += transform.up * -gravity * Time.deltaTime;
if (jumping)
{
movement -= transform.up * -gravity * Time.deltaTime / 2;
}
// Apply movement
CollisionFlags flags = controller.Move(movement * Time.deltaTime);
grounded = (flags & CollisionFlags.CollidedBelow) != 0;
}
public static float CyclicLerp(float a, float b, float t, float period)
{
if (Mathf.Abs(b - a) <= period / 2)
{
return(a * (1 - t) + b * t);
}
if (b < a)
{
a -= period;
}
else
{
b -= period;
}
return(LocomotionUtil.Mod(a * (1 - t) + b * t));
}
public override float[] Interpolate(float[] output, bool normalize)
{
float[] weights = BasicChecks(output);
if (weights != null)
{
return(weights);
}
weights = new float[samples.Length];
Vector3 outp;
Vector3[] samp = new Vector3[samples.Length];
if (output.Length == 2)
{
outp = new Vector3(output[0], output[1], 0);
for (int i = 0; i < samples.Length; i++)
{
samp[i] = new Vector3(samples[i][0], samples[i][1], 0);
}
}
else if (output.Length == 3)
{
outp = new Vector3(output[0], output[1], output[2]);
for (int i = 0; i < samples.Length; i++)
{
samp[i] = new Vector3(samples[i][0], samples[i][1], samples[i][2]);
}
}
else
{
return(null);
}
for (int i = 0; i < samples.Length; i++)
{
bool outsideHull = false;
float value = 1;
for (int j = 0; j < samples.Length; j++)
{
if (i == j)
{
continue;
}
Vector3 sampleI = samp[i];
Vector3 sampleJ = samp[j];
float iAngle, oAngle;
Vector3 outputProj;
float angleMultiplier = 2;
if (sampleI == Vector3.zero)
{
iAngle = Vector3.Angle(outp, sampleJ) * Mathf.Deg2Rad;
oAngle = 0;
outputProj = outp;
angleMultiplier = 1;
}
else if (sampleJ == Vector3.zero)
{
iAngle = Vector3.Angle(outp, sampleI) * Mathf.Deg2Rad;
oAngle = iAngle;
outputProj = outp;
angleMultiplier = 1;
}
else
{
iAngle = Vector3.Angle(sampleI, sampleJ) * Mathf.Deg2Rad;
if (iAngle > 0)
{
if (outp == Vector3.zero)
{
oAngle = iAngle;
outputProj = outp;
}
else
{
Vector3 axis = Vector3.Cross(sampleI, sampleJ);
outputProj = LocomotionUtil.ProjectOntoPlane(outp, axis);
oAngle = Vector3.Angle(sampleI, outputProj) * Mathf.Deg2Rad;
if (iAngle < Mathf.PI * 0.99f)
{
if (Vector3.Dot(Vector3.Cross(sampleI, outputProj), axis) < 0)
{
oAngle *= -1;
}
}
}
}
else
{
outputProj = outp;
oAngle = 0;
}
}
float magI = sampleI.magnitude;
float magJ = sampleJ.magnitude;
float magO = outputProj.magnitude;
float avgMag = (magI + magJ) / 2;
magI /= avgMag;
magJ /= avgMag;
magO /= avgMag;
Vector3 vecIJ = new Vector3(iAngle * angleMultiplier, magJ - magI, 0);
Vector3 vecIO = new Vector3(oAngle * angleMultiplier, magO - magI, 0);
float newValue = 1 - Vector3.Dot(vecIJ, vecIO) / vecIJ.sqrMagnitude;
if (newValue < 0)
{
outsideHull = true;
break;
}
value = Mathf.Min(value, newValue);
}
if (!outsideHull)
{
weights[i] = value;
}
}
// Normalize weights
if (normalize)
{
float summedWeight = 0;
for (int i = 0; i < samples.Length; i++)
{
summedWeight += weights[i];
}
if (summedWeight > 0)
{
for (int i = 0; i < samples.Length; i++)
{
weights[i] /= summedWeight;
}
}
}
return(weights);
}
public static void TransformFromMatrix(Matrix4x4 matrix, Transform trans)
{
trans.rotation = LocomotionUtil.QuaternionFromMatrix(matrix);
trans.position = matrix.GetColumn(3); // uses implicit conversion from Vector4 to Vector3
}
public void CalculateTimeOffsets()
{
float[] offsets = new float[cycleMotions.Length];
float[] offsetChanges = new float[cycleMotions.Length];
for (int i = 0; i < cycleMotions.Length; i++)
{
offsets[i] = 0;
}
int springs = (cycleMotions.Length * cycleMotions.Length - cycleMotions.Length) / 2;
int iteration = 0;
bool finished = false;
while (iteration < 100 && finished == false)
{
for (int i = 0; i < cycleMotions.Length; i++)
{
offsetChanges[i] = 0;
}
// Calculate offset changes
for (int i = 1; i < cycleMotions.Length; i++)
{
for (int j = 0; j < i; j++)
{
for (int leg = 0; leg < legs.Length; leg++)
{
float ta = cycleMotions[i].cycles[leg].stanceTime + offsets[i];
float tb = cycleMotions[j].cycles[leg].stanceTime + offsets[j];
Vector2 va = new Vector2(Mathf.Cos(ta * 2 * Mathf.PI), Mathf.Sin(ta * 2 * Mathf.PI));
Vector2 vb = new Vector2(Mathf.Cos(tb * 2 * Mathf.PI), Mathf.Sin(tb * 2 * Mathf.PI));
Vector2 abVector = vb - va;
Vector2 va2 = va + abVector * 0.1f;
Vector2 vb2 = vb - abVector * 0.1f;
float ta2 = LocomotionUtil.Mod(Mathf.Atan2(va2.y, va2.x) / 2 / Mathf.PI);
float tb2 = LocomotionUtil.Mod(Mathf.Atan2(vb2.y, vb2.x) / 2 / Mathf.PI);
float aChange = LocomotionUtil.Mod(ta2 - ta);
float bChange = LocomotionUtil.Mod(tb2 - tb);
if (aChange > 0.5f)
{
aChange = aChange - 1;
}
if (bChange > 0.5f)
{
bChange = bChange - 1;
}
offsetChanges[i] += aChange * 5.0f / springs;
offsetChanges[j] += bChange * 5.0f / springs;
}
}
}
// Apply new offset changes
float maxChange = 0;
for (int i = 0; i < cycleMotions.Length; i++)
{
offsets[i] += offsetChanges[i];
maxChange = Mathf.Max(maxChange, Mathf.Abs(offsetChanges[i]));
}
iteration++;
if (maxChange < 0.0001)
{
finished = true;
}
}
// Apply the offsets to the motions
for (int m = 0; m < cycleMotions.Length; m++)
{
cycleMotions[m].cycleOffset = offsets[m];
for (int leg = 0; leg < legs.Length; leg++)
{
cycleMotions[m].cycles[leg].stanceTime =
LocomotionUtil.Mod(cycleMotions[m].cycles[leg].stanceTime + offsets[m]);
}
}
}
