//Adds a segment node into the OFKT data structure
private void AddNode(Transform segment)
{
if (FindNode(segment) == null)
{
KinematicJoint joint = segment.GetComponent <KinematicJoint>();
Objective objective = segment.GetComponent <Objective>();
Node node = new Node(this, FindNode(segment.parent), segment, joint, objective);
if (joint != null)
{
if (joint.GetDoF() == 0)
{
joint = null;
}
else
{
if (joint.GetXMotion().IsEnabled())
{
MotionPtr motionPtr = new MotionPtr(joint.GetXMotion(), node, MotionPtrs.Length);
System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
node.XEnabled = true;
node.XIndex = motionPtr.Index;
}
if (joint.GetYMotion().IsEnabled())
{
MotionPtr motionPtr = new MotionPtr(joint.GetYMotion(), node, MotionPtrs.Length);
System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
node.YEnabled = true;
node.YIndex = motionPtr.Index;
}
if (joint.GetZMotion().IsEnabled())
{
MotionPtr motionPtr = new MotionPtr(joint.GetZMotion(), node, MotionPtrs.Length);
System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
node.ZEnabled = true;
node.ZIndex = motionPtr.Index;
}
}
}
if (objective != null)
{
System.Array.Resize(ref ObjectivePtrs, ObjectivePtrs.Length + 1);
ObjectivePtrs[ObjectivePtrs.Length - 1] = new ObjectivePtr(segment.GetComponent <Objective>(), node);
}
System.Array.Resize(ref Nodes, Nodes.Length + 1);
Nodes[Nodes.Length - 1] = node;
}
}
private void DrawJoint(KinematicJoint joint)
{
Vector3 connection = joint.GetAnchorInWorldSpace();
//DrawSphere(connection, JointSize, JointColor);
DrawCube(connection, joint.transform.rotation * Quaternion.Euler(joint.GetOrientation()), JointSize, JointColor);
DrawLine(joint.transform.position, joint.GetAnchorInWorldSpace(), JointColor);
//GUIStyle style = new GUIStyle();
//style.normal.textColor = Color.black;
//Handles.Label(connection, joint.name, style);
if (joint.GetXMotion().IsEnabled())
{
Handles.color = Color.red;
Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetXMotion().Axis), ArrowSize);
}
if (joint.GetYMotion().IsEnabled())
{
Handles.color = Color.green;
Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetYMotion().Axis), ArrowSize);
}
if (joint.GetZMotion().IsEnabled())
{
Handles.color = Color.blue;
Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetZMotion().Axis), ArrowSize);
}
}
public override void OnInspectorGUI()
{
Undo.RecordObject(Target, Target.name);
using (var scope = new EditorGUILayout.VerticalScope("Button")) {
EditorGUILayout.HelpBox("Geometry", MessageType.None);
Target.SetJointType((JointType)EditorGUILayout.EnumPopup("Joint Type", Target.GetJointType()));
Target.SetAnchor(EditorGUILayout.Vector3Field("Anchor", Target.GetAnchor()));
Target.SetOrientation(EditorGUILayout.Vector3Field("Orientation", Target.GetOrientation()));
}
using (var scope = new EditorGUILayout.VerticalScope("Button")) {
EditorGUILayout.HelpBox("Motion", MessageType.None);
Target.SetMotionType((MotionType)EditorGUILayout.EnumPopup("Motion Type", Target.GetMotionType()));
Target.SetSmoothing(EditorGUILayout.Slider("Smoothing", Target.GetSmoothing(), 0f, 1f));
if (Target.GetMotionType() == MotionType.Realistic)
{
Target.SetMaximumVelocity(EditorGUILayout.FloatField("Max Velocity", Target.GetMaximumVelocity()));
Target.SetMaximumAcceleration(EditorGUILayout.FloatField("Max Acceleration", Target.GetMaximumAcceleration()));
if (Target.GetMaximumVelocity() == 0f || Target.GetMaximumAcceleration() == 0f)
{
EditorGUILayout.HelpBox("Velocity and Acceleration must be assigned, or nothing will move.", MessageType.Warning);
}
}
DrawMotionInspector(Target.GetXMotion(), "X");
DrawMotionInspector(Target.GetYMotion(), "Y");
DrawMotionInspector(Target.GetZMotion(), "Z");
}
/*
* EditorGUILayout.HelpBox(
* "Current Value: " + Target.GetCurrentValue().ToString("F3") + "\n" +
* "Current Error: " + Target.GetCurrentError().ToString("F3") + "\n" +
* "Current Velocity: " + Target.GetCurrentVelocity().ToString("F3") + "\n" +
* "Current Acceleration: " + Target.GetCurrentAcceleration().ToString("F3"), MessageType.None);
*/
EditorUtility.SetDirty(Target);
/*
* using (var scope = new EditorGUILayout.VerticalScope ("Button")) {
* EditorGUILayout.HelpBox("Debug", MessageType.None);
* EditorGUILayout.Vector3Field("Anchor", Target.GetAnchor());
* EditorGUILayout.Vector3Field("World Anchor", Target.GetAnchorInWorldSpace());
* EditorGUILayout.Vector3Field("Orientation", Target.GetOrientation());
* EditorGUILayout.Vector3Field("X Axis", Target.GetXMotion().Axis);
* EditorGUILayout.Vector3Field("Y Axis", Target.GetYMotion().Axis);
* EditorGUILayout.Vector3Field("Z Axis", Target.GetZMotion().Axis);
* EditorGUILayout.Vector3Field("Default Reference Position", Target.GetDefaultReferencePosition());
* EditorGUILayout.Vector3Field("Default Reference Rotation", Target.GetDefaultReferenceRotation().eulerAngles);
* }
*/
}
//Recursively updates the transform information for the current frame
public void UpdateTransformations()
{
//Update Root
if (Chain.Joints.Length > 0)
{
Vector3 root = Chain.Joints[0].GetAnchorInWorldSpace();
RootX = root.x;
RootY = root.y;
RootZ = root.z;
}
else
{
RootX = Model.OPX;
RootY = Model.OPY;
RootZ = Model.OPZ;
}
//Local
if (Joint == null)
{
Vector3 lp = Segment.localPosition;
Quaternion lr = Segment.localRotation;
LPX = lp.x;
LPY = lp.y;
LPZ = lp.z;
LRX = lr.x;
LRY = lr.y;
LRZ = lr.z;
LRW = lr.w;
}
else
{
XValue = Joint.GetXMotion().GetTargetValue();
YValue = Joint.GetYMotion().GetTargetValue();
ZValue = Joint.GetZMotion().GetTargetValue();
Joint.ComputeLocalTransformation(XValue, YValue, ZValue, out LPX, out LPY, out LPZ, out LRX, out LRY, out LRZ, out LRW);
}
Vector3 ws = Segment.lossyScale;
WSX = ws.x;
WSY = ws.y;
WSZ = ws.z;
//World
ComputeWorldTransformation();
//Feed Forward
foreach (Node child in Childs)
{
child.UpdateTransformations();
}
}
private void ResetPosture(Transform t)
{
KinematicJoint joint = t.GetComponent <KinematicJoint>();
if (joint != null)
{
joint.GetXMotion().SetTargetValue(0f);
joint.GetYMotion().SetTargetValue(0f);
joint.GetZMotion().SetTargetValue(0f);
}
for (int i = 0; i < t.childCount; i++)
{
ResetPosture(t.GetChild(i));
}
}