179 // transform.orientation.Inverse(camera.cOrientation);

180 {

181 Quaternion iquat;

182 iquat.Inverse(camera.cOrientation);

183 transform.orientation = iquat;

83

84 scalar magOmega = mag(omega);

85 quaternion R(omega/magOmega, magOmega);

86 septernion TR(septernion(origin_)*R*septernion(-origin_));

87

26 * The output table specifies the orientation in one of three ways:

27 *

28 * default - quaternions in the order x, y, z, w.

29 *

30 * -z option - Eulers angles, in the order xyz. The model axes are

53 #define YPR 0

54 #define XYZ 1

55 #define QUATERNION 2

56

57 #define DEGREES 0

73 int c;

74

75 mode = QUATERNION; /* default */

76 units = DEGREES;

77

105 int signYaw = 1, signPitch = 1;

106 Radians yaw = spin.yaw, pitch = spin.pitch;

107 Quaternion orientation = cube.transform.orientation;

108 Euler tSpin { yaw * (double)diffTime, pitch * (double)diffTime, 0 };

109 Quaternion thisSpin = tSpin, temp;

179

180

181 #ifdef QUATERNION_SUPPORT

182

183 /* Quaternions */

233 ASSIGN_QUAT(qdst,tmpq) )

234

235 // Quaternion division is like complex division: q1 / q2 = q1 * conj(q2) / maqsq(q2)

236 // Conjugation negates all three imaginary components...

237

269

270

271 /* float times quaternion */

272

273 // These appear to have a quaternion destination, but a real source,

86

87 module = liscr_script_get_userdata (args->script, LIEXT_SCRIPT_SOUND);

88 liscr_args_seti_quaternion (args, &module->listener_rotation);

89 }

90 static void Sound_set_listener_rotation (LIScrArgs* args)

