private float calcSPD(float currTheta, float targetTheta, float currVel, JsonPDController pd)
{
float thetaErr = targetTheta - currTheta;
float result = pd.Kp * thetaErr - pd.Kd * currVel;
return(result);
}
public float[] getTau(JsonAction action, Character character)
{
List <JsonPDController> jsonPDs = character.mjsonObject.PDControllers;
List <GameObject> bodies = character.mBodies;
int numJoint = jsonPDs.Count;
float[] targetTau = new float[numJoint];
float[] tauErr = new float[numJoint];
for (int i = 1; i < numJoint; i++)
{
targetTau [i] = 0;
JsonPDController pd = jsonPDs [i];
GameObject body = bodies [i];
HingeJoint joint = bodies [i].GetComponent <HingeJoint> ();
float currTheta = Mathf.Deg2Rad * joint.angle;
float currVel = Mathf.Deg2Rad * joint.velocity;
float targetTheta;
// if current joint is controlled by action, use that theta
if (pd.Name.Contains("spine"))
{
targetTheta = getTargetTheta(action.StateParams, "SpineCurve");
}
else if (CONTROL_PARAMS.Contains(pd.Name))
{
targetTheta = getTargetTheta(action.StateParams, pd.Name);
}
else
{
targetTheta = pd.TargetTheta;
}
JointSpring spring = joint.spring;
spring.targetPosition = Mathf.Rad2Deg * targetTheta;
joint.spring = spring;
//targetTau[i] += calcSPD(currTheta, targetTheta, currVel, pd);
int parent = character.mjsonObject.Skeleton.Joints[i].Parent;
float mass = character.mjsonObject.BodyDefs [i].Mass;
calcGravity(ref targetTau, body, joint, mass, i, parent);
// targetTau[i] += calcVirtual
tauErr [i] = targetTau [i] - mPastTau [i];
mPastTau [i] = targetTau [i];
}
return(tauErr);
}
public Character(TextAsset jsonText, GameObject refCube)
{
mjsonObject = JsonUtility.FromJson <JsonObject> (jsonText.text);
mBodies = new List <GameObject> ();
// make root
JsonJoint joint = mjsonObject.Skeleton.Joints [0];
JsonBodyDef body = mjsonObject.BodyDefs [0];
GameObject root = GameObject.Instantiate(refCube);
root.name = "root";
Mesh mesh = root.GetComponent <MeshFilter> ().mesh;
mesh.vertices = StretchMesh(mesh.vertices, body.Param0, body.Param1);
mesh.RecalculateBounds();
root.GetComponent <MeshCollider> ().sharedMesh = mesh;
mBodies.Add(root);
for (int i = 1; i < mjsonObject.Skeleton.Joints.Count; i++)
{
joint = mjsonObject.Skeleton.Joints [i];
body = mjsonObject.BodyDefs [i];
// info of parent body & joint
int parentIdx = joint.Parent;
JsonJoint parentJoint = mjsonObject.Skeleton.Joints [parentIdx];
JsonBodyDef parentBody = mjsonObject.BodyDefs [parentIdx];
GameObject parent = GameObject.Find(parentBody.Name);
//Vector3 scale = new Vector3 (body.Param0, body.Param1, 0.1f);
//refCube.transform.localScale = scale;
GameObject obj = (GameObject)GameObject.Instantiate(refCube);
obj.name = body.Name;
// set position of body part
Vector3 position = new Vector3(body.AttachX, body.AttachY) - new Vector3(parentBody.AttachX, parentBody.AttachY);
obj.transform.position = parent.transform.position + position + new Vector3(joint.AttachX, joint.AttachY);
// set joint
HingeJoint hingeJoint = obj.AddComponent <HingeJoint> ();
hingeJoint.connectedBody = parent.GetComponent <Rigidbody>();
hingeJoint.autoConfigureConnectedAnchor = false;
hingeJoint.anchor = -new Vector3(body.AttachX, body.AttachY);
hingeJoint.connectedAnchor = parent.transform.InverseTransformPoint(obj.transform.TransformPoint(hingeJoint.anchor));
hingeJoint.autoConfigureConnectedAnchor = true;
hingeJoint.axis = new Vector3(0, 0, 1);
hingeJoint.useSpring = true;
JointSpring spring = new JointSpring();
JsonPDController pd = mjsonObject.PDControllers [i];
spring.targetPosition = Mathf.Rad2Deg * pd.TargetTheta;
spring.spring = 20 * pd.Kd;
spring.damper = 0.1f * pd.Kp;
hingeJoint.spring = spring;
if (joint.LimLow < joint.LimHigh)
{
hingeJoint.useLimits = true;
JointLimits limit = new JointLimits();
limit.max = Mathf.Rad2Deg * joint.LimHigh;
limit.min = Mathf.Rad2Deg * joint.LimLow;
hingeJoint.limits = limit;
}
// resize mesh
mesh = obj.GetComponent <MeshFilter> ().mesh;
mesh.vertices = StretchMesh(mesh.vertices, body.Param0, body.Param1);
mesh.RecalculateBounds();
obj.GetComponent <MeshCollider> ().sharedMesh = mesh;
obj.GetComponent <Rigidbody> ().mass = body.Mass;
mBodies.Add(obj);
}
for (int i = 0; i < mjsonObject.Skeleton.Joints.Count; i++)
{
for (int j = i; j < mjsonObject.Skeleton.Joints.Count; j++)
{
Physics.IgnoreCollision(mBodies [i].GetComponent <MeshCollider>(), mBodies [j].GetComponent <MeshCollider>());
}
}
refCube.SetActive(false);
}
