private void OnDrawGizmos()
{
if (!drawGizmos)
{
return;
}
Gizmos.color = Color.white;
try
{
if (bones != null)
{
Random.InitState(10);
// draw the anchors
for (int i = 0; i < bones.Length; i++)
{
PlainMath.NextGizmosColor();
Gizmos.DrawSphere(getBonePos(i), gsize);
}
// draw the desired anchor position
for (int i = 0; i < chainLength + 1; i++)
{
Gizmos.color = Color.blue;
Gizmos.DrawSphere(positions[i], gsize);
}
}
}
catch
{
}
}
private void OnDrawGizmos()
{
Gizmos.color = Color.blue;
Gizmos.DrawSphere(transform.position + o1, size);
Gizmos.DrawSphere(transform.position + o2, size);
Gizmos.color = Color.red;
Gizmos.DrawSphere(transform.position + f, size);
Gizmos.color = Color.yellow;
Gizmos.DrawSphere(transform.position + pole, size);
Handles.DrawLine(o1 + transform.position, o2 + transform.position);
///////////////////////
/////////
////////////////
var target = PlainMath.GetClosestWithRespectVisualized(o1, o2, f, pole, transform.position);
//var target2 = PlainCalculator.getClosestWithRespectVisualized(o1 + Vector3.up * 5, o2 + Vector3.up * 5, f + Vector3.up * 5, pole + Vector3.up * 5, transform.position);
//PlainCalculator.getClosestWithRespectVisualized(o1 + transform.position, o2 + transform.position, f + transform.position, pole + transform.position, Vector3.up * 5);
//Gizmos.color = Color.grey;
//Gizmos.DrawSphere(target + transform.position, size);
}
void init()
{
positions = new Vector2[chainLength + 1];
bones = new AnchoredJoint2D[chainLength + 1];
bonesT = new Transform[chainLength + 1];
bonesR = new Rigidbody2D[chainLength + 1];
StartDir = new Vector2[chainLength + 1];
bonesLength = new float[chainLength];
avAngles = new float[chainLength];
angleOffset = new float[chainLength];
completeLength = 0;
var current = GetComponent <AnchoredJoint2D> ();
if (target)
{
var controller = target.GetComponent <IKController> ();
if (!controller)
{
controller = target.gameObject.AddComponent <IKController> ();
}
controller.Init(this);
}
for (int i = chainLength; i >= 0; i--)
{
bones[i] = current;
bonesT[i] = current.transform;
bonesR[i] = current.attachedRigidbody;
if (i == chainLength)
{
StartDir[i] = (Vector2)target.position - getBonePos(i);
}
else
{
var dir = getBonePos(i + 1) - getBonePos(i);
StartDir[i] = dir;
bonesLength[i] = dir.magnitude;
completeLength += bonesLength[i];
}
current = current.connectedBody.GetComponent <AnchoredJoint2D> ();
}
root = bones[0].connectedBody.transform;
rootrb = root.GetComponent <Rigidbody2D> ();
if (!rootrb)
{
rootrb = rootrb.GetComponentInParent <Rigidbody2D> ();
}
for (int i = 1; i <= chainLength; i++)
{
avAngles[i - 1] = PlainMath.AngleBetween(getBonePos(i), root.position);
angleOffset[i - 1] = PlainMath.AngleFromDirection(getBonePosVR2(i - 1) - getBonePosVR2(i)) - root.eulerAngles.z;
//Debug.Log(angleOffset[i - 1] + " , " + bonesT[i].name + " :: " + root.eulerAngles.z + " :: " + bonesT[i].eulerAngles.z);
//Debug.Log(AngleFromDirection((Vector2)root.position - getBonePos(i)) + " , " + bonesT[i].na);
}
//Debug.Log(root + " , " + rootrb);
}
private void ApplyByTorque()
{
float vmd = 10 / rootrb.velocity.sqrMagnitude;
vmd = Mathf.Clamp01(vmd);
var fxs50 = 25 * Time.fixedDeltaTime;
for (int i = 1; i <= chainLength; i++)
{
var t = PlainMath.AngleFromDirection(positions[i - 1] - positions[i]);
var angle = -Mathf.DeltaAngle(bonesT[i - 1].eulerAngles.z + angleOffset[i - 1], t);
var x = angle > 0 ? 1 : -1;
var vel = bonesR[i - 1].angularVelocity;
var a = (chainLength + 2 - i);
var index = chainLength - i + 1;
angle = Mathf.Abs(angle) * 0.1f;
if (angle < 1)
{
angle = angle * angle;
}
//bonesR[i - 1].angularVelocity = 0;
var rv2 = vel * fxs50 * reflectionForce * vmd;
rv2 = Mathf.Clamp(rv2, -force * 0.2f, force * 0.2f);
rootrb.AddTorque(rv2);
bonesR[i - 1].AddTorque(-rv2);
var aaf = Mathf.Log10((chainLength - i) * 10 + 10);
var f = angle * x * force * torqueStrength * aaf * fxs50 * 6;
//f = Mathf.Clamp(f, -270, 270);
rootrb.AddTorque(f);
bonesR[i - 1].AddTorque(-f);
}
}
private void OnDrawGizmos()
{
var current = this.transform;
if (pole)
{
var positions = new Vector3[chainLength + 1];
current = this.transform;
Random.InitState(10);
for (int i = 0; i < (chainLength + 1) && current != null && current.parent != null; i++)
{
positions[i] = current.position;
current = current.parent;
Gizmos.color = Color.HSVToRGB(Random.value, 1, 1);
Gizmos.DrawSphere(positions[i], 0.25f);
}
for (int i = 1; i < chainLength; i++)
{
PlainMath.GetClosestWithRespectVisualized(positions[i - 1], positions[i + 1],
positions[i], pole.position, Vector3.zero);
}
}
current = this.transform;
for (int i = 0; i < chainLength && current != null && current.parent != null; i++)
{
var scale = Vector3.Distance(current.position, current.parent.position) * 0.1f;
Handles.matrix = Matrix4x4.TRS(current.position, Quaternion.FromToRotation(Vector3.up,
current.parent.position - current.position), new Vector3(scale,
Vector3.Distance(current.parent.position, current.position), scale));
Handles.color = Color.green;
Handles.DrawWireCube(Vector3.up * 0.5f, Vector3.one);
current = current.parent;
}
}
void Solve()
{
if (target == null)
{
return;
}
if (bonesLength.Length != chainLength)
{
init();
}
// nothing much to say here
Vector2 targetPos = target.position;
for (int i = 0; i < chainLength + 1; i++)
{
positions[i] = getBonePos(i);
}
// calculate the desireble position if the target is outside reach
var direction = positions[0] - targetPos;
if (direction.sqrMagnitude > completeLength * completeLength)
{
for (int i = 1; i < chainLength + 1; i++)
{
positions[i] = positions[i - 1] - direction.normalized * bonesLength[i - 1];
}
}
else
{
// in here we want to "get back" to the origin starting poss,
// just to mentain some basic form.
for (int i = 0; i < chainLength; i++)
{
positions[i + 1] = Vector3.Lerp(positions[i + 1],
positions[i] + StartDir[i], SnapStrength);
}
// the old good calculation for inverse ik, for 3d but with 2d vectors.
// same stuff.
for (int k = 0; k < iterations; k++)
{
// first step lets assume we are onto the target
positions[chainLength] = targetPos;
for (int i = chainLength - 1; i > 0; i--)
{
//second step lets calculate out way from each position toward wahtever direction we are in currently.
positions[i] = positions[i + 1] + (positions[i] - positions[i + 1]).normalized * bonesLength[i];
}
for (int i = 1; i < chainLength + 1; i++)
{
//this step, lets make sure our bones are actually not stranded off from the root positiion.
var dir = positions[i] - positions[i - 1];
positions[i] = positions[i - 1] + dir.normalized * bonesLength[i - 1];
}
// if we are close enough lets get out
if ((targetPos - positions[chainLength]).sqrMagnitude < delta * delta)
{
break;
}
}
if (pole)
{
// if there is pole we want to make sure we are facing it.
Vector2 polePos = (pole.position - root.position) * 100 + root.position;
for (int i = 1; i < chainLength; i++)
{
var dir = positions[i + 1] - positions[i - 1];
// we want to get the closest point in the line between pos - 1, + 1, to out current position
// then we will know in what direction the pole, and the currenct pose facing relative to the line
var closest = PlainMath.ClosestOnLine(positions[i + 1], dir, positions[i]);
var ttoCenter = closest - positions[i];
var ptoCenter = closest - polePos;
// if we are facing the opposite derection from the center, as the pole we will flip out direction
// note that it will have the same distance from -1, +1, just flipped direction
// also we dont wanna flip anything if the pole to the center, reason being,
// we will get to much flipping (in one frame the position is the same direciton as us, and the other frame its
// the opposide derection)
if (Vector2.Dot(ttoCenter, ptoCenter) < 0)
{
positions[i] = closest + ttoCenter;
}
//var test = Vector2.SignedAngle
}
}
}
}
private void Solver()
{
if (target == null)
{
return;
}
if (bonesLength.Length != chainLength)
{
Init();
}
var targetPos = target.position;
var rootRot = Root.rotation;
for (int i = 0; i < chainLength + 1; i++)
{
positions[i] = bones[i].position;
}
Vector3 direction = bones[0].position - targetPos;
if (direction.sqrMagnitude > completeLength * completeLength)
{
for (int i = 1; i < chainLength + 1; i++)
{
positions[i] = positions[i - 1] - direction.normalized * bonesLength[i - 1];
}
}
else
{
//positions[chainLength] = target.position;
for (int k = 0; k < iterations; k++)
{
if (!pole)
{
for (int i = 0; i < chainLength; i++)
{
positions[i + 1] = Vector3.Lerp(positions[i + 1],
positions[i] + StartDir[i], snapBackStrength);
}
}
positions[chainLength] = targetPos;
for (int i = chainLength - 1; i > 0; i--)
{
positions[i] = positions[i + 1] + (positions[i] - positions[i + 1]).normalized * bonesLength[i];
}
for (int i = 1; i < chainLength + 1; i++)
{
direction = (positions[i] - positions[i - 1]).normalized;
positions[i] = positions[i - 1] + direction * bonesLength[i - 1];
}
if ((targetPos - positions[chainLength]).sqrMagnitude < delta * delta)
{
break;
}
}
}
if (pole)
{
var polePos = pole.position;
for (int i = chainLength - 1; i > 0; i--)
{
positions[i] = PlainMath.CalculateClosestToRinPlaneABwD(positions[i - 1], positions[i + 1],
positions[i], polePos);
}
for (int i = 1; i < chainLength; i++)
{
positions[i] = PlainMath.CalculateClosestToRinPlaneABwD(positions[i - 1], positions[i + 1],
positions[i], polePos);
}
}
for (int i = 0; i < chainLength + 1; i++)
{
bones[i].position = positions[i];
if (i != chainLength)
{
// bones[i].rotation = Quaternion.FromToRotation(StartDir[i], (positions[i + 1] - positions[i]).normalized)
// * Quaternion.Inverse(startRotations[i]);
Vector3 dir = rootRot * Vector3.up;
bones[i].rotation = Quaternion.FromToRotation(dir, (positions[i + 1] - positions[i]));
//bones[i].rotation = Quaternion.LookRotation(startRotations[i], bones[i].up);
}
else
{
bones[i].localRotation = target.rotation;
}
if (Root)
{
bones[i].rotation *= rootRot;
}
}
}
