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);
}
}
}
/**
* This method is used to automatically fix the errors in the joints (i.e. drift errors caused by numercial integration). At some future
* point it can be changed into a proper stabilization technique.
*/
public void fixJointConstraints(bool fixOrientations, bool fixVelocities)
{
if (!root)
{
return;
}
for (int i = 0; i < root.getChildJointCount(); i++)
{
root.getChildJoint(i).fixJointConstraints(fixOrientations, fixVelocities, true);
}
}
/**
* This method is used to automatically fix the errors in the joints (i.e. drift errors caused by numercial integration). At some future
* point it can be changed into a proper stabilization technique.
*/
public void fixJointConstraints(bool fixOrientations, bool fixVelocities, bool recursive)
{
if (child == null)
{
return;
}
//if it has a parent, we will assume that the parent's position is correct, and move the children to satisfy the joint constraint
if (parent != null)
{
//fix the orientation problems here... hopefully no rigid body is locked (except for the root, which is ok)
//first fix the relative orientation, if desired
if (fixOrientations)
{
//Quaternion qRel = computeRelativeOrientation();
//fixAngularConstraint(qRel);
}
//now worry about the joint positions
//compute the vector rc from the child's joint position to the child's center of mass (in world coordinates)
Vector3 rc = child.getWorldCoordinatesVector(Vector3.zero - cJPos);
//and the vector rp that represents the same quanity but for the parent
Vector3 rp = parent.getWorldCoordinatesVector(Vector3.zero - pJPos);
//the location of the child's CM is now: pCM - rp + rc
child.setCMPosition(parent.getCMPosition() + (rc - rp));
//fix the velocities, if need be
/*if (fixVelocities){
* //to get the relative velocity, we note that the child rotates with wRel about the joint (axis given by wRel
* //d = vector from joint position to CM of the child),
* //but it also rotates together with the parent with the parent's angular velocity,
* //so we need to combine these (all velocities are expressed in world coordinates already) (r is the vector
* //from the CM of the parent, to that of the child).
* float wRel = child.getAngularVelocity() - parent.getAngularVelocity();
* Vector2 r = child.getCMPosition() - parent.getCMPosition();
* Vector2 d = child.getCMPosition() - child.getWorldCoordinatesPoint(cJPos);
* Vector2 vRel = Vector3.Cross(parent.getAngularVelocity(), r) + Vector3.Cross(wRel, d);
* child.setCMVelocity(parent.getCMVelocity() + vRel);
* }*/
}
//make sure that we recursivley fix all the other joint constraints in the articulated figure
if (recursive)
{
for (int i = 0; i < child.getChildJointCount(); i++)
{
child.getChildJoint(i).fixJointConstraints(fixOrientations, fixVelocities, recursive);
}
}
}