From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/libccd/src/custom/ccdcustom/quat.h | 69 +++++++++++++ libs/ode-0.16.1/libccd/src/custom/ccdcustom/vec3.h | 114 +++++++++++++++++++++ 2 files changed, 183 insertions(+) create mode 100644 libs/ode-0.16.1/libccd/src/custom/ccdcustom/quat.h create mode 100644 libs/ode-0.16.1/libccd/src/custom/ccdcustom/vec3.h (limited to 'libs/ode-0.16.1/libccd/src/custom') diff --git a/libs/ode-0.16.1/libccd/src/custom/ccdcustom/quat.h b/libs/ode-0.16.1/libccd/src/custom/ccdcustom/quat.h new file mode 100644 index 0000000..157dd85 --- /dev/null +++ b/libs/ode-0.16.1/libccd/src/custom/ccdcustom/quat.h @@ -0,0 +1,69 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * 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_CUSTOM_QUAT_H__ +#define __CCD_CUSTOM_QUAT_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +/** + * Rotate vector s by quaternion q and put result into d. + */ +_ccd_inline void ccdQuatRotVec2(ccd_vec3_t *d, const ccd_vec3_t *s, const ccd_quat_t *q); + + +_ccd_inline void ccdQuatRotVec2(ccd_vec3_t *d, const ccd_vec3_t *s, const ccd_quat_t *q) +{ +#ifndef dLIBCCD_USE_SYSTEM + // original version: 31 mul + 21 add + // optimized version: 18 mul + 12 add + // formula: d = s + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * s) + 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(s); + vy = ccdVec3Y(s); + vz = ccdVec3Z(s); + + 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(d, vx + 2 * cross2_x, vy + 2 * cross2_y, vz + 2 * cross2_z); +#else + ccdVec3Copy(d, s); + ccdQuatRotVec(d, q); +#endif +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_QUAT_H__ */ diff --git a/libs/ode-0.16.1/libccd/src/custom/ccdcustom/vec3.h b/libs/ode-0.16.1/libccd/src/custom/ccdcustom/vec3.h new file mode 100644 index 0000000..2ac2f22 --- /dev/null +++ b/libs/ode-0.16.1/libccd/src/custom/ccdcustom/vec3.h @@ -0,0 +1,114 @@ +/*** + * libccd + * --------------------------------- + * Copyright (c)2010,2011 Daniel Fiser + * + * + * This file is part of libccd. + * + * Distributed under the OSI-approved BSD License (the "License"); + * see accompanying file BDS-LICENSE for details or see + * . + * + * 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_CUSTOM_VEC3_H__ +#define __CCD_CUSTOM_VEC3_H__ + +#include + + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + + +#ifdef CCD_SINGLE +# define CCD_ATAN2(x, y) (atan2f((x), (y))) /*!< atan2 of two floats */ +#endif /* CCD_SINGLE */ + +#ifdef CCD_DOUBLE +# define CCD_ATAN2(x, y) (atan2((x), (y))) /*!< atan2 of two floats */ +#endif /* CCD_DOUBLE */ + +/** + * d = v + w + */ +_ccd_inline void ccdVec3Add2(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *w); + +/** + * d = s * k; + */ +_ccd_inline void ccdVec3CopyScaled(ccd_vec3_t *d, const ccd_vec3_t *s, ccd_real_t k); + +/** + * d = v + s * k; + */ +_ccd_inline void ccdVec3AddScaled(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *s, ccd_real_t k); + + +/** + * Normalizes given vector to unit length. + */ +_ccd_inline int ccdVec3SafeNormalize(ccd_vec3_t *d); + + +_ccd_inline void ccdVec3Add2(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *w) +{ +#ifndef dLIBCCD_USE_SYSTEM + d->v[0] = v->v[0] + w->v[0]; + d->v[1] = v->v[1] + w->v[1]; + d->v[2] = v->v[2] + w->v[2]; +#else + ccdVec3Copy(d, v); + ccdVec3Add(d, w); +#endif +} + +_ccd_inline void ccdVec3CopyScaled(ccd_vec3_t *d, const ccd_vec3_t *s, ccd_real_t k) +{ +#ifndef dLIBCCD_USE_SYSTEM + d->v[0] = s->v[0] * k; + d->v[1] = s->v[1] * k; + d->v[2] = s->v[2] * k; +#else + ccdVec3Copy(d, s); + ccdVec3Scale(d, k); +#endif +} + +_ccd_inline void ccdVec3AddScaled(ccd_vec3_t *d, const ccd_vec3_t *v, const ccd_vec3_t *s, ccd_real_t k) +{ +#ifndef dLIBCCD_USE_SYSTEM + d->v[0] = v->v[0] + s->v[0] * k; + d->v[1] = v->v[1] + s->v[1] * k; + d->v[2] = v->v[2] + s->v[2] * k; +#else + ccdVec3Copy(d, s); + ccdVec3Scale(d, k); + ccdVec3Add(d, v); +#endif +} + +_ccd_inline int ccdVec3SafeNormalize(ccd_vec3_t *d) +{ + int result = -1; + + ccd_real_t len = CCD_SQRT(ccdVec3Len2(d)); + if (len >= CCD_EPS) { + ccdVec3Scale(d, CCD_ONE / len); + result = 0; + } + + return result; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* __CCD_CUSTOM_VEC3_H__ */ -- cgit v1.2.1