void Start()
{
// float value = quaternionTest.Modulus();
// MULTIPLICATION
/*
* m = multiply.Multiply(q1, q2);
* Debug.Log(m.x + ":" + m.y + ":" + m.z + ":" + m.w);
*/
// Axis to Quaternion
//m.Axis2Quad(120, new Vector3(-0.5774f, 0.5774f, 0.5774f));
//Debug.Log(m.x + ":" + m.y + ":" + m.z + ":" + m.w);
// Quaternion to Axis
q2a = q2a.Quad2Axis(m);
//Debug.Log(q2a.x + ":" + q2a.y + ":" + q2a.z + ":" + q2a.w);
Quaternion angle1 = new Quaternion(3.5f, -10, 5.7f, 1.7f);
Quaternion angle2 = new Quaternion(9, -5.3f, +4, -6.6f);
Debug.Log("Ours: " + MyQuat.Angle(q1, q2));
Debug.Log("Unity: " + Quaternion.Angle(angle1, angle2));
}
// Update is called once per frame
void Update()
{
//acceleration & forces
perd.vel = new MyVec(magnuss.x, -gravity, perd.vel.z);
velocity = perd.vel;
magnuss = ((MyVec.Cross(perd.angularV, perd.vel)) * magnussCoef * airDens * crossSectionalArea * radius) * 0.5f;
drag = (MyVec.Scale(perd.vel, perd.vel) * proportionalCoef * crossSectionalArea * airDens) * -0.5f;
acc = (magnuss + drag + new MyVec(0, -gravity * mass, 0)) / mass;
MagnusX = magnuss.x;
DragX = drag.x;
DragY = drag.y;
DragZ = drag.z;
perd.pos.x += (magnuss.x - drag.x) * Time.deltaTime;
perd.pos.y -= (gravity - drag.y) * (Time.deltaTime);
perd.pos.z += (perd.vel.z - drag.z) * Time.deltaTime;
positions = perd.pos;
//rotation
float angle = perd.angularV.magnitude * Time.deltaTime;
angularD = perd.angularV;
rot = new MyQuat(angularD.x * Mathf.Sin(angle / 2), angularD.y * Mathf.Sin(angle / 2), angularD.z * Mathf.Sin(angle / 2), Mathf.Cos(angle / 2));
final = rot * new MyQuat(transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w);
transform.rotation = new Quaternion(final.x, final.y, final.z, final.w);
transform.position = new Vector3(perd.pos.x, perd.pos.y, perd.pos.z);
}
public static MyQuat AngleAxis(float angle, Vec3 axis)
{
Vec3 vn = Vec3.Normalize(axis);
angle = angle * Mathf.Deg2Rad;
MyQuat q = new MyQuat(Mathf.Cos(angle / 2), vn.x * Mathf.Sin(angle / 2), vn.y * Mathf.Sin(angle / 2), vn.z * Mathf.Sin(angle / 2));
return(q);
}
public static MyQuat operator *(MyQuat q, MyQuat r)
{
MyQuat t = new MyQuat();
t.w = (r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z);
t.x = (r.w * q.x + r.x * q.w - r.y * q.z + r.z * q.y);
t.y = (r.w * q.y + r.x * q.z + r.y * q.w - r.z * q.x);
t.z = (r.w * q.z - r.x * q.y + r.y * q.x + r.z * q.w);
return(t);
}
public MyQuat Multiply(MyQuat q, MyQuat r)
{
MyQuat t = new MyQuat();
t.w = (r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z);
t.x = (r.w * q.x + r.x * q.w - r.y * q.z + r.z * q.y);
t.y = (r.w * q.y + r.x * q.z + r.y * q.w - r.z * q.x);
t.z = (r.w * q.z - r.x * q.y + r.y * q.x + r.z * q.w);
return(t);
}
public static MyQuat Multiply(MyQuat q1, MyQuat q2)
{
MyQuat quaternionMultiplied = new MyQuat(0, 0, 0, 0);
quaternionMultiplied.w = (q2.w * q1.w - q2.x * q1.x - q2.y * q1.y - q2.z * q1.z);
quaternionMultiplied.x = (q2.w * q1.x + q2.x * q1.w - q2.y * q1.z + q2.z * q1.y);
quaternionMultiplied.y = (q2.w * q1.y + q2.x * q1.z + q2.y * q1.w - q2.z * q1.x);
quaternionMultiplied.z = (q2.w * q1.z - q2.x * q1.y + q2.y * q1.x + q2.z * q1.w);
return(quaternionMultiplied);
}
public MyQuat Quad2Axis(MyQuat q1)
{
MyQuat returnQuaternion = new MyQuat();
returnQuaternion.x = q1.x / Mathf.Sqrt(1 - q1.w * q1.w);
returnQuaternion.y = q1.y / Mathf.Sqrt(1 - q1.w * q1.w);
returnQuaternion.z = q1.z / Mathf.Sqrt(1 - q1.w * q1.w);
returnQuaternion.w = 2 * Mathf.Acos(q1.w) * Mathf.Rad2Deg;
return(returnQuaternion);
}
public MyQuat Conjugate()
{
MyQuat result = new MyQuat();
result.x = x * -1;
result.y = y * -1;
result.z = z * -1;
result.w = w;
return(result);
}
public static MyQuat FromAxisAngle(axisAngle axis)
{
MyQuat q = new MyQuat();
q.w = Mathf.Cos(axis.w / 2);
q.x = axis.x * Mathf.Sin(axis.w / 2);
q.y = axis.y * Mathf.Sin(axis.w / 2);
q.z = axis.z * Mathf.Sin(axis.w / 2);
q.Normalize();
return(q);
}
public MyQuat inverse()
{
MyQuat p = new MyQuat();
this.normalize();
p.x = -this.x;
p.y = -this.y;
p.z = -this.z;
p.w = this.w;
return(p);
}
public static float Angle(MyQuat q1, MyQuat q2)
{
q1.Normalization();
q2.Normalization();
q2.Inverse();
MyQuat newQuaternion = new MyQuat();
newQuaternion = MyQuat.Multiply(q1, q2);
return(2 * Mathf.Acos(newQuaternion.w) * Mathf.Rad2Deg);
}
//Operators
public static MyQuat operator *(MyQuat q, MyQuat p)
{
MyQuat m = new MyQuat();
m.w = (p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z);
m.x = (p.w * q.x + p.x * q.w - p.y * q.z + p.z * q.y);
m.y = (p.w * q.y + p.x * q.z + p.y * q.w - p.z * q.x);
m.z = (p.w * q.z - p.x * q.y + p.y * q.x + p.z * q.w);
m.Normalize();
return(m);
}
public MyQuat Inverse()
{
MyQuat p = new MyQuat();
Normalize();
p.x = -x;
p.y = -y;
p.z = -z;
p.w = w;
return(p);
}
public static MyQuat Normalize(MyQuat q)
{
MyQuat result = new MyQuat();
float magnitude = Mathf.Sqrt((q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w));
float scale = 1.0f / magnitude;
result.x = q.x * scale;
result.y = q.y * scale;
result.z = q.z * scale;
result.w = q.w * scale;
return(result);
}
void Start()
{
lineMat = m;
delta = 0.033f;
originalSliderpos = new Vec3(510, 1178, -239);
//mod2 = Vec3.Mod(originalSliderpos);
q = new MyQuat(transform.rotation);
//Esta fet aixi degut a que s han d iniialitzar tots els valors, un per un, de les posicions
fx = transform.position.x;
fy = transform.position.y;
fz = transform.position.z;
velocity = new Vec3(speed.x, speed.y, speed.z) * 20;
position = new Vec3(fx, fy, fz);
}
public static MyQuat Axis2Quad(float angle, MyVector3 v3)
{
MyQuat axisQuat = new MyQuat();
// RADIANES
v3.Normalize();
float radianAngle = angle * Mathf.Deg2Rad;
axisQuat.x = v3.x * Mathf.Sin(radianAngle / 2);
axisQuat.y = v3.y * Mathf.Sin(radianAngle / 2);
axisQuat.z = v3.z * Mathf.Sin(radianAngle / 2);
axisQuat.w = Mathf.Cos(radianAngle / 2);
axisQuat.Normalization();
return(axisQuat);
}
public MyQuat fromAxisAngle(float angle, MyVec axis)
{
//angle *= Mathf.Rad2Deg;
Debug.Log("angle :" + angle);
MyQuat q = new MyQuat();
q.w = Mathf.Cos(angle / 2);
q.x = axis.x * Mathf.Sin(angle / 2);
q.y = axis.y * Mathf.Sin(angle / 2);
q.z = axis.z * Mathf.Sin(angle / 2);
q.normalize();
Debug.Log("q.x" + q.x);
Debug.Log("q.y" + q.y);
Debug.Log("q.z" + q.z);
Debug.Log("q.w" + q.w);
return(q);
}
// Use this for initialization
void Start()
{
Time.timeScale = 0.01f;
magnussCoef = 0.15f;
airDens = 1;
radius = 0.5f;
proportionalCoef = 0.15f;
crossSectionalArea = (Mathf.Pow(radius, 2)) * Mathf.PI;
keys = GameObject.Find("InputManager").GetComponent <KeyManager>();
perd.pos = new MyVec(transform.position.x, transform.position.y, transform.position.z);
perd.vel = new MyVec(0, 0, 20) + keys.vent;
perd.angularV = new MyVec(0, angularY, 0);
uolo = new Vector3(perd.angularV.x, perd.angularV.y, perd.angularV.z);
gravity = 9.8f * mass;
//perd.forces = drag + gravity + magnuss;
move = new MyVec(transform.position.x, transform.position.y, transform.position.z);
init = new MyQuat(transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w);
}
//Static Methods
public static MyQuat AngleAxis(float angle, ref MyVec axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
MyQuat result = new MyQuat(0, 0, 0, 1);
float radians = angle * Mathf.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * Mathf.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = Mathf.Cos(radians);
return(Normalize(result));
}
void Update()
{
// Copy the joints positions to work with
for (int i = 0; i < joints.Length; i++)
{
copy[i] = new MyVec(joints[i].position.x, joints[i].position.y, joints[i].position.z); //Copy the joints
if (i < joints.Length - 1)
{
distances[i] = MyVec.Distance(joints[i + 1].position, joints[i].position); //Calculate the distances
}
}
done = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude < treshold_condition;
if (!done)
{
float targetRootDist = MyVec.Distance(copy[0], new MyVec(target.position.x, target.position.y, target.position.z));
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (new MyVec(target.position.x, target.position.y, target.position.z) - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + new MyVec(target.position.x, target.position.y, target.position.z) * lambda;
}
}
else
{
MyVec b = copy[0];
float difA = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude;
// The target is reachable
while (difA > treshold_condition)
{
// STAGE 1: FORWARD REACHING
copy[copy.Length - 1] = new MyVec(target.position.x, target.position.y, target.position.z);
for (int i = copy.Length - 2; i > 0; i--)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i] = copy[i + 1] * (1 - lambda) + copy[i] * lambda;
}
// STAGE 2: BACKWARD REACHING
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + copy[i + 1] * lambda;
}
difA = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i < joints.Length - 1; i++)
{
//TODO
MyVec A = new MyVec(joints[i + 1].position - joints[i].position);
MyVec B = copy[i + 1] - copy[i];
float cosa = MyVec.Dot(MyVec.Normalize(A), MyVec.Normalize(B));
float sina = MyVec.Cross(MyVec.Normalize(A), MyVec.Normalize(B)).magnitude;
float alpha = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
MyVec myAxis = MyVec.Normalize(MyVec.Cross(A, B));
//Vector3 axis = new Vector3(myAxis.x, myAxis.y, myAxis.z);
MyQuat myQuat = MyQuat.AngleAxis(alpha, ref myAxis);
Quaternion quat = new Quaternion(myQuat.x, myQuat.y, myQuat.z, myQuat.w);
joints[i].rotation = quat * joints[i].rotation;
//joints[i].rotation = Quaternion.AngleAxis(alpha, axis) * joints[i].rotation;
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
if (i == 2)
{
// print(joints[i].rotation.z);
}
if ((joints[i].rotation.z > 0.5f || joints[i].rotation.z < -0.5f) && i > 0)
{
joints[i].rotation = joints[i - 1].rotation;
//joints[i].rotation = new Quaternion(joints[i].rotation.x, joints[i].rotation.y, , joints[i - 1].rotation.w);
}
}
}
}
void Update()
{
vecTarget = new MyVec(target.position.x, target.position.y, target.position.z);
copy[0] = new MyVec(joints[0].transform.position.x, joints[0].transform.position.y, joints[0].transform.position.z);
// Copy the joints positions to work with
//TODO
for (int i = 0; i < joints.Length - 1; i++)
{
copy[i + 1] = new MyVec(joints[i + 1].transform.position.x, joints[i + 1].transform.position.y, joints[i + 1].transform.position.z);
distances[i] = (joints[i + 1].position - joints[i].position).magnitude;
}
done = (copy[copy.Length - 1] - vecTarget).length() < treshold_condition;
if (!done)
{
float targetRootDist = new MyVec().Distance(copy[0], vecTarget);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < joints.Length - 1; i++)
{
float r = new MyVec().Distance(vecTarget, copy[i]);
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * vecTarget);
}
}
else
{
// The target is reachable
MyVec b = new MyVec(copy[0].x, copy[0].y, copy[0].z);
float r = new MyVec().Distance(copy[copy.Length - 1], vecTarget);
while (r > treshold_condition)
{
// STAGE 1: FORWARD REACHING
//TODO
copy[copy.Length - 1] = vecTarget;
for (int i = copy.Length - 2; i > 0; i--)
{
float l = new MyVec().Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / l;
copy[i] = (1 - lambda) * copy[i + 1] + (lambda * copy[i]);
}
// STAGE 2: BACKWARD REACHING
//TODO
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float l = new MyVec().Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / l;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * copy[i + 1]);
}
r = new MyVec().Distance(copy[copy.Length - 1], vecTarget);
}
}
// Update original joint rotations
for (int i = 0; i < joints.Length - 1; i++)
{
//TODO
MyVec temp1 = new MyVec(joints[i + 1].position.x, joints[i + 1].position.y, joints[i + 1].position.z);
MyVec temp2 = new MyVec(joints[i].position.x, joints[i].position.y, joints[i].position.z);
MyVec init = temp1 - temp2;
init.normalize();
MyVec now = copy[i + 1] - copy[i];
now.normalize();
float cos = (new MyVec().dotProduct(init.normalized, now.normalized));
float sin = new MyVec().crossProduct(init.normalized, now.normalized).length();
print("cos" + cos);
print("sin" + sin);
float angle = Mathf.Atan2(sin, cos);//* Mathf.Rad2Deg;
MyVec axis = new MyVec().crossProduct(init.normalized, now.normalized).normalize();
MyQuat tempRot = new MyQuat(joints[i].rotation.x, joints[i].rotation.y, joints[i].rotation.z, joints[i].rotation.w);
/*print(axis.x);
* print(axis.y);
* print(axis.z);*/
if (angle != 0)
{
MyQuat finalRot = new MyQuat().fromAxisAngle(angle, axis.normalized) * tempRot;
/*print(finalRot.x);
* print(finalRot.y);
* print(finalRot.z);
* print(finalRot.w);*/
joints[i].rotation = new Quaternion(finalRot.x, finalRot.y, finalRot.z, finalRot.w);
}
Vector3 finalPos = new Vector3(copy[i].x, copy[i].y, copy[i].z);
joints[i].position = finalPos;
}
}
}
void Update()
{
// Copy the joints positions to work with
//TODO
copy[0] = new MyVector3(joints[0].position);
//copy[0] = joints[0].position;
for (int i = 0; i < joints.Length - 1; i++)
{
copy[i + 1] = new MyVector3(joints[i + 1].position);
distances[i] = (copy[i + 1] - copy[i]).Module();
//copy[i + 1] = joints[i + 1].position;
//distances[i] = (copy[i + 1] - copy[i]).magnitude;
}
// CALCULATE ALSO THE DISTANCE BETWEEN JOINTS
//done = TODO
done = (copy[copy.Length - 1] - new MyVector3(target.position)).Module() < tresholdCondition;
//done = (copy[copy.Length - 1] - target.position).magnitude < tresholdCondition;
if (!done)
{
float targetRootDist = (copy[0] - new MyVector3(target.position)).Module();
//float targetRootDist = Vector3.Distance(copy[0], target.position);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (new MyVector3(target.position) - copy[i]).Module();
//float r = (target.position - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * new MyVector3(target.position));
//copy[i + 1] = (1 - lambda) * copy[i] + (lambda * target.position);
}
}
else
{
MyVector3 b = copy[0];
//Vector3 b = copy[0];
// The target is reachable
//while (TODO)
float difference = (copy[copy.Length - 1] - new MyVector3(target.position)).Module();
//float difference = (copy[copy.Length - 1] - target.position).magnitude;
while (difference > tresholdCondition) // treshold = tolerance
{
// numIterations++;
// STAGE 1: FORWARD REACHING
//TODO
copy[copy.Length - 1] = new MyVector3(target.position);
//copy[copy.Length - 1] = target.position;
for (int i = copy.Length - 2; i > 0; i--)
{
float r = (copy[i + 1] - copy[i]).Module();
//float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i] = (1 - lambda) * copy[i + 1] + lambda * copy[i];
}
// STAGE 2: BACKWARD REACHING
//TODO
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (copy[i + 1] - copy[i]).Module();
//float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + lambda * copy[i + 1];
}
difference = (copy[copy.Length - 1] - new MyVector3(target.position)).Module();
//difference = (copy[copy.Length - 1] - target.position).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
// float originalAngle = joints[i].rotation.w;
MyQuat parentRotation = new MyQuat(joints[i + 1].rotation);
MyQuat childRotation = new MyQuat(joints[i].rotation);
//TODO
// Rotation
MyVector3 vectorA = new MyVector3(joints[i + 1].position) - new MyVector3(joints[i].position);
MyVector3 vectorB = copy[i + 1] - copy[i];
//Vector3 vectorA = joints[i + 1].position - joints[i].position;
//Vector3 vectorB = copy[i + 1] - copy[i];
// float angle = Mathf.Acos(Vector3.Dot(vectorA.normalized, vectorB.normalized)) * Mathf.Rad2Deg;
float cosA = (MyVector3.Dot(vectorA.Normalize(), vectorB.Normalize()));
float sinA = MyVector3.Cross(vectorA.Normalize(), vectorB.Normalize()).Module();
//float cosA = (Vector3.Dot(vectorA.normalized, vectorB.normalized));
//float sinA = Vector3.Cross(vectorA.normalized, vectorB.normalized).magnitude;
// Atan = Cos | Atan2 = denominador y...
float angle = Mathf.Atan2(sinA, cosA) * Mathf.Rad2Deg;
MyVector3 axis = MyVector3.Cross(vectorA, vectorB).Normalize();
//Vector3 axis = Vector3.Cross(vectorA, vectorB).normalized;
// joints[i].rotation = Quaternion.AngleAxis(angle, axis) * joints[i].rotation;
joints[i].rotation = MyQuat.Multiply(MyQuat.Axis2Quad(angle, axis), childRotation).ToUnityQuat();
//joints[i].rotation = MyQuat.Multiply(MyQuat.Axis2Quad(angle, axis), childRotation).ToUnityQuat();
childRotation = new MyQuat(joints[i].rotation);
float angleTest = MyQuat.Angle(parentRotation, childRotation);
if (Mathf.Abs(angleTest) > maxAngleRotation)
{
joints[i + 1].rotation = joints[i].rotation;
}
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
//joints[i + 1].position = copy[i + 1];
}
}
}
void Update()
{
// 1.Copy the joints positions to work with.
//Calculate also the distances between joints
//TODO1
Vec3 tpos = new Vec3(target.position);
copy[0] = new Vec3(joints[0].position);
for (int i = 0; i < copy.Length - 1; i++)
{
Vec3 temp = new Vec3(joints[i + 1].position);
copy[i + 1] = temp;
distances[i] = Vec3.Distance(copy[i + 1], copy[i]);
}
//done = TODO2
done = Vec3.Mod(copy[copy.Length - 1] - tpos) < treshold_condition;
if (!done)
{
float targetRootDist = Vec3.Distance(copy[0], tpos);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
//TODO3
for (int i = 0; i < copy.Length - 1; i++)
{
float r = Vec3.Distance(tpos, copy[i]);
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * tpos);
}
}
else
{
float comvulguis = Vec3.Distance(tpos, copy[copy.Length - 1]);
Vec3 b = copy[0];
int iter = 0;
// The target is reachable
while (comvulguis > treshold_condition && iter < maxIterations)
{
iter++;
// STAGE 1: FORWARD REACHING
//TODO
//Debug.Log("FABRIK iteration:" + iter);
copy[copy.Length - 1] = tpos;
for (int i = copy.Length - 2; i > 0; i--)
{
float r = Vec3.Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / r;
copy[i] = (1 - lambda) * copy[i + 1] + (lambda * copy[i]);
}
// STAGE 2: BACKWARD REACHING
//TODO
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = Vec3.Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * copy[i + 1]);
}
comvulguis = Vec3.Distance(copy[copy.Length - 1], tpos);
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
//TODO4
//without rotations of the different pieces:
//joints[i + 1].position = copy[i + 1];
//with rotations of the different pieces:
Vec3 temp1 = new Vec3(joints[i + 1].position);
Vec3 temp2 = new Vec3(joints[i].position);
Vec3 init = temp1 - temp2;
Vec3 now = copy[i + 1] - copy[i];
//float angle = Mathf.Acos(Vector3.Dot(init.normalized, now.normalized))*Mathf.Rad2Deg;
float cosa = Vec3.Dot(Vec3.Normalize(init), Vec3.Normalize(now));
float sina = Vec3.Mod(Vec3.Cross(Vec3.Normalize(init), Vec3.Normalize(now)));
float angle = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
Vec3 axis = Vec3.Normalize(Vec3.Cross(init, now));
MyQuat temp = new MyQuat(joints[i].rotation);
MyQuat q = MyQuat.AngleAxis(angle, axis) * temp;
Quaternion trueQ = new Quaternion(q.x, q.y, q.z, q.w);
joints[i].rotation = trueQ;
Vector3 v = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
joints[i + 1].position = v;
}
}
}
