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; } } }