public void Forward()
{
//Debug.Log(EndEffector);
if (effectors.Count > 1)
{
effectors[1].Position = BaseEffector.Position + BaseEffector.Rotation * Vector3.forward * BaseEffector.Length;
}
for (int i = 2; i < effectors.Count; i++)
{
Vector3 direction = Vector3.Normalize(effectors[i].Position - effectors[i - 1].Position);
effectors[i - 1].ApplyConstraints(direction);
effectors[i].Position = effectors[i - 1].Position + effectors[i - 1].Rotation * Vector3.forward * effectors[i - 1].Length;
}
// This is a sub-base, reset Target to zero to be recalculated in Backward
if (children.Count != 0)
{
Target = Vector3.zero;
// In order to constrain a sub-base end effector, we must average the directions of its children
Vector3 direction = Vector3.zero;
foreach (FABRIKChain child in children)
{
direction += Vector3.Normalize(child.effectors[1].Position - EndEffector.Position);
}
direction /= (float)children.Count;
EndEffector.ApplyConstraints(direction);
}
}
public void AddEffector()
{
if (lastlimbindex != 0)
{
GameObject limb = Instantiate(Effector, Last_EndJointPosition, Quaternion.identity) as GameObject;
limb.name = "Efektor";
limb.transform.parent = limbs[FindLastJointIndex()].transform;
limb.GetComponent <EndEffector>().Target = Point2;
//if (limbs[0].GetComponent<RevoluteJoint>().innerTag == "VerticalRevoluteJoint")
// limb.GetComponent<EndEffector>().DefaultLength = Vector3.Magnitude(limb.transform.position);//- limbs[0].GetComponent<RevoluteJoint>().długośćKości;
limbs[lastlimbindex] = limb;
EffectorClass = limb.GetComponent <EndEffector>();
lastlimbindex++;
EndEffector = true;
}
}
public void Forward()
{
if (parent != null)
{
Vector3 direction = (effectors[1].Position - effectors[0].Position).normalized;
effectors[1].Position = effectors[0].transform.position + direction * effectors[0].Length;
effectors[0].Rotation = Quaternion.LookRotation(direction);
}
for (int i = 2; i < effectors.Count; i++)
{
Vector3 direction = Vector3.Normalize(effectors[i].Position - effectors[i - 1].Position);
effectors[i - 1].ApplyConstraints(direction);
effectors[i].Position = effectors[i - 1].Position + effectors[i - 1].Rotation * Vector3.forward * effectors[i - 1].Length;
}
// This is a sub-base, reset Target to zero to be recalculated in Backward
if (children.Count != 0)
{
Target = Vector3.zero;
// In order to constrain a sub-base end effector, we must average the directions of its children
Vector3 average = Vector3.zero;
foreach (FABRIKChain child in children)
{
Vector3 direction = (child.effectors[1].Position - EndEffector.Position).normalized;
child.effectors[1].Position = EndEffector.Position + direction * EndEffector.Length;
average += direction;
}
average /= (float)children.Count;
EndEffector.ApplyConstraints(average);
}
}
void Update()
{
if (lastlimbindex > 0 && EndEffector != true)
{
//Debug.Log(FindLastJointIndex());
RevoluteJoint joint = limbs[FindLastJointIndex()].GetComponent <RevoluteJoint>();
Last_EndJointPosition = joint.NextJointPosition;
}
if (EndEffector == true)
{
if (EffectorClass2 == null)
{
EffectorClass2 = FindEfectorObj().GetComponent <EndEffector>();
}
RevoluteJoint joint = limbs[FindLastJointIndex()].GetComponent <RevoluteJoint>();
Last_EndJointPosition = joint.NextJointPosition;
EffectorClass.LastPosition = limbs[FindLastJointIndex()].transform.position;
// Debug.Log(FindLastJointIndex());
}
}
private void EndEffectorTiltCW()
{
EndEffector endEffector = new EndEffector(10, 0);
ArduinoSerialPort.RunManualControl(endEffector);
}
private void EndEffectorRotationCW()
{
EndEffector endEffector = new EndEffector(0, 10);
ArduinoSerialPort.RunManualControl(endEffector);
}
public EndEffectorViewModel(EndEffector endEffector) : base(endEffector)
{
}
