38 #include "physical_constants.h"
43 float sinLat, sinLon, cosLat, cosLon;
45 sinLat = (float)sinf(DEG2RAD * LLA[0]);
46 sinLon = (float)sinf(DEG2RAD * LLA[1]);
47 cosLat = (float)cosf(DEG2RAD * LLA[0]);
48 cosLon = (float)cosf(DEG2RAD * LLA[1]);
50 Rne[0][0] = -sinLat * cosLon;
51 Rne[0][1] = -sinLat * sinLon;
56 Rne[2][0] = -cosLat * cosLon;
57 Rne[2][1] = -cosLat * sinLon;
64 float R13, R11, R12, R23, R33;
65 float q0s = q[0] * q[0];
66 float q1s = q[1] * q[1];
67 float q2s = q[2] * q[2];
68 float q3s = q[3] * q[3];
70 R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
71 R11 = q0s + q1s - q2s - q3s;
72 R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
73 R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]);
74 R33 = q0s - q1s - q2s + q3s;
85 rpy[1] = RAD2DEG * asinf(-R13);
86 rpy[0] = 2 * RAD2DEG * atan2f(q[3], q[0]);
89 }
else if (R13 <= -0.985
f) {
95 rpy[1] = RAD2DEG * asinf(-R13);
96 rpy[0] = -2 * RAD2DEG * atan2f(q[3], q[0]);
102 rpy[1] = RAD2DEG * asinf(-R13);
103 rpy[2] = RAD2DEG * atan2f(R12, R11);
104 rpy[0] = RAD2DEG * atan2f(R23, R33);
110 float phi, theta, psi;
111 float cphi, sphi, ctheta, stheta, cpsi, spsi;
113 phi = DEG2RAD * rpy[0] / 2;
114 theta = DEG2RAD * rpy[1] / 2;
115 psi = DEG2RAD * rpy[2] / 2;
118 ctheta = cosf(theta);
119 stheta = sinf(theta);
123 q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi;
124 q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi;
125 q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi;
126 q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi;
139 float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
141 Rbe[0][0] = q0s + q1s - q2s - q3s;
142 Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
143 Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
144 Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
145 Rbe[1][1] = q0s - q1s + q2s - q3s;
146 Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
147 Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);
148 Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);
149 Rbe[2][2] = q0s - q1s - q2s + q3s;
156 float sF = sinf(rpy[0]), cF = cosf(rpy[0]);
157 float sT = sinf(rpy[1]), cT = cosf(rpy[1]);
158 float sP = sinf(rpy[2]), cP = cosf(rpy[2]);
163 Rbe[1][0] = sF*sT*cP - cF*sP;
164 Rbe[1][1] = sF*sT*sP + cF*cP;
166 Rbe[2][0] = cF*sT*cP + sF*sP;
167 Rbe[2][1] = cF*sT*sP - sF*cP;
178 m[0] = 1 + R[0][0] + R[1][1] + R[2][2];
179 m[1] = 1 + R[0][0] - R[1][1] - R[2][2];
180 m[2] = 1 - R[0][0] + R[1][1] - R[2][2];
181 m[3] = 1 - R[0][0] - R[1][1] + R[2][2];
196 q[1] = (R[1][2]-R[2][1])/mag;
197 q[2] = (R[2][0]-R[0][2])/mag;
198 q[3] = (R[0][1]-R[1][0])/mag;
200 else if (index == 1) {
202 q[0] = (R[1][2]-R[2][1])/mag;
203 q[2] = (R[0][1]+R[1][0])/mag;
204 q[3] = (R[0][2]+R[2][0])/mag;
206 else if (index == 2) {
208 q[0] = (R[2][0]-R[0][2])/mag;
209 q[1] = (R[0][1]+R[1][0])/mag;
210 q[3] = (R[1][2]+R[2][1])/mag;
214 q[0] = (R[0][1]-R[1][0])/mag;
215 q[1] = (R[0][2]+R[2][0])/mag;
216 q[2] = (R[1][2]+R[2][1])/mag;
231 uint8_t
RotFrom2Vectors(
const float v1b[3],
const float v1e[3],
const float v2b[3],
const float v2e[3],
float Rbe[3][3])
233 float Rib[3][3], Rie[3][3];
246 if (fabsf(mag) < 1e-30
f)
249 Rib[0][i]=v1b[i]/mag;
252 if (fabsf(mag) < 1e-30
f)
255 Rie[0][i]=v1e[i]/mag;
260 if (fabsf(mag) < 1e-30
f)
263 Rib[1][i]=Rib[1][i]/mag;
267 if (fabsf(mag) < 1e-30
f)
270 Rie[1][i]=Rie[1][i]/mag;
281 Rbe[i][j] += Rib[k][i]*Rie[k][j];
294 if (angle <= 0.00048828125
f) {
305 q[0] = cosf(angle*0.5
f);
306 float scale = sinf(angle*0.5f) / angle;
316 void CrossProduct(
const float v1[3],
const float v2[3],
float result[3])
318 result[0] = v1[1]*v2[2] - v2[1]*v1[2];
319 result[1] = v2[0]*v1[2] - v1[0]*v2[2];
320 result[2] = v1[0]*v2[1] - v2[0]*v1[1];
326 return(sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
359 void quat_mult(
const float q1[4],
const float q2[4],
float qout[4])
361 qout[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
362 qout[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
363 qout[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
364 qout[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
374 void rot_mult(
float R[3][3],
const float vec[3],
float vec_out[3],
bool transpose)
377 vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2];
378 vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2];
379 vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2];
382 vec_out[0] = R[0][0] * vec[0] + R[1][0] * vec[1] + R[2][0] * vec[2];
383 vec_out[1] = R[0][1] * vec[0] + R[1][1] * vec[1] + R[2][1] * vec[2];
384 vec_out[2] = R[0][2] * vec[0] + R[1][2] * vec[1] + R[2][2] * vec[2];
void rot_mult(float R[3][3], const float vec[3], float vec_out[3], bool transpose)
Rotate a vector by a rotation matrix.
void Rv2Rot(float Rv[3], float R[3][3])
void Quaternion2R(float q[4], float Rbe[3][3])
void RPY2Quaternion(const float rpy[3], float q[4])
void quat_copy(const float q[4], float qnew[4])
Duplicate a quaternion.
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
float VectorMagnitude(const float v[3])
void quat_mult(const float q1[4], const float q2[4], float qout[4])
Multiply two quaternions into a third.
void RneFromLLA(float LLA[3], float Rne[3][3])
void R2Quaternion(float R[3][3], float q[4])
void Euler2R(float rpy[3], float Rbe[3][3])
Header for Coordinate conversions library in coordinate_conversions.c.
void CrossProduct(const float v1[3], const float v2[3], float result[3])
void quat_inverse(float q[4])
Compute the inverse of a quaternion.
uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])
void Quaternion2RPY(const float q[4], float rpy[3])