--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/sphere.cpp Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,218 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+/*
+
+standard ODE geometry primitives: public API and pairwise collision functions.
+
+the rule is that only the low level primitive collision functions should set
+dContactGeom::g1 and dContactGeom::g2.
+
+*/
+
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_util.h"
+
+
+
+//****************************************************************************
+// sphere public API
+
+dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1)
+{
+ type = dSphereClass;
+ radius = _radius;
+}
+
+
+void dxSphere::computeAABB()
+{
+ aabb[0] = final_posr->pos[0] - radius;
+ aabb[1] = final_posr->pos[0] + radius;
+ aabb[2] = final_posr->pos[1] - radius;
+ aabb[3] = final_posr->pos[1] + radius;
+ aabb[4] = final_posr->pos[2] - radius;
+ aabb[5] = final_posr->pos[2] + radius;
+}
+
+
+EXPORT_C dGeomID dCreateSphere (dSpaceID space, dReal radius)
+{
+ return new dxSphere (space,radius);
+}
+
+
+EXPORT_C void dGeomSphereSetRadius (dGeomID g, dReal radius)
+{
+ dxSphere *s = (dxSphere*) g;
+ s->radius = radius;
+ dGeomMoved (g);
+}
+
+
+EXPORT_C dReal dGeomSphereGetRadius (dGeomID g)
+{
+ dxSphere *s = (dxSphere*) g;
+ return s->radius;
+}
+
+
+EXPORT_C dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
+{
+ g->recomputePosr();
+
+ dxSphere *s = (dxSphere*) g;
+ dReal * pos = s->final_posr->pos;
+ return s->radius - dSqrt (dMUL((x-pos[0]),(x-pos[0])) +
+ dMUL((y-pos[1]),(y-pos[1])) +
+ dMUL((z-pos[2]),(z-pos[2])));
+}
+
+//****************************************************************************
+// pairwise collision functions for standard geom types
+
+int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int /*flags*/,
+ dContactGeom *contact, int /*skip*/)
+{
+ dxSphere *sphere1 = (dxSphere*) o1;
+ dxSphere *sphere2 = (dxSphere*) o2;
+
+ contact->g1 = o1;
+ contact->g2 = o2;
+
+ return dCollideSpheres (o1->final_posr->pos,sphere1->radius,
+ o2->final_posr->pos,sphere2->radius,contact);
+}
+
+
+int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int /*flags*/,
+ dContactGeom *contact, int /*skip*/)
+{
+ // this is easy. get the sphere center `p' relative to the box, and then clip
+ // that to the boundary of the box (call that point `q'). if q is on the
+ // boundary of the box and |p-q| is <= sphere radius, they touch.
+ // if q is inside the box, the sphere is inside the box, so set a contact
+ // normal to push the sphere to the closest box face.
+
+ dVector3 l,t,p,q,r;
+ dReal depth;
+ int onborder = 0;
+
+ dxSphere *sphere = (dxSphere*) o1;
+ dxBox *box = (dxBox*) o2;
+
+ contact->g1 = o1;
+ contact->g2 = o2;
+
+ p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
+ p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
+ p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];
+
+ l[0] = dMUL(box->side[0],REAL(0.5));
+ t[0] = dDOT14(p,o2->final_posr->R);
+ if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
+ if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; }
+
+ l[1] = dMUL(box->side[1],REAL(0.5));
+ t[1] = dDOT14(p,o2->final_posr->R+1);
+ if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
+ if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; }
+
+ t[2] = dDOT14(p,o2->final_posr->R+2);
+ l[2] = dMUL(box->side[2],REAL(0.5));
+ if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
+ if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; }
+
+ if (!onborder) {
+ // sphere center inside box. find closest face to `t'
+ dReal min_distance = l[0] - dFabs(t[0]);
+ int mini = 0;
+ for (int i=1; i<3; i++) {
+ dReal face_distance = l[i] - dFabs(t[i]);
+ if (face_distance < min_distance) {
+ min_distance = face_distance;
+ mini = i;
+ }
+ }
+ // contact position = sphere center
+ contact->pos[0] = o1->final_posr->pos[0];
+ contact->pos[1] = o1->final_posr->pos[1];
+ contact->pos[2] = o1->final_posr->pos[2];
+ // contact normal points to closest face
+ dVector3 tmp;
+ tmp[0] = 0;
+ tmp[1] = 0;
+ tmp[2] = 0;
+ tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
+ dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp);
+ // contact depth = distance to wall along normal plus radius
+ contact->depth = min_distance + sphere->radius;
+ return 1;
+ }
+
+ t[3] = 0; //@@@ hmmm
+ dMULTIPLY0_331 (q,o2->final_posr->R,t);
+ r[0] = p[0] - q[0];
+ r[1] = p[1] - q[1];
+ r[2] = p[2] - q[2];
+ depth = sphere->radius - dSqrt(dDOT(r,r));
+ if (depth < 0) return 0;
+ contact->pos[0] = q[0] + o2->final_posr->pos[0];
+ contact->pos[1] = q[1] + o2->final_posr->pos[1];
+ contact->pos[2] = q[2] + o2->final_posr->pos[2];
+ contact->normal[0] = r[0];
+ contact->normal[1] = r[1];
+ contact->normal[2] = r[2];
+ dNormalize3 (contact->normal);
+ contact->depth = depth;
+ return 1;
+}
+
+
+int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int /*flags*/,
+ dContactGeom *contact, int /*skip*/)
+{
+ dxSphere *sphere = (dxSphere*) o1;
+ dxPlane *plane = (dxPlane*) o2;
+
+ contact->g1 = o1;
+ contact->g2 = o2;
+ dReal k = dDOT (o1->final_posr->pos,plane->p);
+ dReal depth = plane->p[3] - k + sphere->radius;
+ if (depth >= 0) {
+ contact->normal[0] = plane->p[0];
+ contact->normal[1] = plane->p[1];
+ contact->normal[2] = plane->p[2];
+ contact->pos[0] = o1->final_posr->pos[0] - dMUL(plane->p[0],sphere->radius);
+ contact->pos[1] = o1->final_posr->pos[1] - dMUL(plane->p[1],sphere->radius);
+ contact->pos[2] = o1->final_posr->pos[2] - dMUL(plane->p[2],sphere->radius);
+ contact->depth = depth;
+ return 1;
+ }
+ else return 0;
+}