/*
* This function calculates the default position using the parameters: Stride, Length and Height
* This default position is an important anchor point used for new step point calculation.
*/
private Vector3 calculateDefault()
{
float diameter = chainLength - minDistance;
Vector3 rootRotAxis = rootJoint.getRotationAxis();
//Be careful with the use of transform.up and rootjoint.getRotationAxis(). In my case they are equivalent with the exception of the right side being inverted.
//However they can be different and this has to be noticed here. The code below is probably wrong for the general case.
Vector3 normal = spider.transform.up;
Vector3 toEnd = ikChain.getEndEffector().position - rootJoint.getRotationPoint();
toEnd = Vector3.ProjectOnPlane(toEnd, normal).normalized;
Vector3 pivot = spider.getColliderBottomPoint() + Vector3.ProjectOnPlane(rootJoint.getRotationPoint() - spider.transform.position, normal);
Vector3 midOrient = Quaternion.AngleAxis(0.5f * (rootJoint.maxAngle + rootJoint.minAngle), rootRotAxis) * toEnd;
//Set the following debug variables for the DOF Arc
minOrient = spider.transform.InverseTransformDirection(Quaternion.AngleAxis(rootJoint.minAngle, rootRotAxis) * toEnd);
maxOrient = spider.transform.InverseTransformDirection(Quaternion.AngleAxis(rootJoint.maxAngle, rootRotAxis) * toEnd);
// Now set the default position using the given stride, length and height
Vector3 defOrientation = Quaternion.AngleAxis(defaultOffsetStride * 0.5f * rootJoint.getAngleRange(), rootRotAxis) * midOrient;
Vector3 def = pivot;
def += (minDistance + 0.5f * (1f + defaultOffsetLength) * diameter) * defOrientation;
def += defaultOffsetHeight * 2f * spider.getColliderRadius() * rootRotAxis;
return(spider.transform.InverseTransformPoint(def));
}
// Solves the specific joint for the CCD solver
private static void solveJointCCD(ref JointHinge joint, ref Transform endEffector, ref TargetInfo target, float singularityRadius, bool adjustToTargetNormal)
{
Vector3 rotPoint = joint.getRotationPoint();
Vector3 rotAxis = joint.getRotationAxis();
Vector3 toEnd = Vector3.ProjectOnPlane((endEffector.position - rotPoint), rotAxis);
Vector3 toTarget = Vector3.ProjectOnPlane(target.position - rotPoint, rotAxis);
// If singularity, skip.
if (toTarget == Vector3.zero || toEnd == Vector3.zero)
{
return;
}
if (toTarget.magnitude < singularityRadius)
{
return; // Here even if adjustToTargetNormal is on i might not adjust if target is in this radius.
}
//if (toEnd.magnitude < singularityRadius) return; Notsure if i want this?
float angle;
//This is a special case, where i want the foot, that is the last joint of the chain to adjust to the normal it hit
if (adjustToTargetNormal)
{
angle = footAngleToNormal + 90.0f - Vector3.SignedAngle(Vector3.ProjectOnPlane(target.normal, rotAxis), toEnd, rotAxis);
}
else
{
angle = weight * joint.getWeight() * Vector3.SignedAngle(toEnd, toTarget, rotAxis);
}
joint.applyRotation(angle);
}