add ode

1 files changed, 85 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/libccd/src/testsuites/support.c b/libs/ode-0.16.1/libccd/src/testsuites/support.c
new file mode 100644
index 0000000..5f2b4c7
--- /dev/null
+++ b/libs/ode-0.16.1/libccd/src/testsuites/support.c
@@ -0,0 +1,85 @@
+/***
+ * 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.
+ */
+
+#include <stdio.h>
+#include <ccd/ccd.h>
+#include <ccd/vec3.h>
+#include "support.h"
+
+void ccdSupport(const void *_obj, const ccd_vec3_t *_dir,
+                ccd_vec3_t *v)
+{
+    // Support function is made according to Gino van den Bergen's paper
+    //  A Fast and Robust CCD Implementation for Collision Detection of
+    //  Convex Objects
+
+    ccd_obj_t *obj = (ccd_obj_t *)_obj;
+    ccd_vec3_t dir;
+    ccd_quat_t qinv;
+
+    ccdVec3Copy(&dir, _dir);
+    ccdQuatInvert2(&qinv, &obj->quat);
+
+    ccdQuatRotVec(&dir, &qinv);
+
+    if (obj->type == CCD_OBJ_BOX){
+        ccd_box_t *box = (ccd_box_t *)obj;
+        ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * box->x * CCD_REAL(0.5),
+                      ccdSign(ccdVec3Y(&dir)) * box->y * CCD_REAL(0.5),
+                      ccdSign(ccdVec3Z(&dir)) * box->z * CCD_REAL(0.5));
+    }else if (obj->type == CCD_OBJ_SPHERE){
+        ccd_sphere_t *sphere = (ccd_sphere_t *)obj;
+        ccd_real_t len;
+
+        len = ccdVec3Len2(&dir);
+        if (len - CCD_EPS > CCD_ZERO){
+            ccdVec3Copy(v, &dir);
+            ccdVec3Scale(v, sphere->radius / CCD_SQRT(len));
+        }else{
+            ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO);
+        }
+    }else if (obj->type == CCD_OBJ_CYL){
+        ccd_cyl_t *cyl = (ccd_cyl_t *)obj;
+        ccd_real_t zdist, rad;
+
+        zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
+        zdist = CCD_SQRT(zdist);
+        if (ccdIsZero(zdist)){
+            ccdVec3Set(v, CCD_ZERO, CCD_ZERO,
+                          ccdSign(ccdVec3Z(&dir)) * cyl->height * CCD_REAL(0.5));
+        }else{
+            rad = cyl->radius / zdist;
+
+            ccdVec3Set(v, rad * ccdVec3X(&dir),
+                          rad * ccdVec3Y(&dir),
+                          ccdSign(ccdVec3Z(&dir)) * cyl->height * CCD_REAL(0.5));
+        }
+    }
+
+    // transform support vertex
+    ccdQuatRotVec(v, &obj->quat);
+    ccdVec3Add(v, &obj->pos);
+}
+
+void ccdObjCenter(const void *_obj, ccd_vec3_t *center)
+{
+    ccd_obj_t *obj = (ccd_obj_t *)_obj;
+
+    ccdVec3Set(center, CCD_ZERO, CCD_ZERO, CCD_ZERO);
+    // rotation is not needed
+    ccdVec3Add(center, &obj->pos);
+}