initial upload
This commit is contained in:
tom.hempel
@ -0,0 +1,345 @@
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
|
||||
namespace RootMotion.FinalIK {
|
||||
|
||||
public partial class Grounding {
|
||||
|
||||
/// <summary>
|
||||
/// The %Grounding %Leg.
|
||||
/// </summary>
|
||||
public class Leg {
|
||||
|
||||
/// <summary>
|
||||
/// Returns true distance from foot to ground is less that maxStep
|
||||
/// </summary>
|
||||
public bool isGrounded { get; private set; }
|
||||
/// <summary>
|
||||
/// Gets the current IK position of the foot.
|
||||
/// </summary>
|
||||
public Vector3 IKPosition { get; private set; }
|
||||
/// <summary>
|
||||
/// Gets the current rotation offset of the foot.
|
||||
/// </summary>
|
||||
public Quaternion rotationOffset = Quaternion.identity;
|
||||
/// <summary>
|
||||
/// Returns true, if the leg is valid and initiated
|
||||
/// </summary>
|
||||
public bool initiated { get; private set; }
|
||||
/// <summary>
|
||||
/// The height of foot from ground.
|
||||
/// </summary>
|
||||
public float heightFromGround { get; private set; }
|
||||
/// <summary>
|
||||
/// Velocity of the foot
|
||||
/// </summary>
|
||||
public Vector3 velocity { get; private set; }
|
||||
/// <summary>
|
||||
/// Gets the foot Transform.
|
||||
/// </summary>
|
||||
public Transform transform { get; private set; }
|
||||
/// <summary>
|
||||
/// Gets the current IK offset.
|
||||
/// </summary>
|
||||
public float IKOffset { get; private set; }
|
||||
|
||||
public bool invertFootCenter;
|
||||
|
||||
public RaycastHit heelHit { get; private set; }
|
||||
public RaycastHit capsuleHit { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the RaycastHit last used by the Grounder to get ground height at foot position.
|
||||
/// </summary>
|
||||
public RaycastHit GetHitPoint {
|
||||
get
|
||||
{
|
||||
if (grounding.quality == Quality.Best) return capsuleHit;
|
||||
return heelHit;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Overrides the animated position of the foot.
|
||||
/// </summary>
|
||||
public void SetFootPosition(Vector3 position)
|
||||
{
|
||||
doOverrideFootPosition = true;
|
||||
overrideFootPosition = position;
|
||||
}
|
||||
|
||||
private Grounding grounding;
|
||||
private float lastTime, deltaTime;
|
||||
private Vector3 lastPosition;
|
||||
private Quaternion toHitNormal, r;
|
||||
private Vector3 up = Vector3.up;
|
||||
private bool doOverrideFootPosition;
|
||||
private Vector3 overrideFootPosition;
|
||||
private Vector3 transformPosition;
|
||||
|
||||
// Initiates the Leg
|
||||
public void Initiate(Grounding grounding, Transform transform) {
|
||||
initiated = false;
|
||||
this.grounding = grounding;
|
||||
this.transform = transform;
|
||||
up = Vector3.up;
|
||||
IKPosition = transform.position;
|
||||
rotationOffset = Quaternion.identity;
|
||||
|
||||
initiated = true;
|
||||
OnEnable();
|
||||
}
|
||||
|
||||
// Should be called each time the leg is (re)activated
|
||||
public void OnEnable() {
|
||||
if (!initiated) return;
|
||||
|
||||
lastPosition = transform.position;
|
||||
lastTime = Time.deltaTime;
|
||||
}
|
||||
|
||||
// Set everything to 0
|
||||
public void Reset() {
|
||||
lastPosition = transform.position;
|
||||
lastTime = Time.deltaTime;
|
||||
IKOffset = 0f;
|
||||
IKPosition = transform.position;
|
||||
rotationOffset = Quaternion.identity;
|
||||
}
|
||||
|
||||
// Raycasting, processing the leg's position
|
||||
public void Process() {
|
||||
if (!initiated) return;
|
||||
if (grounding.maxStep <= 0) return;
|
||||
|
||||
transformPosition = doOverrideFootPosition ? overrideFootPosition : transform.position;
|
||||
doOverrideFootPosition = false;
|
||||
|
||||
deltaTime = Time.time - lastTime;
|
||||
lastTime = Time.time;
|
||||
if (deltaTime == 0f) return;
|
||||
|
||||
up = grounding.up;
|
||||
heightFromGround = Mathf.Infinity;
|
||||
|
||||
// Calculating velocity
|
||||
velocity = (transformPosition - lastPosition) / deltaTime;
|
||||
//velocity = grounding.Flatten(velocity);
|
||||
lastPosition = transformPosition;
|
||||
|
||||
Vector3 prediction = velocity * grounding.prediction;
|
||||
|
||||
if (grounding.footRadius <= 0) grounding.quality = Grounding.Quality.Fastest;
|
||||
|
||||
isGrounded = false;
|
||||
|
||||
// Raycasting
|
||||
switch (grounding.quality)
|
||||
{
|
||||
|
||||
// The fastest, single raycast
|
||||
case Grounding.Quality.Fastest:
|
||||
|
||||
RaycastHit predictedHit = GetRaycastHit(prediction);
|
||||
SetFootToPoint(predictedHit.normal, predictedHit.point);
|
||||
if (predictedHit.collider != null) isGrounded = true;
|
||||
break;
|
||||
|
||||
// Medium, 3 raycasts
|
||||
case Grounding.Quality.Simple:
|
||||
|
||||
heelHit = GetRaycastHit(Vector3.zero);
|
||||
Vector3 f = grounding.GetFootCenterOffset();
|
||||
if (invertFootCenter) f = -f;
|
||||
RaycastHit toeHit = GetRaycastHit(f + prediction);
|
||||
RaycastHit sideHit = GetRaycastHit(grounding.root.right * grounding.footRadius * 0.5f);
|
||||
|
||||
if (heelHit.collider != null || toeHit.collider != null || sideHit.collider != null) isGrounded = true;
|
||||
|
||||
Vector3 planeNormal = Vector3.Cross(toeHit.point - heelHit.point, sideHit.point - heelHit.point).normalized;
|
||||
if (Vector3.Dot(planeNormal, up) < 0) planeNormal = -planeNormal;
|
||||
|
||||
SetFootToPlane(planeNormal, heelHit.point, heelHit.point);
|
||||
break;
|
||||
|
||||
// The slowest, raycast and a capsule cast
|
||||
case Grounding.Quality.Best:
|
||||
heelHit = GetRaycastHit(invertFootCenter ? -grounding.GetFootCenterOffset() : Vector3.zero);
|
||||
capsuleHit = GetCapsuleHit(prediction);
|
||||
|
||||
if (heelHit.collider != null || capsuleHit.collider != null) isGrounded = true;
|
||||
|
||||
SetFootToPlane(capsuleHit.normal, capsuleHit.point, heelHit.point);
|
||||
break;
|
||||
}
|
||||
|
||||
float offsetTarget = stepHeightFromGround;
|
||||
if (!grounding.rootGrounded) offsetTarget = 0f;
|
||||
|
||||
IKOffset = Interp.LerpValue(IKOffset, offsetTarget, grounding.footSpeed, grounding.footSpeed);
|
||||
IKOffset = Mathf.Lerp(IKOffset, offsetTarget, deltaTime * grounding.footSpeed);
|
||||
|
||||
float legHeight = grounding.GetVerticalOffset(transformPosition, grounding.root.position);
|
||||
float currentMaxOffset = Mathf.Clamp(grounding.maxStep - legHeight, 0f, grounding.maxStep);
|
||||
|
||||
IKOffset = Mathf.Clamp(IKOffset, -currentMaxOffset, IKOffset);
|
||||
|
||||
RotateFoot();
|
||||
|
||||
// Update IK values
|
||||
IKPosition = transformPosition - up * IKOffset;
|
||||
|
||||
float rW = grounding.footRotationWeight;
|
||||
rotationOffset = rW >= 1? r: Quaternion.Slerp(Quaternion.identity, r, rW);
|
||||
}
|
||||
|
||||
// Gets the height from ground clamped between min and max step height
|
||||
public float stepHeightFromGround {
|
||||
get {
|
||||
return Mathf.Clamp(heightFromGround, -grounding.maxStep, grounding.maxStep);
|
||||
}
|
||||
}
|
||||
|
||||
// Get predicted Capsule hit from the middle of the foot
|
||||
private RaycastHit GetCapsuleHit(Vector3 offsetFromHeel)
|
||||
{
|
||||
RaycastHit hit = new RaycastHit();
|
||||
Vector3 f = grounding.GetFootCenterOffset();
|
||||
if (invertFootCenter) f = -f;
|
||||
Vector3 origin = transformPosition + f;
|
||||
|
||||
if (grounding.overstepFallsDown)
|
||||
{
|
||||
hit.point = origin - up * grounding.maxStep;
|
||||
}
|
||||
else
|
||||
{
|
||||
hit.point = new Vector3(origin.x, grounding.root.position.y, origin.z);
|
||||
}
|
||||
hit.normal = up;
|
||||
|
||||
// Start point of the capsule
|
||||
Vector3 capsuleStart = origin + grounding.maxStep * up;
|
||||
// End point of the capsule depending on the foot's velocity.
|
||||
Vector3 capsuleEnd = capsuleStart + offsetFromHeel;
|
||||
|
||||
if (grounding.CapsuleCast(capsuleStart, capsuleEnd, grounding.footRadius, -up, out hit, grounding.maxStep * 2, grounding.layers, QueryTriggerInteraction.Ignore))
|
||||
{
|
||||
// Safeguarding from a CapsuleCast bug in Unity that might cause it to return NaN for hit.point when cast against large colliders.
|
||||
if (float.IsNaN(hit.point.x))
|
||||
{
|
||||
hit.point = origin - up * grounding.maxStep * 2f;
|
||||
hit.normal = up;
|
||||
}
|
||||
}
|
||||
|
||||
// Since Unity2017 Raycasts will return Vector3.zero when starting from inside a collider
|
||||
if (hit.point == Vector3.zero && hit.normal == Vector3.zero)
|
||||
{
|
||||
if (grounding.overstepFallsDown)
|
||||
{
|
||||
hit.point = origin - up * grounding.maxStep;
|
||||
}
|
||||
else
|
||||
{
|
||||
hit.point = new Vector3(origin.x, grounding.root.position.y, origin.z);
|
||||
}
|
||||
}
|
||||
|
||||
return hit;
|
||||
}
|
||||
|
||||
// Get simple Raycast from the heel
|
||||
private RaycastHit GetRaycastHit(Vector3 offsetFromHeel)
|
||||
{
|
||||
RaycastHit hit = new RaycastHit();
|
||||
Vector3 origin = transformPosition + offsetFromHeel;
|
||||
|
||||
if (grounding.overstepFallsDown)
|
||||
{
|
||||
hit.point = origin - up * grounding.maxStep;
|
||||
}
|
||||
else
|
||||
{
|
||||
hit.point = new Vector3(origin.x, grounding.root.position.y, origin.z);
|
||||
}
|
||||
hit.normal = up;
|
||||
|
||||
if (grounding.maxStep <= 0f) return hit;
|
||||
|
||||
grounding.Raycast(origin + grounding.maxStep * up, -up, out hit, grounding.maxStep * 2, grounding.layers, QueryTriggerInteraction.Ignore);
|
||||
|
||||
// Since Unity2017 Raycasts will return Vector3.zero when starting from inside a collider
|
||||
if (hit.point == Vector3.zero && hit.normal == Vector3.zero)
|
||||
{
|
||||
if (grounding.overstepFallsDown)
|
||||
{
|
||||
hit.point = origin - up * grounding.maxStep;
|
||||
}
|
||||
else
|
||||
{
|
||||
hit.point = new Vector3(origin.x, grounding.root.position.y, origin.z);
|
||||
}
|
||||
}
|
||||
|
||||
return hit;
|
||||
}
|
||||
|
||||
// Rotates ground normal with respect to maxFootRotationAngle
|
||||
private Vector3 RotateNormal(Vector3 normal) {
|
||||
if (grounding.quality == Grounding.Quality.Best) return normal;
|
||||
return Vector3.RotateTowards(up, normal, grounding.maxFootRotationAngle * Mathf.Deg2Rad, deltaTime);
|
||||
}
|
||||
|
||||
// Set foot height from ground relative to a point
|
||||
private void SetFootToPoint(Vector3 normal, Vector3 point) {
|
||||
toHitNormal = Quaternion.FromToRotation(up, RotateNormal(normal));
|
||||
|
||||
heightFromGround = GetHeightFromGround(point);
|
||||
}
|
||||
|
||||
// Set foot height from ground relative to a plane
|
||||
private void SetFootToPlane(Vector3 planeNormal, Vector3 planePoint, Vector3 heelHitPoint) {
|
||||
planeNormal = RotateNormal(planeNormal);
|
||||
toHitNormal = Quaternion.FromToRotation(up, planeNormal);
|
||||
|
||||
Vector3 pointOnPlane = V3Tools.LineToPlane(transformPosition + up * grounding.maxStep, -up, planeNormal, planePoint);
|
||||
|
||||
// Get the height offset of the point on the plane
|
||||
heightFromGround = GetHeightFromGround(pointOnPlane);
|
||||
|
||||
// Making sure the heel doesn't penetrate the ground
|
||||
float heelHeight = GetHeightFromGround(heelHitPoint);
|
||||
heightFromGround = Mathf.Clamp(heightFromGround, -Mathf.Infinity, heelHeight);
|
||||
}
|
||||
|
||||
// Calculate height offset of a point
|
||||
private float GetHeightFromGround(Vector3 hitPoint) {
|
||||
return grounding.GetVerticalOffset(transformPosition, hitPoint) - rootYOffset;
|
||||
}
|
||||
|
||||
// Adding ground normal offset to the foot's rotation
|
||||
private void RotateFoot() {
|
||||
// Getting the full target rotation
|
||||
Quaternion rotationOffsetTarget = GetRotationOffsetTarget();
|
||||
|
||||
// Slerping the rotation offset
|
||||
r = Quaternion.Slerp(r, rotationOffsetTarget, deltaTime * grounding.footRotationSpeed);
|
||||
}
|
||||
|
||||
// Gets the target hit normal offset as a Quaternion
|
||||
private Quaternion GetRotationOffsetTarget() {
|
||||
if (grounding.maxFootRotationAngle <= 0f) return Quaternion.identity;
|
||||
if (grounding.maxFootRotationAngle >= 180f) return toHitNormal;
|
||||
return Quaternion.RotateTowards(Quaternion.identity, toHitNormal, grounding.maxFootRotationAngle);
|
||||
}
|
||||
|
||||
// The foot's height from ground in the animation
|
||||
private float rootYOffset {
|
||||
get {
|
||||
return grounding.GetVerticalOffset(transformPosition, grounding.root.position - up * grounding.heightOffset);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user