private void _UpdateTargetCollider()
{
if (m_target)
{
m_targetCollider = m_target.GetComponent <Collider>();
if (m_targetCollider == null)
{
m_useRaycast = false;
}
}
else
{
Dbg.CLogWarn(this, "Floor._UpdateTargetCollider: not set m_target yet: {0}", name);
}
}
public override void Apply(ISolver solver, int jointIdx)
{
if (m_nextJoint == null)
{
Dbg.CLogWarn(this, "AngleConstraintMB.Apply: nextJoint not set");
return;
}
if (jointIdx == solver.Count)
{
return; //if no child joint, cannot apply angle constraint
}
var joints = solver.GetJoints();
Transform j = joints[jointIdx];
if (m_rotAxis == Vector3.zero)
{
Dbg.LogErr("AngleConstraintMB.Apply: the axis is zero vector");
return;
}
if (m_minLimit > m_maxLimit)
{
Misc.Swap(ref m_minLimit, ref m_maxLimit);
}
Transform nextJ = joints[jointIdx + 1];
Transform parentJ = j.parent; //THIS could be NULL, use Misc.TransformDirectoin/InverseTransformDirection
Vector3 jpos = j.position;
Vector3 nextJpos = nextJ.position;
Vector3 rotAxisWorld = Misc.TransformDirection(parentJ, m_rotAxis); //axis convert from parentSpace to worldSpace
// project to the rotation plane
Vector3 diff0 = nextJpos - jpos; //world space
Vector3 projDiff = Vector3.ProjectOnPlane(diff0, rotAxisWorld); //world space
Vector3 worldPrimAxis = Misc.TransformDirection(parentJ, m_primAxis);
float angle = Misc.ToAngleAxis(worldPrimAxis, projDiff, rotAxisWorld);
if (angle < m_minLimit || m_maxLimit < angle)
{ //need clamp
angle = Mathf.Clamp(angle, m_minLimit, m_maxLimit);
}
j.localRotation = Quaternion.AngleAxis(angle, m_rotAxis) * m_startLocalRot; //local
}
// private method
private void _OnNextJointChanged()
{
AngleConstraintMB mb = this;
var nextJoint = mb.m_nextJoint;
if (nextJoint != null)
{
if (nextJoint.parent == mb.transform)
{
AutoSetParameters();
}
else
{
Dbg.CLogWarn(mb, nextJoint.name + " is not children of " + mb.name);
mb.m_nextJoint = null;
}
}
}
public static void CLogWarn <T1, T2, T3, T4>(Object ctx, string fmt, T1 par1, T2 par2, T3 par3, T4 par4)
{
string msg = string.Format(fmt, par1, par2, par3, par4);
Dbg.CLogWarn(ctx, msg);
}
public static void CLogWarn <T1, T2>(Object ctx, string fmt, T1 par1, T2 par2)
{
string msg = string.Format(fmt, par1, par2);
Dbg.CLogWarn(ctx, msg);
}
/// <summary>
/// 1. rotate bone back from current rotation to refAxis; angle is "X"
/// 2. clamp X;
/// 3. rotate back with clamped X;
/// 4. calculate and clamp twist;
/// </summary>
public override void Apply(ISolver solver, int jointIdx)
{
if (m_nextJoint == null)
{
Dbg.CLogWarn(this, "ConeConstraintMB.Apply: nextJoint not set: {0}", name);
return;
}
Dbg.CAssert(this, m_angleLimit >= 0, "ConeConstraintMB.Apply: m_angleLimit should >= 0, but: {0}", m_angleLimit);
Dbg.CAssert(this, -180f <= m_minTwistLimit && m_minTwistLimit <= 180f, "ConeConstraintMB.Apply: minTwistLimit: {0}", m_minTwistLimit);
Dbg.CAssert(this, -180f <= m_maxTwistLimit && m_maxTwistLimit <= 180f, "ConeConstraintMB.Apply: maxTwistLimit: {0}", m_maxTwistLimit);
Dbg.CAssert(this, 0 <= m_angleLimit && m_angleLimit <= 180f, "ConeConstraintMB.Apply: angleLimit: {0}", m_angleLimit);
var joints = solver.GetJoints();
Transform j = joints[jointIdx];
Transform cj = m_nextJoint;
Transform pj = j.parent;
//1
Vector3 boneDirWorld = cj.position - j.position;
Vector3 refDirWorld = Misc.TransformDirection(pj, m_refAxis);
Quaternion q = Quaternion.FromToRotation(boneDirWorld, refDirWorld);
float angle; Vector3 rotAxis;
q.ToAngleAxis(out angle, out rotAxis);
angle = Misc.NormalizeAnglePI(angle);
j.rotation = q * j.rotation;
//Dbg.Log("coneconstraint: angle: {0}, rotAxis: {1}", angle, rotAxis);
//2
angle = Mathf.Clamp(angle, -m_angleLimit, m_angleLimit);
//3
j.Rotate(rotAxis, -angle, Space.World);
//4
if (m_limitTwist)
{ //use swing-twist decomposition of quaternion to limit twist
Quaternion deltaRot = j.localRotation * Quaternion.Inverse(m_initRot);
Vector3 curTwistAxisDirWorld = (m_nextJoint.position - tr.position).normalized;
Vector3 curTwistAxisDirParent = Misc.InverseTransformDirection(pj, curTwistAxisDirWorld);
Quaternion swingRot = Quaternion.FromToRotation(m_twistAxis, curTwistAxisDirParent);
Quaternion twistRot = Quaternion.Inverse(swingRot) * deltaRot;
Vector3 tmpAxis; float twist;
twistRot.ToAngleAxis(out twist, out tmpAxis);
twist = Misc.NormalizeAnglePI(twist);
if (float.IsInfinity(tmpAxis.x)) //SPECIAL case, some extreme data will make tmpAxis to be <inf,inf,inf>
{
tmpAxis = Vector3.right;
twist = 0;
}
if (Misc.IsObtuseAngle(tmpAxis, m_twistAxis))
{
twist = -twist;
tmpAxis = -tmpAxis;
}
twist = Mathf.Clamp(twist, m_minTwistLimit, m_maxTwistLimit);
twistRot = Quaternion.AngleAxis(twist, tmpAxis);
deltaRot = swingRot * twistRot;
var applied = deltaRot * m_initRot;
j.localRotation = applied;
}
}