91 {

92 LIExtModule* module;

93 LIMatQuaternion value;

94

95 if (liscr_args_geti_quaternion (args, 0, &value))

44 #include "transformField.H"

45 #include "Pair.H"

46 #include "quaternion.H"

47 #include "mathematicalConstants.H"

48

150 v *= pi/180.0;

151

152 quaternion R(v.x(), v.y(), v.z());

153

154 Info<< "Rotating points by quaternion " << R << endl;

170 scalar roll = v.z();

171

172 quaternion R = quaternion(vector(0, 0, 1), yaw);

173 R *= quaternion(vector(0, 1, 0), pitch);

174 R *= quaternion(vector(1, 0, 0), roll);

175

176 Info<< "Rotating points by quaternion " << R << endl;

24 *

25 * Rotorcraft attitude reference generation.

26 * (quaternion float version)

27 *

28 */

100 #define DT_UPDATE (1./PERIODIC_FREQUENCY)

101

102 // default to fast but less precise quaternion integration

103 #ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP

104 #define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE

116 struct FloatQuat delta_q;

117 FLOAT_QUAT_DIFFERENTIAL(delta_q, stab_att_ref_rate, DT_UPDATE);

118 /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */

119 struct FloatQuat new_ref_quat;

120 FLOAT_QUAT_COMP(new_ref_quat, delta_q, stab_att_ref_quat);

297 return 0;

298 index += self->args_start;

299 tmp = liscr_isdata (self->lua, index, LISCR_SCRIPT_QUATERNION);

300 if (tmp != NULL)

301 *result = *((LIMatQuaternion*) tmp->data);

830 if (self->output_mode != LISCR_ARGS_OUTPUT_TABLE)

831 {

832 quat = liscr_data_new_alloc (self->script, self->lua, sizeof (LIMatQuaternion), LISCR_SCRIPT_QUATERNION);

833 if (quat != NULL)

834 {

845 }

846 lua_pushnumber (self->lua, ++self->ret);

847 quat = liscr_data_new_alloc (self->script, self->lua, sizeof (LIMatQuaternion), LISCR_SCRIPT_QUATERNION);

848 if (quat != NULL)

849 {

6 * The output table specifies the orientation in one of three ways:

7 *

8 * default - quaternions in the order x, y, z, w.

9 *

10 * -z option - Eulers angles, in the order xyz. The model axes are

45 #define YPR 0

46 #define XYZ 1

47 #define QUATERNION 2

48

49 #define DEGREES 0

113 printf("%f\t%f\t%f\n",angle[X],angle[Y],angle[Z]);

114 }

115 else if (mode==QUATERNION){

116 mat2quat(quat,viewrot);

117 printf("%f\t%f\t%f\t%f\n",quat[X],quat[Y],quat[Z],quat[W]);

109 TRV[1] *= pi/180.0;

110

111 quaternion R(TRV[1].x(), TRV[1].y(), TRV[1].z());

112 septernion TR(septernion(CofG_ + TRV[0])*R*septernion(-CofG_));

113

32 /* maintains a rotation matrix representation */

33 struct FloatEulers bafe_dcm;

34 /* time derivative of our quaternion */

35 struct FloatQuat bafe_qdot;

36

66 * Q is our estimate noise variance. It is supposed to be an NxN

67 * matrix, but with elements only on the diagonals. Additionally,

68 * since the quaternion has no expected noise (we can't directly measure

69 * it), those are zero. For the gyro, we expect around 5 deg/sec noise,

70 * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.

98 * bias_dot = 0

99 *

100 * Wxq is the quaternion omega matrix:

101 *

102 * [ 0, -p, -q, -r ]

26 * the code, a code indicating the argument types, a mask indicating

27 * what precisions (and combinations) are allowed, and a mask

28 * indicating what types (real/complex/quaternion/mixed/etc)

29 * are allowed.

30 */

5 * t=popmat(t) pop matrix stack

6 * rot(t, a, axis) multiply stack top by rotation

7 * qrot(t, q) multiply stack top by rotation, q is unit quaternion

8 * scale(t, x, y, z) multiply stack top by scale

9 * move(t, x, y, z) multiply stack top by translation

54 ixform(t, m, inv);

55 }

56 void qrot(Space *t, Quaternion q){

57 Matrix m, inv;

58 int i, j;

31

32 const char* const Foam::septernion::typeName = "septernion";

33 const Foam::septernion Foam::septernion::zero(vector::zero, quaternion::zero);

34 const Foam::septernion Foam::septernion::I(vector::zero, quaternion::I);

102 if (pd->orientation_inherit)

103 {

104 evas_vec4_quaternion_multiply(&pd->orientation_world,

105 orientation_parent, &pd->orientation);

106 }

120 {

121 evas_vec3_multiply(&pd->position_world, &pd->position, scale_parent);

122 evas_vec3_quaternion_rotate(&pd->position_world, &pd->position_world,

123 orientation_parent);

124 evas_vec3_add(&pd->position_world, &pd->position_world,

265 Evas_Vec3 rotate;

266 evas_vec3_set(&rotate, minmaxdata[0], minmaxdata[1], minmaxdata[2]);

267 evas_vec3_quaternion_rotate(&rotate, &rotate, &orientation);

268 vxmax = vxmin = minmaxdata[0] = rotate.x;

269 vymax = vymin = minmaxdata[1] = rotate.y;

26

27 /*!

28 * \defgroup quat Quaternion Mathematics

29 *

30 * \author J.E. Hoffmann <je-h@gmx.net>

72 }

73

74 void MultQuaternion(const Vector3D s, const Quaternion quat)

75 {

76 Vector3D v { quat.x, quat.y, quat.z };

251 }

252

253 void MultQuaternion(const Vector3Df source, const Quaternion quat)

254 {

255 Vector3D s { source.x, source.y, source.z };

177

178

179 #define FIRST_QUATERNIONS \

180 case _user_: user_CASE; \

181 case _real_: real_CASE; \

205

206 switch(LOWER_DWORD(val)) {

207 FIRST_QUATERNIONS;

208 default:

209 PARSE_SHORT;

81 aveq( "cross", 3, vr, mr, 1e-9 );

82

83 /* QUATERNION-VECTOR */

84

85 // tf

17 * bias_dot = 0

18 *

19 * Wxq is the quaternion omega matrix:

20 *

21 * [ 0, -p, -q, -r ]

166 }

167

168 if (!strcmp(s->s_name,"quaternion")) {

169 x->x_dataformat = BIRDCMD_MODE_QUAT;

170 x->x_maxcount = BIRD_BYTES_QUAT;

99

100 /* Suppose that you have a rotation matrix that needs to be */

101 /* converted to a quaternion. The SPICE matrix to quaternion */

102 /* conversion routine M2Q performs error checks on the input */

103 /* matrix and signals an error if it does not meet the checks */

104 /* for a quaternion. By calling this routine you can ensure that */

105 /* your rotation matrix (provided it's non-singular) will pass */

106 /* the restrictions imposed by M2Q. */

80

81 /**

82 * Converts a quaternion to a rotation matrix.

83 * @param[in] quat 4-element quaternion in fixed point. One is 2^30.

88 * to World.

89 */

90 void inv_quaternion_to_rotation(const long *quat, long *rot)

91 {

92 rot[0] =

83

84 /* Dot product of 3D components, the scalar component of the intermediate *

85 * quaternion. */

86 GLfloat dot = -v3Dot( qTmp.xyz, v );

87

88 /* 3D component of quaternion conjugate. */

89 vec3 conj = v3Scale( -1.0f, qTmp.xyz );

90

91 /* Vector portion of the intermediate quaternion. */

92 vec3 intVec = v3Scale( qTmp.w, v ) + v3Cross( qTmp.xyz, v );

93

200 uint32_t type;

201 LIMatVector position;

202 LIMatQuaternion rotation;

203

204 /* Read header. */

445 LIMatVector* z)

446 {

447 *x = limat_quaternion_transform (self->rest_transform.global.rotation, limat_vector_init (1.0f, 0.0f, 0.0f));

448 *y = limat_quaternion_transform (self->rest_transform.global.rotation, limat_vector_init (0.0f, 1.0f, 0.0f));

449 *z = limat_quaternion_transform (self->rest_transform.global.rotation, limat_vector_init (0.0f, 0.0f, 1.0f));

450 }

451

182

183 liren_light_get_transform (args->self, &transform);

184 liscr_args_seti_quaternion (args, &transform.rotation);

185 }

186 static void Light_set_rotation (LIScrArgs* args)

187 {

188 LIMatTransform transform;

189 LIMatQuaternion value;

190

191 if (liscr_args_geti_quaternion (args, 0, &value))

39 {

40 D3DXMATRIX m1, m2, m3, m4, m5;

41 D3DXQUATERNION rot;

42 D3DXVECTOR3 rot_center, trans;

43

76 D3DXMatrixTranslation(&m2, -rot_center.x, -rot_center.y, -rot_center.z);

77 D3DXMatrixTranslation(&m4, rot_center.x, rot_center.y, rot_center.z);

78 D3DXMatrixRotationQuaternion(&m3, &rot);

79 D3DXMatrixTranslation(&m5, trans.x, trans.y, trans.z);

80

90 * D3DXMatrixDecompose

91 */

92 HRESULT WINAPI D3DXMatrixDecompose(D3DXVECTOR3 *poutscale, D3DXQUATERNION *poutrotation, D3DXVECTOR3 *pouttranslation, D3DXMATRIX *pm)

93 {

94 D3DXMATRIX normalized;

166 return q;

167 }

168 Quaternion qmul(Quaternion q, Quaternion r){

169 Quaternion s;

174 return s;

175 }

176 Quaternion qdiv(Quaternion q, Quaternion r){

177 return qmul(q, qinv(r));

178 }

199 return sqrt(p.r*p.r+p.i*p.i+p.j*p.j+p.k*p.k);

200 }

201 Quaternion slerp(Quaternion q, Quaternion r, double a){

202 double u, v, ang, s;

203 double dot=q.r*r.r+q.i*r.i+q.j*r.j+q.k*r.k;

216 * Only works if qlen(q)==qlen(r)==1

217 */

218 Quaternion qmid(Quaternion q, Quaternion r){

219 double l;

220 q=qadd(q, r);

223

224 /* ***** */

225 mth_Quaternion mth_quatMul(const mth_Quaternion *q1, const mth_Quaternion *q2)

226 {

227 mth_Quaternion r;

235

236 /* ***** */

237 mth_Quaternion mth_quatConjugate(const mth_Quaternion *q)

238 {

239 mth_Quaternion r;

247

248 /* ***** */

249 mth_Vector4 mth_quatMulVec(const mth_Quaternion *q, const mth_Vector4 *v)

250 {

251 mth_Vector4 vn, r;

27

28 static m_inline vec3 transformq(const quat q, const vec3 v) { // !!! ok, i guess

29 // [src] https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion (laurent couvidou)

30 // Extract the vector part of the quaternion

31 vec3 u = vec3(q.x, q.y, q.z);

32 // Extract the scalar part of the quaternion

33 float s = q.w;

34 // Do the math

73 }

74

75 // A quaternion can describe both a rotation and a uniform scaling in 3D space

76 static m_inline quat transform_quat (const mat44 m, const quat q) {

77 vec3 s = scale3(transform_vector(m, q.xyz), det44(m) < 0 ? -1.f : 1.f);

6

7 /** \file quat_setpoint.c

8 * \brief Quaternion setpoint generation

9 *

10 */

133 sticks_eulers.psi = 0;

134

135 // convert yaw rate * dt into quaternion

136 INT32_QUAT_OF_EULERS(sticks_quat, sticks_eulers);

137 QUAT_COPY(prev_sp_quat, stab_att_sp_quat)

46 /******************************************************************************

47 *

48 * Quaternion Functions

49 *

50 *****************************************************************************/

477 float cz,sz;

478

479 // CONVERT QUATERNION TO MATRIX - I DON'T REALLY NEED ALL OF IT

480

481 matrix[0][0] = SG_ONE - (SG_TWO * quat[SG_Y] * quat[SG_Y])

66 case COMPLEX_IMAG: return 1;

67 case COMPLEX_COMPLEX: return 2;

68 case COMPLEX_QUATERNION: return 4;

69 }

70 assert(0 && "Internal error: invalid type__complex_multiplier()");

166 case COMPLEX_IMAG: printf("i"); break;

167 case COMPLEX_COMPLEX: printf("c"); break;

168 case COMPLEX_QUATERNION: printf("q"); break;

169 }

170 }

1 #include <ultra64.h>

2 #include "quaternion.h"

3 #include "math_asinfacosf.h"

4

5 #define HALF2RAD(x) (x * (M_PI_F / 32768.0f))

6

7 void quaternion_set_rotation_around_xyz(vec3u angles, quatf q)

8 {

9 f32 cos_x = cosf(HALF2RAD(angles[0]) * 0.5f);

23 }

24

25 void quaternion_set_rotation_around_xyzf(vec3f angles, quatf q)

26 {

27 f32 cos_x = cosf(angles[0] * 0.5f);

41 }

42

43 void quaternion_set_rotation_around_x(f32 angle, quatf q)

44 {

45 q[0] = cosf(angle * 0.5f);

87 * t.R.v = S(-1) t.v S

88 * where

89 * t are the Pauli matrices (similar to Quaternions, easier to use)

90 * v is an arbitrary 3-vector

91 * and S is a 2x2 hermitian matrix:

231 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new", Quaternion_new);

232 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new_euler", Quaternion_new_euler);

233 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_add", Quaternion_add);

237 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_nlerp", Quaternion_nlerp);

238 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_normalize", Quaternion_normalize);

239 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_get_conjugate", Quaternion_get_conjugate);

243 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_set_x", Quaternion_set_x);

244 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_get_y", Quaternion_get_y);

245 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_set_y", Quaternion_set_y);

246 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_get_z", Quaternion_get_z);

247 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_set_z", Quaternion_set_z);

248 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_get_w", Quaternion_get_w);

249 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_set_w", Quaternion_set_w);

568 }

569

570 int liscr_args_gets_quaternion (

571 LIScrArgs* self,

572 const char* name,

850 if (self->output_mode != LISCR_ARGS_OUTPUT_TABLE)

851 {

852 quat = liscr_data_new_alloc (self->script, self->lua, sizeof (LIMatQuaternion), LISCR_SCRIPT_QUATERNION);

853 if (quat != NULL)

854 {

865 }

866 lua_pushnumber (self->lua, ++self->ret);

867 quat = liscr_data_new_alloc (self->script, self->lua, sizeof (LIMatQuaternion), LISCR_SCRIPT_QUATERNION);

868 if (quat != NULL)

869 {

376 {

377 LIMatTransform tmp;

378 LIMatQuaternion* q;

379 LIScrData* data;

380

381 if (liscr_args_geti_data (args, 0, LISCR_SCRIPT_QUATERNION, &data))

382 {

383 liphy_object_get_transform (args->self, &tmp);

390 {

391 LIMatTransform transform;

392 LIMatQuaternion quat;

393

394 if (liscr_args_geti_quaternion (args, 0, &quat))

146 {

147 LIMatTransform transform;

148 LIMatQuaternion value;

149 LIExtLight* light = args->self;

150

151 if (liscr_args_geti_quaternion (args, 0, &value))

152 {

153 liren_render_light_get_transform (light->module->render, light->id, &transform);

154 transform.rotation = value;

155 limat_quaternion_normalize (transform.rotation);

156 liren_render_light_set_transform (light->module->render, light->id, &transform);

157 }

22 Camera camera

23 {

24 attachedQuaternion,

25 position = Vector3D { 0, 0, -250 },

26 orientation = Euler { 120, 30, 0 },

22 Camera camera

23 {

24 attachedQuaternion,

25 position = Vector3D { 0, 0, -250 },

26 orientation = Euler { 120, 30, 0 },

77 /* Product of 2 quaternions */

78 LPD3DRMQUATERNION WINAPI D3DRMQuaternionMultiply(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b)

79 {

80 D3DRMQUATERNION temp;

127 /* Interpolation between two quaternions */

128 LPD3DRMQUATERNION WINAPI D3DRMQuaternionSlerp(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b, D3DVALUE alpha)

129 {

130 D3DVALUE dot, epsilon, temp, theta, u;

248 LPD3DVECTOR WINAPI D3DRMVectorRotate(LPD3DVECTOR r, LPD3DVECTOR v, LPD3DVECTOR axis, D3DVALUE theta)

249 {

250 D3DRMQUATERNION quaternion1, quaternion2, quaternion3;

251 D3DVECTOR norm;

252

260 D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion3);

261 D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion2);

262

263 *r = *D3DRMVectorNormalize(&quaternion1.v);

443 *scale = (1.0f - blend) * s0 + blend * s1;

444 value->position = limat_vector_lerp (t1.position, t0.position, blend);

445 value->rotation = limat_quaternion_nlerp (t1.rotation, t0.rotation, blend);

446 }

447

88

89 /**

90 * @brief Body-to-world frame quaternion.

91 * The elements are output in the following order: W, X, Y, Z.

92 * @param[out] data Quaternion, q30 fixed point.

105

106 /**

107 * @brief Quaternion-derived heading.

108 * @param[out] data Heading in degrees, q16 fixed point.

109 * @param[out] accuracy Accuracy of the measurement from 0 (least accurate)

222 int inv_get_sensor_type_rot_mat(long *data, int8_t *accuracy, inv_time_t *timestamp)

223 {

224 inv_quaternion_to_rotation(eMPL_out.quat, data);

225 accuracy[0] = eMPL_out.quat_accuracy;

226 timestamp[0] = eMPL_out.nine_axis_timestamp;

42 #include "transformField.H"

43 #include "Pair.H"

44 #include "quaternion.H"

45

46 using namespace Foam;

120 v *= pi/180.0;

121

122 quaternion R(v.x(), v.y(), v.z());

123

124 Info<< "Rotating points by quaternion " << R << endl;

142 scalar roll = v.z();

143

144 quaternion R = quaternion(vector(0, 0, 1), yaw);

145 R *= quaternion(vector(0, 1, 0), pitch);

146 R *= quaternion(vector(1, 0, 0), roll);

147

148 Info<< "Rotating points by quaternion " << R << endl;

20

21 /*!

22 * Set a quaternion to Identity

23 */

24 void

30

31 /*!

32 * Copy a quaternion.

33 */

34 void

42

43 /*!

44 * Compute a quaternion from axis and angle.

45 *

46 * \param c Computed quaternion

127 td->ext->rotAxis = NULL;

128 td->ext->rotAngle = NULL;

129 td->ext->quat = cursor->rotation_quaternion;

130

131 copy_qt_qt(td->ext->iquat, cursor->rotation_quaternion);

190

191 long quat[4] = {0};

192 IMUgetQuaternion(quat);

193 for (int i = 0; i < 4; ++i) {

194 reply_buf[7 + i * 2] = quat[i] >> 16;

40 uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };

41 uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI

42 uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI

43 // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI

44 // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI

58 // Quat Filter

59 for (int i=0;i<7;i++) {

60 InsUartSend1(quaternions[i]);

61 }

62

31

32 volatile float beta = betaDef; // 2 * proportional gain (Kp)

33 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // quaternion of sensor frame relative to auxiliary frame

34

35 //---------------------------------------------------------------------------------------------------

171 }

172

173 // Integrate rate of change of quaternion to yield quaternion

174 q[0] += qDot1 * (1.0f / sampleFreq);

175 q[1] += qDot2 * (1.0f / sampleFreq);

246 }

247

248 // Integrate rate of change of quaternion to yield quaternion

249 q[0] += qDot1 * (1.0f / sampleFreq);

250 q[1] += qDot2 * (1.0f / sampleFreq);

54 self->music_volume = 1.0f;

55 self->music_fading = 1.0f;

56 self->listener_rotation = limat_quaternion_identity ();

57

58 #ifndef LI_DISABLE_SOUND

253 /* Update listener position. */

254 velocity = self->listener_velocity;

255 direction = limat_quaternion_get_basis (self->listener_rotation, 2);

256 up = limat_quaternion_get_basis (self->listener_rotation, 1);

91 heaveA_*(sin(wr*time + phh) - sin(phh))

92 );

93 quaternion R(quaternion::XYZ, vector(rollA*sin(wr*time + phr), 0, 0));

94 septernion TR(septernion(-CofG_ - T)*R*septernion(CofG_));

95

24 *

25 * Rotorcraft attitude reference generation.

26 * (quaternion int version)

27 *

28 */

166 uint32_t type;

167 LIMatVector position;

168 LIMatQuaternion rotation;

169

170 /* Read header. */

368 LIMatVector* z)

369 {

370 *x = limat_quaternion_transform (self->transform.global.rotation, limat_vector_init (1.0f, 0.0f, 0.0f));

371 *y = limat_quaternion_transform (self->transform.global.rotation, limat_vector_init (0.0f, 1.0f, 0.0f));

372 *z = limat_quaternion_transform (self->transform.global.rotation, limat_vector_init (0.0f, 0.0f, 1.0f));

373 }

374

23

24 /** \file stabilization_attitude_ref_int.c

25 * \brief Booz attitude reference generation (quaternion int version)

26 *

27 */

285 }

286

287 // Dual Quaternions

288 void g_dual_quat_scale_add( GDualQuat* r, GDualQuat* dq, float t ){

289 float dot_real = r->q.x*dq->q.x + r->q.y*dq->q.y + r->q.z*dq->q.z + r->q.w*dq->q.w;

340 dq->q.z = a->q.w*b->q.z + a->q.z*b->q.w + a->q.x*b->q.y - a->q.y*b->q.x;

341

342 // Dual unit Quaternion.

343 dq->d.x = a->d.x*b->q.w + a->q.w*b->d.x + a->d.w*b->q.x + a->q.x*b->d.w -

344 a->d.z*b->q.y + a->q.y*b->d.z + a->d.y*b->q.z - a->q.z*b->d.y;

94 {"setLinearVelocity", IoODEBody_setLinearVelocity},

95

96 {"quaternion", IoODEBody_quaternion},

97 //{"setQuaternion", IoODEBody_setQuaternion},

387 }

388

389 IoObject *IoODEBody_quaternion(IoODEBody *self, IoObject *locals, IoMessage *m)

390 {

391 IoODEBody_assertValidBody(self, locals, m);

392 {

393 const dReal *q = dBodyGetQuaternion(BODYID);

394 IoSeq *v = IoSeq_newFloatArrayOfSize_(IOSTATE, 4);

395 UArray *u = IoSeq_rawUArray(v);

403

404 /*

405 IoObject *IoODEBody_setQuaternion(IoODEBody *self, IoObject *locals, IoMessage *m)

406 {

407 const double w = IoMessage_locals_doubleArgAt_(m, locals, 0);

1 /*

2 * qmult.c - quaternion multiplication dsp object

3 * Copyright (c) 2000-2003 by Tom Schouten

4 *

71 {

72

73 /* read input quaternions */

74 wl = *in0l++;

75 xl = *in1l++;

83

84

85 /* multiply quaternions */

86 w = wl * wr;

87 x = wl * xr;

106

107

108 /* write output quaternion */

109 *out0++ = w;

110 *out1++ = x;

37 void UpdateObjects(Time diffTime)

38 {

39 Quaternion orientation = cube.transform.orientation;

40 orientation.RotateYawPitch((Degrees)diffTime * 0.5, (Degrees)diffTime * 0.5);

41 orientation.RotateRoll((Degrees)diffTime * 0.5);

160

161 lieng_object_get_transform (args->self, &tmp);

162 liscr_args_seti_quaternion (args, &tmp.rotation);

163 }

164

168 LIScrData* quat;

169

170 if (liscr_args_geti_data (args, 0, LISCR_SCRIPT_QUATERNION, &quat))

171 {

172 lieng_object_get_transform (args->self, &transform);

173 transform.rotation = *((LIMatQuaternion*) quat->data);

174 lieng_object_set_transform (args->self, &transform);

175 }

143 0);

144

145 ADD_TEST (test_euler_quaternion, 0, 0);

146 ADD_TEST (test_color_hsl, 0, 0);

147

196 case COMPLEX_ARGS: return "complex"; break;

197 case MIXED_ARGS: return "real/complex"; break;

198 case QUATERNION_ARGS: return "quaternion"; break;

199 case QMIXED_ARGS: return "real/quaternion"; break;

106

107 /*

108 * Set up quaternion describing the rotation of alpha. Need to

109 * normalise vec3 component of quaternion so rotation is correct.

74 const vector displacement = velocity_*t;

75

76 quaternion R(0, 0, 0);

77 septernion TR(septernion(displacement)*R);

78

68 //Quaternion Implementation

69 Quaternion operator*(Quaternion a,Quaternion b) {

70 Quaternion c;

75 return c;

76 }

77 Quaternion operator-(Quaternion q) {

78 return Quaternion(q.r*-1,q.x,q.y,q.z);

89 }

90

91 Quaternion operator+(Quaternion a,Quaternion b) {

92 return Quaternion(a.r+b.r, a.x+b.x, a.y+b.y, a.z+b.z);

95 return (a.r*b.r + a.x*b.x + a.y*b.y + a.z*b.z);

96 }

97 Quaternion slerp(Quaternion a,Quaternion b,float interp){

98 if((a^b) <0.0) {

99 a.r=-a.r;

120 }

121

122 // ****** find roll, pitch, yaw from quaternion ********

123 void Quaternion2RPY(float q[4], float rpy[3])

140 }

141

142 // ****** find quaternion from roll, pitch, yaw ********

143 void RPY2Quaternion(float rpy[3], float q[4])

169 }

170

171 //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **

172 void Quaternion2R(float q[4], float Rbe[3][3])

35 * @file attitude_estimator_q_params.c

36 *

37 * Parameters for attitude estimator (quaternion based)

38 *

39 * @author Anton Babushkin <anton.babushkin@me.com>

2 *

3 * Convert between different orientation formats. The formats are:

4 * quaternion, yaw-pitch-roll, xyz angles, pre-multiplication

5 * (object) matrices, and post-multiplication (view) matrices.

6 * The conversion is done by converting each input form to a matrix,

82 eulerAngles *= pi/180.0;

83

84 quaternion R(eulerAngles.x(), eulerAngles.y(), eulerAngles.z());

85 septernion TR(septernion(CofG_)*R*septernion(-CofG_));

86

94 }

95

96 void Multiply(Quaternion q1, Quaternion q2)

97 {

98 w = q1.w * q2.w - q2.x * q1.x - q1.y * q2.y - q1.z * q2.z;

102 }

103

104 void Divide(Quaternion q1, Quaternion q2)

105 {

106 w = q2.w * q1.w + q2.x * q1.x + q2.y * q1.y + q2.z * q1.z;

180 #define DELTA 0

181

182 void Slerp(Quaternion from, Quaternion to, float t)

183 {

184 double to1[4];

91

92 // ----------------------------------------------------------------------------

93 // Packet "Quaternions" parameters table

94 // ----------------------------------------------------------------------------

95 const PacketParameter packetQuaternionsParameterList[4] =

134 /* DistanceSensors */ { 0xF0, packetDistanceSensorsParameterList, 8, 8 },

135 /* RobotFeedback */ { 0xD4, packetRobotFeedbackParameterList, 4, 8 },

136 /* Quaternions */ { 0xF2, packetQuaternionsParameterList, 4, 8 },

137 /* Emergency */ { 0xCF, packetEmergencyParameterList, 4, 4 },

138 /* Gyro */ { 0xF1, packetGyroParameterList, 3, 6 }

58 const vector rotation = rotVelocity_*(t - startMotionTime_);

59

60 const quaternion R(rotation.x(), rotation.y(), rotation.z());

61 const septernion TR

62 (

100 }

101

102 const quaternion R(rotation_.x(), rotation_.y(), rotation_.z());

103 const septernion TR

104 (

214 const septernion TR = calcTransformation();

215

216 const quaternion ROld(rotationOld_.x(), rotationOld_.y(), rotationOld_.z());

217 const septernion TROld

218 (

107 return;

108

109 tmp = limat_quaternion_add (*((LIMatQuaternion*) args->self), *((LIMatQuaternion*) liscr_data_get_data (b)));

110 liscr_args_seti_quaternion (args, &tmp);

290 LIScrScript* self)

291 {

292 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new", Quaternion_new);

293 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new_from_axis", Quaternion_new_from_axis);

294 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new_from_dir", Quaternion_new_from_dir);

295 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new_from_euler", Quaternion_new_from_euler);

296 liscr_script_insert_cfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_new_from_vectors", Quaternion_new_from_vectors);

297 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_add", Quaternion_add);

298 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_concat", Quaternion_concat);

299 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_multiply", Quaternion_multiply);

300 liscr_script_insert_mfunc (self, LISCR_SCRIPT_QUATERNION, "quaternion_sub", Quaternion_sub);

1 /*****************************************************************************

2 *

3 matrix.c- code for matrix utilities for quaternion library; routines

4 here are only those that have nothing to do with quaternions.

3

4 // ËÄÔªÊýת»¯³ÉÒ»¸ö½«ÏòÁ¿ÓÉNED×ø±êϵÐýתµ½»úÌå×ø±êϵµÄ¾ØÕó

5 void Quaternion2Tnb(const float q[4], float Tnb[3][3])

6 {

7 // Calculate the ned to body cosine matrix

30

31 //ËÄÔªÊýת»¯³ÉÒ»¸ö½«ÏòÁ¿ÓÉ»úÌå×ø±êϵÐýתµ½NED×ø±êϵµÄ¾ØÕó

32 void Quaternion2Tbn(const float q[4], float Tbn[3][3])

33 {

34 // Calculate the body to ned cosine matrix

56

57 // ÓÉËÄÔªÊýת³ÉÅ·À­½Ç

58 void Quaternion2Euler(const float q[4], float euler[3])

59 {

60 float R13, R11, R12, R23, R33;

93 }

94

95 void RotationQuaternion(const Quaternion quat)

96 {

97 double xx = quat.x*quat.x, yy = quat.y*quat.y, zz = quat.z*quat.z;

128 }

129

130 void Rotate(const Quaternion quat)

131 {

132 Matrix rmat;

133 Matrix mat1;

134 rmat.RotationQuaternion(quat);

135 mat1.Multiply3x4(this, rmat);

136 this = mat1;

27 /*****************************************************************************

28 *

29 q_xyz_quat_invert - invert a vector/quaternion transformation pair

30

31 input:

23

24 /** \file stabilization_attitude_ref_float.c

25 * \brief Booz attitude reference generation (quaternion float version)

26 *

27 */

6 * The output table specifies the orientation in one of three ways:

7 *

8 * default - quaternions in the order x, y, z, w.

9 *

10 * -z option - Eulers angles, in the order xyz. The model axes are

48 #define YPR 0

49 #define XYZ 1

50 #define QUATERNION 2

51

52 #define DEGREES 0

118 printf("%.10g\t%.10g\t%.10g\n",angle[X],angle[Y],angle[Z]);

119 }

120 else if (mode==QUATERNION){

121 anim_mat2quat(quat,viewrot);

122 printf("%.10g\t%.10g\t%.10g\t%.10g\n",quat[X],quat[Y],quat[Z],quat[W]);

93 LIExtModule* module;

94 LIMatVector position;

95 LIMatQuaternion rotation;

96 LIMatTransform transform;

97

98 if (liscr_args_geti_vector (args, 0, &position) &&

99 liscr_args_geti_quaternion (args, 1, &rotation))

100 {

101 module = liscr_script_get_userdata (args->script, LIEXT_SCRIPT_RENDER);

114 (

115 velocity_*2*rampTime_/pi*(1 - cos(pi/(2*rampTime_)*t)),

116 quaternion::I

117 );

118 }

129 + (t - rampTime_)

130 ),

131 quaternion::I

132 );

133 }

148 (

149 rampFactor()*velocity_,

150 quaternion::I/time_.deltaT().value()

151 );

152

78 scalar angle = omega_->integrate(0, t);

79

80 quaternion R(axis_, angle);

81 septernion TR(septernion(origin_)*R*septernion(-origin_));

82

47 bu_vls_trunc(&gedp->ged_result_str, 0);

48

49 /* return Viewrot as a quaternion */

50 if (argc == 1) {

51 quat_mat2quat(quat, gedp->ged_gvp->gv_rotation);

59 }

60

61 /* Set the view orientation given a quaternion */

62 if (sscanf(argv[2], "%lf", quat) != 1 ||

63 sscanf(argv[3], "%lf", quat+1) != 1 ||

37

38 // ----------------------------------------------------------------------------

39 // Packet "Quaternions" parameters table

40 // ----------------------------------------------------------------------------

41 const PacketParameter packetQuaternionsParameterList[4] =

99 /* RobotFeedback */ { 0xD4, packetRobotFeedbackParameterList, 4, 8 },

100 /* DistanceSensors */ { 0xF0, packetDistanceSensorsParameterList, 8, 8 },

101 /* Quaternions */ { 0xF2, packetQuaternionsParameterList, 4, 8 },

102 /* MotorSpeeds */ { 0xD0, packetMotorSpeedsParameterList, 2, 4 },

103 /* RobotControl */ { 0xD3, packetRobotControlParameterList, 2, 4 },

40

41 // test from the app note

42 // the quaternion should be: 0.2474, -0.1697, -0.1713, 0.9384

43 /*

44 bytes = 15;

33 }

34

35 void Multiply(const Quaternion q1, const Quaternion q2)

36 {

37 w = q1.w * q2.w - q2.x * q1.x - q1.y * q2.y - q1.z * q2.z;

81 }

82

83 static void rotateQuats(Quaternion qPitch, Quaternion qYaw, Quaternion qRoll, EulerRotationOrder rotationOrder)

84 {

85 Quaternion q, a;

332 }

333

334 void FromQuaternion(const Quaternion q, EulerRotationOrder order)

335 {

336 Matrix m;

31 int16_t sum_r, sum_p, sum_y;

32

33 mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw);

34 roll_deg = roll * RAD_TO_DEG;

35 pitch_deg = pitch * RAD_TO_DEG;

109 float sinAng2; // sin(half-angle)

110

111 // Convert a GL-style rotation to a quaternion. The GL rotation looks like this:

112 // {angle, x, y, z}, the corresponding quaternion looks like this:

125

126 // Figure out A' = A . dA

127 // In quaternions: let q0 <- A, and q1 <- dA.

128 // Figure out q2 = q1 + q0 (note the order reversal!).

129 // A' <- q3.

137 q2[2] = q1[0]*q0[1] - q1[1]*q0[0] + q1[3]*q0[2] + q1[2]*q0[3];

138 q2[3] = q1[3]*q0[3] - q1[0]*q0[0] - q1[1]*q0[1] - q1[2]*q0[2];

139 // Here's an excersize for the reader: it's a good idea to re-normalize your quaternions

140 // every so often. Experiment with different frequencies.

141

142 // An identity rotation is expressed as rotation by 0 about any axis.

143 // The "angle" term in a quaternion is really the cosine of the half-angle.

144 // So, if the cosine of the half-angle is one (or, 1.0 within our tolerance),

145 // then you have an identity rotation.

25 * The output table specifies the orientation in one of three ways:

26 *

27 * default - quaternions in the order x, y, z, w.

28 *

29 * -z option - Eulers angles, in the order xyz. The model axes are

52 #define YPR 0

53 #define XYZ 1

54 #define QUATERNION 2

55

56 #define DEGREES 0

123 printf("%.10g\t%.10g\t%.10g\n", angle[X], angle[Y], angle[Z]);

124 }

125 else if (mode==QUATERNION) {

126 anim_mat2quat(quat, viewrot);

127 printf("%.10g\t%.10g\t%.10g\t%.10g\n", quat[X], quat[Y], quat[Z], quat[W]);

68 //Quaternion Implementation

69 Quaternion operator*(Quaternion a,Quaternion b) {

70 Quaternion c;

75 return c;

76 }

77 Quaternion operator-(Quaternion q) {

78 return Quaternion(q.r*-1,q.x,q.y,q.z);

89 }

90

91 Quaternion operator+(Quaternion a,Quaternion b) {

92 return Quaternion(a.r+b.r, a.x+b.x, a.y+b.y, a.z+b.z);

95 return (a.r*b.r + a.x*b.x + a.y*b.y + a.z*b.z);

96 }

97 Quaternion slerp(Quaternion a,Quaternion b,float interp){

98 if((a^b) <0.0) {

99 a.r=-a.r;

106 fprintf(fd, "%.16f [%.16f %.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f %.16f %.16f %.16f %.16f %.16f]\n",

107 samples[i].time,

108 output[i].quat_est.qi, output[i].quat_est.qx, output[i].quat_est.qy, output[i].quat_est.qz, // quaternion

109 output[i].rate_est.p, output[i].rate_est.q, output[i].rate_est.r, // omega

110 output[i].bias_est.p, output[i].bias_est.q, output[i].bias_est.r, // bias

169 }

170 // --- compute rotation matrix ---

171 RotationQuaternion(max_evec, R);

172

173 // --- compute translation vector ---

126 /* . */

127

128 /* 2) Make sure that a `unit' quaternion is non-zero before */

129 /* converting it to a rotation matrix. */

130

34 (

35 vector(0, 0, 0),

36 quaternion(0, vector(0, 0, 0))

37 );

38 const Foam::septernion Foam::septernion::I

39 (

40 vector(0, 0, 0),

41 quaternion(1, vector(0, 0, 0))

42 );

43

12 const quaternion_t quat_unitary = { 1.0f, {{{ 0.0f, 0.0f, 0.0f}}} };

13

14 void mul_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2)

15 {

16 vector31_t temp_v;

31 }

32

33 void add_quat( quaternion_t* out, quaternion_t* q1, quaternion_t* q2 )

34 {

35 // scalar result

39 }

40

41 void mulconst_quat( quaternion_t* out, quaternion_t* q, float32_t k )

42 {

43 out->a = (q->a) * k;

18

19 /** @file lib3ds_quat.c

20 Quaternion mathematics implementation */

21

22 #include "lib3ds_impl.h"

24

25 /*!

26 * Set a quaternion to Identity

27 */

28 void

34

35 /*!

36 * Copy a quaternion.

37 */

38 void

82 eulerAngles *= pi/180.0;

83

84 quaternion R(eulerAngles.x(), eulerAngles.y(), eulerAngles.z());

85 septernion TR(septernion(origin_)*R*septernion(-origin_));

86

30 // * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //

31

32 const char* const Foam::quaternion::typeName = "quaternion";

33 const Foam::quaternion Foam::quaternion::zero(0, vector(0, 0, 0));

34 const Foam::quaternion Foam::quaternion::I(1, vector(0, 0, 0));

35

36 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //

37

38 Foam::quaternion::quaternion(Istream& is)

39 {

40 is >> *this;

70

71

72 Foam::quaternion Foam::exp(const quaternion& q)

73 {

74 const scalar magV = mag(q.v());

51

52 double qr[4];

53 aa_tf_rotmat2quat(R, qr); // convert to quaternion

54 aveq("quat", 4, q, qr, .001);

55

59 aveq("vec", 3, vq, vr, .001);

60

61 double qvr[4], qvq[4]; // convert back to quaternions

62 aa_tf_rotvec2quat( vq, qvq );

63 aa_tf_rotvec2quat( vr, qvr );