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