private ownQuat hamiltonProduct2(ownQuat a, ownQuat b) { ownQuat ret = new ownQuat(); ret.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; ret.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; ret.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x; ret.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w; return(ret); }
private ownQuat RotToQuat(float yaw, float pitch, float roll) { ownQuat ret = new ownQuat(); // siehe WP: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles float cy = Convert.ToSingle(Math.Cos(yaw * 0.5 / 180.0 * Math.PI)); float cp = Convert.ToSingle(Math.Cos(pitch * 0.5 / 180.0 * Math.PI)); float cr = Convert.ToSingle(Math.Cos(roll * 0.5 / 180.0 * Math.PI)); float sy = Convert.ToSingle(Math.Sin(yaw * 0.5 / 180.0 * Math.PI)); float sp = Convert.ToSingle(Math.Sin(pitch * 0.5 / 180.0 * Math.PI)); float sr = Convert.ToSingle(Math.Sin(roll * 0.5 / 180.0 * Math.PI)); // Calculate ownQuat values ret.w = cy * cp * cr + sy * sp * sr; ret.x = cy * cp * sr - sy * sp * cr; ret.y = sy * cp * sr + cy * sp * cr; ret.z = sy * cp * cr - cy * sp * sr; return(ret); }
// supposed to remove gravity value from our Wiimote Accelerator... works more or less well // Ich habe das ganze zunächst mit Unity-Datenstrukturen (insb. Vector3 und Quaternion) probiert, später dann mit eigenen (insb. ownQuat) // Der Code mit den Unity-Strukturen ist noch auskommentiert // Wichtig dabei ist, dass Unity's Umwandlung von Vector3 in Quaternion eine andere Reihenfolge benutzt als meine Methode - deshalb ist die Reihenfolge der Rotationswerte // bei der initialisierung von v_rot anders als für die ownQuat-Srtuktur q_rot private Vector3 RemoveGravity3(float rotX, float rotY, float rotZ, float accelX, float accelY, float accelZ) { Vector3 ret = new Vector3(0, 0, 0); //Debug.Log("Raw/Input rotation data: (" + rotX + ", " + rotY + ", " + rotZ + ")"); //Debug.Log("Raw/Input acceleration data: (" + accelX + ", " + accelY + ", " + accelZ + ")"); // Create a vector for our acceleration and rotation values, this is only utilized when using Unity's Quaternions //Vector3 v_accel = new Vector3(accelY, accelX, accelZ); //Vector3 v_rot = new Vector3(rotZ, rotX, rotY); // Create a quaternion for our current rotation - atm it's an ownQuat //Quaternion q_rot = Quaternion.Euler(v_rot); //Debug.Log("Rotation Quaternion (w,x,y,z): (" + q_rot.w + ", " + q_rot.x + ", " + q_rot.y + ", " + q_rot.z + ")"); ownQuat q_rot = RotToQuat(rotY, rotX, rotZ); // Create a quaternion for our acceleration - this should not be normalized ... as a matter of fact it's not even a real quaternion, but who cares //Quaternion q_accel = new Quaternion(v_accel.x, v_accel.y, v_accel.z, 0); //Debug.Log("Acceleration Quaternion (w,x,y,z): (" + q_accel.w + ", " + q_accel.x + ", " + q_accel.y + ", " + q_accel.z + ")"); ownQuat q_accel = new ownQuat(); q_accel.w = 0; q_accel.x = accelY; q_accel.y = accelX; q_accel.z = accelZ; // Now we need to Multiply our Quaternion with our Vector - this should give us a "normalized" Vector (i.e. Gravity should then always point "down" [in z direction]) // It could be, that there is an easier (built-in) way to do this, but I haven't found it yet // DIe hier erzeugten Quaternions sind in Wahrheit keine - das ganze ist nur dafür da, damit man den Kram leichter "multiplizieren" kann... //Quaternion q_temp = hamiltonProduct(q_rot, q_accel); ownQuat q_temp = hamiltonProduct2(q_rot, q_accel); //Quaternion q_res = hamiltonProduct(q_temp, Quaternion.Inverse(q_rot)); ownQuat q_rinv = new ownQuat(); q_rinv.w = q_rot.w; q_rinv.x = -q_rot.x; q_rinv.y = -q_rot.y; q_rinv.z = -q_rot.z; ownQuat q_res = hamiltonProduct2(q_temp, q_rinv); //Debug.Log("Normalized accel vector: (" + q_res.x + ", " + q_res.y + ", " + q_res.z + ")"); //Debug.Log("Normalized Accel data: (" + v_accel_norm.x + ", " + v_accel_norm.y + ", " + v_accel_norm.z + ")"); // All that needs to be done now, is to remove the gravity data // Sanity check... this has driven me nuts - I used these values to correctly set up the rotation quaternion /*float roll = Mathf.Atan2(2 * q_rot.y * q_rot.w + 2 * q_rot.x * q_rot.z, 1 - 2 * q_rot.y * q_rot.y - 2 * q_rot.z * q_rot.z); * float pitch = Mathf.Atan2(2 * q_rot.x * q_rot.w + 2 * q_rot.y * q_rot.z, 1 - 2 * q_rot.x * q_rot.x - 2 * q_rot.y * q_rot.y); * float yaw = Mathf.Asin(2 * q_rot.x * q_rot.y + 2 * q_rot.z * q_rot.w);*/ // siehe ebenfalls WP float roll = Mathf.Atan2(2 * (q_rot.w * q_rot.x + q_rot.y * q_rot.z), 1 - 2 * (q_rot.x * q_rot.x + q_rot.y * q_rot.y)); float pitch = Mathf.Asin(2 * (q_rot.w * q_rot.y - q_rot.x * q_rot.z)); float yaw = Mathf.Atan2(2 * (q_rot.w * q_rot.z + q_rot.x * q_rot.y), 1 - 2 * (q_rot.y * q_rot.y + q_rot.z * q_rot.z)); Debug.Log("Roll: " + roll * 180 / Math.PI + " Pitch: " + pitch * 180 / Math.PI + " Yaw: " + yaw * 180 / Math.PI); // Remove about 0.95 from absolute of z value (i.e. shift it towards 0) q_res.z += (q_res.z < 0) ? 0.95f : -0.95f; //Quaternion q_temp2 = hamiltonProduct(Quaternion.Inverse(q_rot), q_res); ownQuat q_temp2 = hamiltonProduct2(q_rinv, q_res); //Quaternion q_orig = hamiltonProduct(q_temp2, q_rot); ownQuat q_cleared = hamiltonProduct2(q_temp2, q_rot); Debug.Log("cleansed vector: (" + q_cleared.x + ", " + q_cleared.y + ", " + q_cleared.z + ")"); ret.x = q_cleared.x; ret.y = q_cleared.y; ret.z = q_cleared.z; return(ret); }