public void setBodySpringStuff(CWRigidBody body, float spring, float damper, bool isFirst = true)
{
if (isFirst && body.getParentJoint() != null)
{
//Debug.Log(" ===== body === "+body.getParentJoint().jName);
body.getParentJoint().setSpring(spring);
body.getParentJoint().setDamper(damper);
if (body.name.Contains("LowerArm") || body.name.Contains("LowerLeg"))
{
body.getParentJoint().getParent().getParentJoint().setSpring(spring);
body.getParentJoint().getParent().getParentJoint().setDamper(damper);
}
}
int count = body.getChildJointCount();
for (int i = 0; i < count; i++)
{
CWJoint joint = body.getChildJoint(i);
joint.setSpring(spring);
joint.setDamper(damper);
//Debug.Log(" ===== child body === "+joint.jName);
if (joint.getChild().getChildJointCount() > 0)
{
setBodySpringStuff(joint.getChild(), spring, damper, false);
}
}
}
public ArticulatedFigure()
{
this.root = null;
this.name = "";
this.mass = 0;
this.joints = new ArrayList();
this.arbs = new ArrayList();
}
/**
* This method adds one rigid body (articulated or not).
*/
public void addArticulatedRigidBody(CWRigidBody _arb)
{
_arb.setAFParent(this);
arbs.Add(_arb);
if (_arb.getParentJoint() != null)
{
joints.Add(_arb.getParentJoint());
}
}
/**
* All quantities that are passed in as parameters here need to be expressed in the same "global" coordinate frame, unless otherwise noted:
* - p1: this is the location of the origin of the parent link
* - p2: this is the target location of the end effector on the child link
* - n: this is the default normal to the rotation plane (it will get modified as little as possible to account for the target location)
*
* - vParent: vector from parent origin to child origin, expressed in parent coordinates
* - nParent: this is the rotation axis, expressed in parent coordinates. The relative orientation between child and parent will be about this axis
* - vChild: vector from child origin, to position of the end effector, expressed in child coordinates
*
* Output:
* - qP: relative orientation of parent, relative to "global" coordinate frame
* - qC: relative orientation of child, relative to the parent coordinate frame
*
*
* NOTE: for now, the vector vChild is pretty much ignored. Only its length is taken into account. The axis that the child is lined up around is the same as the direction
* of the vector from the parent's origin to the child's origin (when there is zero relative orientation between the two, the axis has the same coordinates in both
* child and parent frame).
*/
public static void getIKOrientations(Controller con, CWJoint pJoint, CWRigidBody gParent, Vector3 p1, Vector3 p2, Vector3 n, Vector3 vParent, Vector3 nParent, Vector3 vChild, ref Quaternion qP, ref Quaternion qC)
{
//modify n so that it's perpendicular to the vector (p1, p2), and still as close as possible to n
Vector3 line = p2 - p1;
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(p2), Color.red);
Vector3 temp = Vector3.Cross(n, line);
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(p1) + 5*temp, Color.red);
Vector3 nG = Vector3.Cross(line, temp);
nG.Normalize();
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(p1) + gParent.getWorldCoordinatesVector(5*nG), Color.red);
//now compute the location of the child origin, in "global" coordinates
Vector3 solvedJointPosW = TwoLinkIK.solve(p1, p2, nG, vParent.magnitude, vChild.magnitude);
Vector3 vParentG = solvedJointPosW - p1;
Vector3 vChildG = p2 - solvedJointPosW;
//now we need to solve for the orientations of the parent and child
//if the parent has 0 relative orientation to the grandparent (default), then the grandparent's orientation is also the parent's
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(solvedJointPosW), gParent.getWorldCoordinatesPoint(solvedJointPosW+nG),Color.red);
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(solvedJointPosW),Color.red);
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p2), gParent.getWorldCoordinatesPoint(solvedJointPosW),Color.red);
if (con.getCharacter().getRoot() == gParent)
{
qP = TwoLinkIK.getParentLegOrientation(con, vParentG);
}
else
{
qP = TwoLinkIK.getParentArmOrientation(con, pJoint, gParent, vParentG, vChildG);
}
float childAngle = TwoLinkIK.getChildRotationAngle(vParentG, vChildG, nG);
if (Vector3.Dot(nG, nParent) < 0)
{
childAngle = -childAngle;
}
qC = Quaternion.AngleAxis(childAngle, Vector3.right);
//Debug.Log("aaaaaa "+qP.ToString("F4"));
//Debug.Log("bbbbbb "+qC.ToString("F4"));
}
/**
* Sets the root
*/
public void setRoot(CWRigidBody _root)
{
_root.setAFParent(this);
this.root = _root;
arbs.Add(_root);
}
/**
* This method determines the orientation for the parent link, relative to some other coordinate ("global") frame.
* Two vectors (one that goes from the parent origin to the child origin v, as well as a normal vector n) are known,
* expressed both in the global frame and the parent frame. Using this information, we can figure out the relative orientation
* between the parent frame and the global frame.
*
* Input:
* vGlobal - parent's v expressed in grandparent coordinates
* nGlobal - this is the rotation axis that is used for the relative rotation between the child and the parent joint, expressed in
* grandparent coordinates
* vLocal - parent's v expressed in the parent's local coordinates
* nLocal - this is the rotation axis that is used for the relative rotation between the child and the parent joint, expressed in
* parent's local coordinates
* Output:
* q - the relative orientation between the parent and the grandparent (i.e. transforms vectors from parent coordinates to grandparent coordinates).
*/
public static Quaternion getParentArmOrientation(Controller con, CWJoint pJoint, CWRigidBody gParent, Vector3 vParent, Vector3 vChild)
{
Quaternion q = Quaternion.identity;
Vector3 up = Vector3.zero, left = Vector3.zero, front = Vector3.zero;
Vector3 tPos = vParent + vChild;
float tangle = Vector3.Angle(tPos, con.getCharacter().getLocalUpAxis());
float vangle = Vector3.Angle(vParent, vChild);
//Debug.Log(tangle+ " aaaaaaa "+vangle + " aaaa "+tPos[con.getLocalFrontAxisID()]+" aaa "+tPos[con.getLocalLeftAxisID()]);
if (tangle <= 90)
{
if (tPos[con.getLocalFrontAxisID()] < 0 && vangle > 80)
{
up = -1 * vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.left, Vector3.down);
}
else
{
up = vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.right, Vector3.up);
}
}
else
{
if (tPos[con.getLocalFrontAxisID()] > 0 && vangle > 30)
{
up = vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.right, Vector3.up);
}
else
{
up = -1 * vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.left, Vector3.down);
}
}
return(Quaternion.Inverse(q) * Quaternion.LookRotation(up, front));
}
/**
* set the chil
*/
public void setChild(CWRigidBody c)
{
this.child = c;
child.setParentJoint(this);
}
public void setParent(CWRigidBody p)
{
this.parent = p;
this.parent.addChildJoint(this);
}
