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]); } } }