private void AddNode(Transform segment)
{
if (FindNode(segment) == null)
{
KinematicJoint joint = segment.GetComponent <KinematicJoint>();
MotionPtr[] motions = new MotionPtr[3];
Node node = new Node(this, FindNode(segment.parent), segment, joint, motions);
if (joint != null)
{
if (joint.GetDOF() == 0)
{
joint = null;
}
else
{
if (joint.XMotion.State != JointState.Fixed)
{
MotionPtr motionPtr = new MotionPtr(joint.XMotion, node, Motions.Length);
System.Array.Resize(ref Motions, Motions.Length + 1);
Motions[Motions.Length - 1] = motionPtr;
motions[0] = motionPtr;
}
if (joint.YMotion.State != JointState.Fixed)
{
MotionPtr motionPtr = new MotionPtr(joint.YMotion, node, Motions.Length);
System.Array.Resize(ref Motions, Motions.Length + 1);
Motions[Motions.Length - 1] = motionPtr;
motions[1] = motionPtr;
}
if (joint.ZMotion.State != JointState.Fixed)
{
MotionPtr motionPtr = new MotionPtr(joint.ZMotion, node, Motions.Length);
System.Array.Resize(ref Motions, Motions.Length + 1);
Motions[Motions.Length - 1] = motionPtr;
motions[2] = motionPtr;
}
}
}
IKTip tip = segment.GetComponent <IKTip>();
if (tip != null)
{
System.Array.Resize(ref Tips, Tips.Length + 1);
Tips[Tips.Length - 1] = new TipPtr(segment.GetComponent <IKTip>(), node, Tips.Length);
}
System.Array.Resize(ref Nodes, Nodes.Length + 1);
Nodes[Nodes.Length - 1] = node;
}
}
private float ComputeFitness(float[] genes, bool balanced, bool backpropagate)
{
float fitnessSum = 0f;
Model.ApplyConfiguration(genes);
for (int i = 0; i < Model.Tips.Length; i++)
{
float fitness = 0f;
IKTip tip = Model.Tips[i].Tip;
Model.Node node = Model.Tips[i].Node;
switch (tip.Objective.Type)
{
case IKTip.ObjectiveType.Position:
fitness = tip.Weight * node.ComputeTranslationalDistance(tip.TPX, tip.TPY, tip.TPZ) / node.ComputeAngularScale();
break;
case IKTip.ObjectiveType.Orientation:
fitness = tip.Weight * node.ComputeRotationalDistance(tip.TRX, tip.TRY, tip.TRZ, tip.TRW);
break;
case IKTip.ObjectiveType.Pose:
if (balanced)
{
fitness =
tip.Weight *
(0.5f * node.ComputeTranslationalDistance(tip.TPX, tip.TPY, tip.TPZ) / node.ComputeAngularScale()
+ 0.5f * node.ComputeRotationalDistance(tip.TRX, tip.TRY, tip.TRZ, tip.TRW));
}
else
{
float weight = Random.value;
fitness =
tip.Weight *
(weight * node.ComputeTranslationalDistance(tip.TPX, tip.TPY, tip.TPZ) / node.ComputeAngularScale()
+ (1f - weight) * node.ComputeRotationalDistance(tip.TRX, tip.TRY, tip.TRZ, tip.TRW));
}
break;
case IKTip.ObjectiveType.LookAt:
fitness = tip.Weight * node.ComputeDirectionalError(tip.TPX, tip.TPY, tip.TPZ, tip.Objective.Direction);
break;
}
if (backpropagate)
{
node.BackpropagateHeuristicError(fitness);
}
fitnessSum += fitness;
}
return(fitnessSum);
}
private IKTip[] SearchIKTips(Transform t, List <IKTip> tips)
{
IKTip tip = t.GetComponent <IKTip>();
if (tip != null)
{
if (tip.isActiveAndEnabled)
{
tips.Add(tip);
}
}
for (int i = 0; i < t.childCount; i++)
{
SearchIKTips(t.GetChild(i), tips);
}
return(tips.ToArray());
}
public bool IsConverged(float[] configuration)
{
ApplyConfiguration(configuration);
for (int i = 0; i < Tips.Length; i++)
{
IKTip tip = Tips[i].Tip;
Node node = Tips[i].Node;
switch (tip.Objective.Type)
{
case IKTip.ObjectiveType.Position:
if (node.ComputeTranslationalDistance(tip.TPX, tip.TPY, tip.TPZ) > tip.Objective.MaximumPositionError)
{
return(false);
}
break;
case IKTip.ObjectiveType.Orientation:
if (node.ComputeRotationalDistance(tip.TRX, tip.TRY, tip.TRZ, tip.TRW) > tip.Objective.MaximumOrientationError)
{
return(false);
}
break;
case IKTip.ObjectiveType.Pose:
if (node.ComputeTranslationalDistance(tip.TPX, tip.TPY, tip.TPZ) > tip.Objective.MaximumPositionError ||
node.ComputeRotationalDistance(tip.TRX, tip.TRY, tip.TRZ, tip.TRW) > tip.Objective.MaximumOrientationError)
{
return(false);
}
break;
case IKTip.ObjectiveType.LookAt:
if (node.ComputeDirectionalError(tip.TPX, tip.TPY, tip.TPZ, tip.Objective.Direction) > tip.Objective.MaximumDirectionError)
{
return(false);
}
break;
}
}
return(true);
}
void Awake()
{
Target = (IKTip)target;
}
public TipPtr(IKTip tip, Node node, int index)
{
Tip = tip;
Node = node;
Index = index;
}