diff options
Diffstat (limited to 'libs/ode-0.16.1/libccd/src/ccd/quat.h')
-rw-r--r-- | libs/ode-0.16.1/libccd/src/ccd/quat.h | 231 |
1 files changed, 231 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/libccd/src/ccd/quat.h b/libs/ode-0.16.1/libccd/src/ccd/quat.h new file mode 100644 index 0000000..3929671 --- /dev/null +++ b/libs/ode-0.16.1/libccd/src/ccd/quat.h @@ -0,0 +1,231 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz> + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * <http://www.opensource.org/licenses/bsd-license.php>. + * + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +#ifndef __CCD_QUAT_H__ +#define __CCD_QUAT_H__ + +#include <ccd/compiler.h> +#include <ccd/vec3.h> + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +struct _ccd_quat_t { + ccd_real_t q[4]; //!< x, y, z, w +}; +typedef struct _ccd_quat_t ccd_quat_t; + +#define CCD_QUAT(name, x, y, z, w) \ + ccd_quat_t name = { {x, y, z, w} } + +_ccd_inline ccd_real_t ccdQuatLen2(const ccd_quat_t *q); +_ccd_inline ccd_real_t ccdQuatLen(const ccd_quat_t *q); + +_ccd_inline void ccdQuatSet(ccd_quat_t *q, ccd_real_t x, ccd_real_t y, ccd_real_t z, ccd_real_t w); +_ccd_inline void ccdQuatCopy(ccd_quat_t *dest, const ccd_quat_t *src); + +_ccd_inline int ccdQuatNormalize(ccd_quat_t *q); + +_ccd_inline void ccdQuatSetAngleAxis(ccd_quat_t *q, + ccd_real_t angle, const ccd_vec3_t *axis); + +_ccd_inline void ccdQuatScale(ccd_quat_t *q, ccd_real_t k); + +/** + * q = q * q2 + */ +_ccd_inline void ccdQuatMul(ccd_quat_t *q, const ccd_quat_t *q2); + +/** + * q = a * b + */ +_ccd_inline void ccdQuatMul2(ccd_quat_t *q, + const ccd_quat_t *a, const ccd_quat_t *b); + +/** + * Inverts quaternion. + * Returns 0 on success. + */ +_ccd_inline int ccdQuatInvert(ccd_quat_t *q); +_ccd_inline int ccdQuatInvert2(ccd_quat_t *dest, const ccd_quat_t *src); + + +/** + * Rotate vector v by quaternion q. + */ +_ccd_inline void ccdQuatRotVec(ccd_vec3_t *v, const ccd_quat_t *q); + + +/**** INLINES ****/ +_ccd_inline ccd_real_t ccdQuatLen2(const ccd_quat_t *q) +{ + ccd_real_t len; + + len = q->q[0] * q->q[0]; + len += q->q[1] * q->q[1]; + len += q->q[2] * q->q[2]; + len += q->q[3] * q->q[3]; + + return len; +} + +_ccd_inline ccd_real_t ccdQuatLen(const ccd_quat_t *q) +{ + return CCD_SQRT(ccdQuatLen2(q)); +} + +_ccd_inline void ccdQuatSet(ccd_quat_t *q, ccd_real_t x, ccd_real_t y, ccd_real_t z, ccd_real_t w) +{ + q->q[0] = x; + q->q[1] = y; + q->q[2] = z; + q->q[3] = w; +} + +_ccd_inline void ccdQuatCopy(ccd_quat_t *dest, const ccd_quat_t *src) +{ + *dest = *src; +} + + +_ccd_inline int ccdQuatNormalize(ccd_quat_t *q) +{ + ccd_real_t len = ccdQuatLen(q); + if (len < CCD_EPS) + return 0; + + ccdQuatScale(q, CCD_ONE / len); + return 1; +} + +_ccd_inline void ccdQuatSetAngleAxis(ccd_quat_t *q, + ccd_real_t angle, const ccd_vec3_t *axis) +{ + ccd_real_t a, x, y, z, n, s; + + a = angle/2; + x = ccdVec3X(axis); + y = ccdVec3Y(axis); + z = ccdVec3Z(axis); + n = CCD_SQRT(x*x + y*y + z*z); + + // axis==0? (treat this the same as angle==0 with an arbitrary axis) + if (n < CCD_EPS){ + q->q[0] = q->q[1] = q->q[2] = CCD_ZERO; + q->q[3] = CCD_ONE; + }else{ + s = sin(a)/n; + + q->q[3] = cos(a); + q->q[0] = x*s; + q->q[1] = y*s; + q->q[2] = z*s; + + ccdQuatNormalize(q); + } +} + + +_ccd_inline void ccdQuatScale(ccd_quat_t *q, ccd_real_t k) +{ + size_t i; + for (i = 0; i < 4; i++) + q->q[i] *= k; +} + +_ccd_inline void ccdQuatMul(ccd_quat_t *q, const ccd_quat_t *q2) +{ + ccd_quat_t a; + ccdQuatCopy(&a, q); + ccdQuatMul2(q, &a, q2); +} + +_ccd_inline void ccdQuatMul2(ccd_quat_t *q, + const ccd_quat_t *a, const ccd_quat_t *b) +{ + q->q[0] = a->q[3] * b->q[0] + + a->q[0] * b->q[3] + + a->q[1] * b->q[2] + - a->q[2] * b->q[1]; + q->q[1] = a->q[3] * b->q[1] + + a->q[1] * b->q[3] + - a->q[0] * b->q[2] + + a->q[2] * b->q[0]; + q->q[2] = a->q[3] * b->q[2] + + a->q[2] * b->q[3] + + a->q[0] * b->q[1] + - a->q[1] * b->q[0]; + q->q[3] = a->q[3] * b->q[3] + - a->q[0] * b->q[0] + - a->q[1] * b->q[1] + - a->q[2] * b->q[2]; +} + +_ccd_inline int ccdQuatInvert(ccd_quat_t *q) +{ + ccd_real_t len2 = ccdQuatLen2(q); + if (len2 < CCD_EPS) + return -1; + + len2 = CCD_ONE / len2; + + q->q[0] = -q->q[0] * len2; + q->q[1] = -q->q[1] * len2; + q->q[2] = -q->q[2] * len2; + q->q[3] = q->q[3] * len2; + + return 0; +} +_ccd_inline int ccdQuatInvert2(ccd_quat_t *dest, const ccd_quat_t *src) +{ + ccdQuatCopy(dest, src); + return ccdQuatInvert(dest); +} + +_ccd_inline void ccdQuatRotVec(ccd_vec3_t *v, const ccd_quat_t *q)
+{
+ // original version: 31 mul + 21 add
+ // optimized version: 18 mul + 12 add
+ // formula: v = v + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * v)
+ ccd_real_t cross1_x, cross1_y, cross1_z, cross2_x, cross2_y, cross2_z;
+ ccd_real_t x, y, z, w;
+ ccd_real_t vx, vy, vz;
+
+ vx = ccdVec3X(v);
+ vy = ccdVec3Y(v);
+ vz = ccdVec3Z(v);
+
+ w = q->q[3];
+ x = q->q[0];
+ y = q->q[1];
+ z = q->q[2];
+
+ cross1_x = y * vz - z * vy + w * vx;
+ cross1_y = z * vx - x * vz + w * vy;
+ cross1_z = x * vy - y * vx + w * vz;
+ cross2_x = y * cross1_z - z * cross1_y;
+ cross2_y = z * cross1_x - x * cross1_z;
+ cross2_z = x * cross1_y - y * cross1_x;
+ ccdVec3Set(v, vx + 2 * cross2_x, vy + 2 * cross2_y, vz + 2 * cross2_z);
+}
+ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_QUAT_H__ */ |