gmueller/bluecore
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
cirdan
2008-01-16 11:45:17 +00:00
commit 8f17a3a819
1068 changed files with 384278 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
ode/src/array.cpp Normal file
View File

@ -0,0 +1,80 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/config.h>
#include <ode/memory.h>
#include <ode/error.h>
#include "array.h"
static inline int roundUpToPowerOfTwo (int x)
{
int i = 1;
while (i < x) i <<= 1;
return i;
}
void dArrayBase::_freeAll (int sizeofT)
{
if (_data) {
if (_data == this+1) return; // if constructLocalArray() was called
dFree (_data,_anum * sizeofT);
}
}
void dArrayBase::_setSize (int newsize, int sizeofT)
{
if (newsize < 0) return;
if (newsize > _anum) {
if (_data == this+1) {
// this is a no-no, because constructLocalArray() was called
dDebug (0,"setSize() out of space in LOCAL array");
}
int newanum = roundUpToPowerOfTwo (newsize);
if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT);
else _data = dAlloc (newanum*sizeofT);
_anum = newanum;
}
_size = newsize;
}
void * dArrayBase::operator new (size_t size)
{
return dAlloc (size);
}
void dArrayBase::operator delete (void *ptr, size_t size)
{
dFree (ptr,size);
}
void dArrayBase::constructLocalArray (int __anum)
{
_size = 0;
_anum = __anum;
_data = this+1;
}

135
ode/src/array.h Normal file
View File

@ -0,0 +1,135 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/* this comes from the `reuse' library. copy any changes back to the source.
*
* Variable sized array template. The array is always stored in a contiguous
* chunk. The array can be resized. A size increase will cause more memory
* to be allocated, and may result in relocation of the array memory.
* A size decrease has no effect on the memory allocation.
*
* Array elements with constructors or destructors are not supported!
* But if you must have such elements, here's what to know/do:
* - Bitwise copy is used when copying whole arrays.
* - When copying individual items (via push(), insert() etc) the `='
* (equals) operator is used. Thus you should define this operator to do
* a bitwise copy. You should probably also define the copy constructor.
*/
#ifndef _ODE_ARRAY_H_
#define _ODE_ARRAY_H_
#include <ode/config.h>
// this base class has no constructors or destructor, for your convenience.
class dArrayBase {
protected:
int _size; // number of elements in `data'
int _anum; // allocated number of elements in `data'
void *_data; // array data
void _freeAll (int sizeofT);
void _setSize (int newsize, int sizeofT);
// set the array size to `newsize', allocating more memory if necessary.
// if newsize>_anum and is a power of two then this is guaranteed to
// set _size and _anum to newsize.
public:
// not: dArrayBase () { _size=0; _anum=0; _data=0; }
int size() const { return _size; }
int allocatedSize() const { return _anum; }
void * operator new (size_t size);
void operator delete (void *ptr, size_t size);
void constructor() { _size=0; _anum=0; _data=0; }
// if this structure is allocated with malloc() instead of new, you can
// call this to set it up.
void constructLocalArray (int __anum);
// this helper function allows non-reallocating arrays to be constructed
// on the stack (or in the heap if necessary). this is something of a
// kludge and should be used with extreme care. this function acts like
// a constructor - it is called on uninitialized memory that will hold the
// Array structure and the data. __anum is the number of elements that
// are allocated. the memory MUST be allocated with size:
// sizeof(ArrayBase) + __anum*sizeof(T)
// arrays allocated this way will never try to reallocate or free the
// memory - that's your job.
};
template <class T> class dArray : public dArrayBase {
public:
void equals (const dArray<T> &x) {
setSize (x.size());
memcpy (_data,x._data,x._size * sizeof(T));
}
dArray () { constructor(); }
dArray (const dArray<T> &x) { constructor(); equals (x); }
~dArray () { _freeAll(sizeof(T)); }
void setSize (int newsize) { _setSize (newsize,sizeof(T)); }
T *data() const { return (T*) _data; }
T & operator[] (int i) const { return ((T*)_data)[i]; }
void operator = (const dArray<T> &x) { equals (x); }
void push (const T item) {
if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T));
}
void swap (dArray<T> &x) {
int tmp1;
void *tmp2;
tmp1=_size; _size=x._size; x._size=tmp1;
tmp1=_anum; _anum=x._anum; x._anum=tmp1;
tmp2=_data; _data=x._data; x._data=tmp2;
}
// insert the item at the position `i'. if i<0 then add the item to the
// start, if i >= size then add the item to the end of the array.
void insert (int i, const T item) {
if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
if (i >= (_size-1)) i = _size-1; // add to end
else {
if (i < 0) i=0; // add to start
int n = _size-1-i;
if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T));
}
((T*)_data)[i] = item;
}
void remove (int i) {
if (i >= 0 && i < _size) { // passing this test guarantees size>0
int n = _size-1-i;
if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T));
_size--;
}
}
};
#endif

830
ode/src/box.cpp Normal file
View File

@ -0,0 +1,830 @@
/*************************************************************************
* *
* 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"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// box public API
dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
{
dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
type = dBoxClass;
side[0] = lx;
side[1] = ly;
side[2] = lz;
}
void dxBox::computeAABB()
{
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) +
dFabs (R[1] * side[1]) + dFabs (R[2] * side[2]));
dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) +
dFabs (R[5] * side[1]) + dFabs (R[6] * side[2]));
dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) +
dFabs (R[9] * side[1]) + dFabs (R[10] * side[2]));
aabb[0] = pos[0] - xrange;
aabb[1] = pos[0] + xrange;
aabb[2] = pos[1] - yrange;
aabb[3] = pos[1] + yrange;
aabb[4] = pos[2] - zrange;
aabb[5] = pos[2] + zrange;
}
dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
{
return new dxBox (space,lx,ly,lz);
}
void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
{
dUASSERT (g && g->type == dBoxClass,"argument not a box");
dAASSERT (lx > 0 && ly > 0 && lz > 0);
dxBox *b = (dxBox*) g;
b->side[0] = lx;
b->side[1] = ly;
b->side[2] = lz;
dGeomMoved (g);
}
void dGeomBoxGetLengths (dGeomID g, dVector3 result)
{
dUASSERT (g && g->type == dBoxClass,"argument not a box");
dxBox *b = (dxBox*) g;
result[0] = b->side[0];
result[1] = b->side[1];
result[2] = b->side[2];
}
dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dBoxClass,"argument not a box");
g->recomputePosr();
dxBox *b = (dxBox*) g;
// Set p = (x,y,z) relative to box center
//
// This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)
dVector3 p,q;
p[0] = x - b->final_posr->pos[0];
p[1] = y - b->final_posr->pos[1];
p[2] = z - b->final_posr->pos[2];
// Rotate p into box's coordinate frame, so we can
// treat the OBB as an AABB
dMULTIPLY1_331 (q,b->final_posr->R,p);
// Record distance from point to each successive box side, and see
// if the point is inside all six sides
dReal dist[6];
int i;
bool inside = true;
for (i=0; i < 3; i++) {
dReal side = b->side[i] * REAL(0.5);
dist[i ] = side - q[i];
dist[i+3] = side + q[i];
if ((dist[i] < 0) || (dist[i+3] < 0)) {
inside = false;
}
}
// If point is inside the box, the depth is the smallest positive distance
// to any side
if (inside) {
dReal smallest_dist = (dReal) (unsigned) -1;
for (i=0; i < 6; i++) {
if (dist[i] < smallest_dist) smallest_dist = dist[i];
}
return smallest_dist;
}
// Otherwise, if point is outside the box, the depth is the largest
// distance to any side. This is an approximation to the 'proper'
// solution (the proper solution may be larger in some cases).
dReal largest_dist = 0;
for (i=0; i < 6; i++) {
if (dist[i] > largest_dist) largest_dist = dist[i];
}
return -largest_dist;
}
//****************************************************************************
// box-box collision utility
// find all the intersection points between the 2D rectangle with vertices
// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
//
// the intersection points are returned as x,y pairs in the 'ret' array.
// the number of intersection points is returned by the function (this will
// be in the range 0 to 8).
static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16])
{
// q (and r) contain nq (and nr) coordinate points for the current (and
// chopped) polygons
int nq=4,nr;
dReal buffer[16];
dReal *q = p;
dReal *r = ret;
for (int dir=0; dir <= 1; dir++) {
// direction notation: xy[0] = x axis, xy[1] = y axis
for (int sign=-1; sign <= 1; sign += 2) {
// chop q along the line xy[dir] = sign*h[dir]
dReal *pq = q;
dReal *pr = r;
nr = 0;
for (int i=nq; i > 0; i--) {
// go through all points in q and all lines between adjacent points
if (sign*pq[dir] < h[dir]) {
// this point is inside the chopping line
pr[0] = pq[0];
pr[1] = pq[1];
pr += 2;
nr++;
if (nr & 8) {
q = r;
goto done;
}
}
dReal *nextq = (i > 1) ? pq+2 : q;
if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
// this line crosses the chopping line
pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
(nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
pr[dir] = sign*h[dir];
pr += 2;
nr++;
if (nr & 8) {
q = r;
goto done;
}
}
pq += 2;
}
q = r;
r = (q==ret) ? buffer : ret;
nq = nr;
}
}
done:
if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal));
return nr;
}
// given n points in the plane (array p, of size 2*n), generate m points that
// best represent the whole set. the definition of 'best' here is not
// predetermined - the idea is to select points that give good box-box
// collision detection behavior. the chosen point indexes are returned in the
// array iret (of size m). 'i0' is always the first entry in the array.
// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
// in the range [0..n-1].
void cullPoints (int n, dReal p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
dReal a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = REAL(0.5)*(p[0] + p[2]);
cy = REAL(0.5)*(p[1] + p[3]);
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
a += q;
cx += q*(p[i*2]+p[i*2+2]);
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
a = dRecip(REAL(3.0)*(a+q));
cx = a*(cx + q*(p[n*2-2]+p[0]));
cy = a*(cy + q*(p[n*2-1]+p[1]));
}
// compute the angle of each point w.r.t. the centroid
dReal A[8];
for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
// search for points that have angles closest to A[i0] + i*(2*pi/m).
int avail[8];
for (i=0; i<n; i++) avail[i] = 1;
avail[i0] = 0;
iret[0] = i0;
iret++;
for (j=1; j<m; j++) {
a = dReal(j)*(2*M_PI/m) + A[i0];
if (a > M_PI) a -= 2*M_PI;
dReal maxdiff=1e9,diff;
#ifndef dNODEBUG
*iret = i0; // iret is not allowed to keep this value
#endif
for (i=0; i<n; i++) {
if (avail[i]) {
diff = dFabs (A[i]-a);
if (diff > M_PI) diff = 2*M_PI - diff;
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
#ifndef dNODEBUG
dIASSERT (*iret != i0); // ensure iret got set
#endif
avail[*iret] = 0;
iret++;
}
}
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
// 1,2,3 = box 2 intersects with a face of box 1
// 4,5,6 = box 1 intersects with a face of box 2
// 7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
int dBoxBox (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2,
dVector3 normal, dReal *depth, int *return_code,
int maxc, dContactGeom *contact, int skip)
{
const dReal fudge_factor = REAL(1.05);
dVector3 p,pp,normalC;
const dReal *normalR = 0;
dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0]*REAL(0.5);
A[1] = side1[1]*REAL(0.5);
A[2] = side1[2]*REAL(0.5);
B[0] = side2[0]*REAL(0.5);
B[1] = side2[1]*REAL(0.5);
B[2] = side2[2]*REAL(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[4];
normal[2] = normalR[8];
}
else {
dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
*depth = -s;
// compute contact point(s)
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
dReal sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
dReal alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
contact[0].depth = *depth;
*return_code = code;
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = dFabs (nr[0]);
anr[1] = dFabs (nr[1]);
anr[2] = dFabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
dReal quad[8]; // 2D coordinate of incident face (x,y pairs)
dReal c1,c2,m11,m12,m21,m22;
c1 = dDOT14 (center,Ra+code1);
c2 = dDOT14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT44 (Ra+code1,Rb+a1);
m12 = dDOT44 (Ra+code1,Rb+a2);
m21 = dDOT44 (Ra+code2,Rb+a1);
m22 = dDOT44 (Ra+code2,Rb+a2);
{
dReal k1 = m11*Sb[a1];
dReal k2 = m21*Sb[a1];
dReal k3 = m12*Sb[a2];
dReal k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
dReal rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
dReal ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
dReal point[3*8]; // penetrating contact points
dReal dep[8]; // depths for those points
dReal det1 = dRecip(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
con->depth = dep[j];
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
dReal maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
con->depth = dep[iret[j]];
}
cnum = maxc;
}
*return_code = code;
return cnum;
}
int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dBoxClass);
dIASSERT (o2->type == dBoxClass);
dVector3 normal;
dReal depth;
int code;
dxBox *b1 = (dxBox*) o1;
dxBox *b2 = (dxBox*) o2;
int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
normal,&depth,&code,flags & NUMC_MASK,contact,skip);
for (int i=0; i<num; i++) {
CONTACT(contact,i*skip)->normal[0] = -normal[0];
CONTACT(contact,i*skip)->normal[1] = -normal[1];
CONTACT(contact,i*skip)->normal[2] = -normal[2];
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return num;
}
int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dBoxClass);
dIASSERT (o2->type == dPlaneClass);
dxBox *box = (dxBox*) o1;
dxPlane *plane = (dxPlane*) o2;
contact->g1 = o1;
contact->g2 = o2;
int ret = 0;
//@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
const dReal *R = o1->final_posr->R; // rotation of box
const dReal *n = plane->p; // normal vector
// project sides lengths along normal vector, get absolute values
dReal Q1 = dDOT14(n,R+0);
dReal Q2 = dDOT14(n,R+1);
dReal Q3 = dDOT14(n,R+2);
dReal A1 = box->side[0] * Q1;
dReal A2 = box->side[1] * Q2;
dReal A3 = box->side[2] * Q3;
dReal B1 = dFabs(A1);
dReal B2 = dFabs(A2);
dReal B3 = dFabs(A3);
// early exit test
dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->final_posr->pos);
if (depth < 0) return 0;
// find number of contacts requested
int maxc = flags & NUMC_MASK;
if (maxc < 1) maxc = 1;
if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed
// find deepest point
dVector3 p;
p[0] = o1->final_posr->pos[0];
p[1] = o1->final_posr->pos[1];
p[2] = o1->final_posr->pos[2];
#define FOO(i,op) \
p[0] op REAL(0.5)*box->side[i] * R[0+i]; \
p[1] op REAL(0.5)*box->side[i] * R[4+i]; \
p[2] op REAL(0.5)*box->side[i] * R[8+i];
#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) }
BAR(0,1);
BAR(1,2);
BAR(2,3);
#undef FOO
#undef BAR
// the deepest point is the first contact point
contact->pos[0] = p[0];
contact->pos[1] = p[1];
contact->pos[2] = p[2];
contact->normal[0] = n[0];
contact->normal[1] = n[1];
contact->normal[2] = n[2];
contact->depth = depth;
ret = 1; // ret is number of contact points found so far
if (maxc == 1) goto done;
// get the second and third contact points by starting from `p' and going
// along the two sides with the smallest projected length.
#define FOO(i,j,op) \
CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \
CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \
CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j];
#define BAR(ctact,side,sideinc) \
depth -= B ## sideinc; \
if (depth < 0) goto done; \
if (A ## sideinc > 0) { FOO(ctact,side,+) } else { FOO(ctact,side,-) } \
CONTACT(contact,ctact*skip)->depth = depth; \
ret++;
CONTACT(contact,skip)->normal[0] = n[0];
CONTACT(contact,skip)->normal[1] = n[1];
CONTACT(contact,skip)->normal[2] = n[2];
if (maxc == 3) {
CONTACT(contact,2*skip)->normal[0] = n[0];
CONTACT(contact,2*skip)->normal[1] = n[1];
CONTACT(contact,2*skip)->normal[2] = n[2];
}
if (B1 < B2) {
if (B3 < B1) goto use_side_3; else {
BAR(1,0,1); // use side 1
if (maxc == 2) goto done;
if (B2 < B3) goto contact2_2; else goto contact2_3;
}
}
else {
if (B3 < B2) {
use_side_3: // use side 3
BAR(1,2,3);
if (maxc == 2) goto done;
if (B1 < B2) goto contact2_1; else goto contact2_2;
}
else {
BAR(1,1,2); // use side 2
if (maxc == 2) goto done;
if (B1 < B3) goto contact2_1; else goto contact2_3;
}
}
contact2_1: BAR(2,0,1); goto done;
contact2_2: BAR(2,1,2); goto done;
contact2_3: BAR(2,2,3); goto done;
#undef FOO
#undef BAR
done:
for (int i=0; i<ret; i++) {
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return ret;
}

361
ode/src/capsule.cpp Normal file
View File

@ -0,0 +1,361 @@
/*************************************************************************
* *
* 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"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// capped cylinder public API
dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) :
dxGeom (space,1)
{
dAASSERT (_radius > 0 && _length > 0);
type = dCapsuleClass;
radius = _radius;
lz = _length;
}
void dxCapsule::computeAABB()
{
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dReal xrange = dFabs(R[2] * lz) * REAL(0.5) + radius;
dReal yrange = dFabs(R[6] * lz) * REAL(0.5) + radius;
dReal zrange = dFabs(R[10] * lz) * REAL(0.5) + radius;
aabb[0] = pos[0] - xrange;
aabb[1] = pos[0] + xrange;
aabb[2] = pos[1] - yrange;
aabb[3] = pos[1] + yrange;
aabb[4] = pos[2] - zrange;
aabb[5] = pos[2] + zrange;
}
dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length)
{
return new dxCapsule (space,radius,length);
}
void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
dAASSERT (radius > 0 && length > 0);
dxCapsule *c = (dxCapsule*) g;
c->radius = radius;
c->lz = length;
dGeomMoved (g);
}
void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
dxCapsule *c = (dxCapsule*) g;
*radius = c->radius;
*length = c->lz;
}
dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
g->recomputePosr();
dxCapsule *c = (dxCapsule*) g;
const dReal* R = g->final_posr->R;
const dReal* pos = g->final_posr->pos;
dVector3 a;
a[0] = x - pos[0];
a[1] = y - pos[1];
a[2] = z - pos[2];
dReal beta = dDOT14(a,R+2);
dReal lz2 = c->lz*REAL(0.5);
if (beta < -lz2) beta = -lz2;
else if (beta > lz2) beta = lz2;
a[0] = c->final_posr->pos[0] + beta*R[0*4+2];
a[1] = c->final_posr->pos[1] + beta*R[1*4+2];
a[2] = c->final_posr->pos[2] + beta*R[2*4+2];
return c->radius -
dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2]));
}
int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dSphereClass);
dxCapsule *ccyl = (dxCapsule*) o1;
dxSphere *sphere = (dxSphere*) o2;
contact->g1 = o1;
contact->g2 = o2;
// find the point on the cylinder axis that is closest to the sphere
dReal alpha =
o1->final_posr->R[2] * (o2->final_posr->pos[0] - o1->final_posr->pos[0]) +
o1->final_posr->R[6] * (o2->final_posr->pos[1] - o1->final_posr->pos[1]) +
o1->final_posr->R[10] * (o2->final_posr->pos[2] - o1->final_posr->pos[2]);
dReal lz2 = ccyl->lz * REAL(0.5);
if (alpha > lz2) alpha = lz2;
if (alpha < -lz2) alpha = -lz2;
// collide the spheres
dVector3 p;
p[0] = o1->final_posr->pos[0] + alpha * o1->final_posr->R[2];
p[1] = o1->final_posr->pos[1] + alpha * o1->final_posr->R[6];
p[2] = o1->final_posr->pos[2] + alpha * o1->final_posr->R[10];
return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact);
}
int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dBoxClass);
dxCapsule *cyl = (dxCapsule*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
// get p1,p2 = cylinder axis endpoints, get radius
dVector3 p1,p2;
dReal clen = cyl->lz * REAL(0.5);
p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2];
p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6];
p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10];
p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2];
p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6];
p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10];
dReal radius = cyl->radius;
// copy out box center, rotation matrix, and side array
dReal *c = o2->final_posr->pos;
dReal *R = o2->final_posr->R;
const dReal *side = box->side;
// get the closest point between the cylinder axis and the box
dVector3 pl,pb;
dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
// generate contact point
return dCollideSpheres (pl,radius,pb,0,contact);
}
int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
int i;
const dReal tolerance = REAL(1e-5);
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dCapsuleClass);
dxCapsule *cyl1 = (dxCapsule*) o1;
dxCapsule *cyl2 = (dxCapsule*) o2;
contact->g1 = o1;
contact->g2 = o2;
// copy out some variables, for convenience
dReal lz1 = cyl1->lz * REAL(0.5);
dReal lz2 = cyl2->lz * REAL(0.5);
dReal *pos1 = o1->final_posr->pos;
dReal *pos2 = o2->final_posr->pos;
dReal axis1[3],axis2[3];
axis1[0] = o1->final_posr->R[2];
axis1[1] = o1->final_posr->R[6];
axis1[2] = o1->final_posr->R[10];
axis2[0] = o2->final_posr->R[2];
axis2[1] = o2->final_posr->R[6];
axis2[2] = o2->final_posr->R[10];
// if the cylinder axes are close to parallel, we'll try to detect up to
// two contact points along the body of the cylinder. if we can't find any
// points then we'll fall back to the closest-points algorithm. note that
// we are not treating this special case for reasons of degeneracy, but
// because we want two contact points in some situations. the closet-points
// algorithm is robust in all casts, but it can return only one contact.
dVector3 sphere1,sphere2;
dReal a1a2 = dDOT (axis1,axis2);
dReal det = REAL(1.0)-a1a2*a1a2;
if (det < tolerance) {
// the cylinder axes (almost) parallel, so we will generate up to two
// contacts. alpha1 and alpha2 (line position parameters) are related by:
// alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2)
// or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2)
// first compute where the two cylinders overlap in alpha1 space:
if (a1a2 < 0) {
axis2[0] = -axis2[0];
axis2[1] = -axis2[1];
axis2[2] = -axis2[2];
}
dReal q[3];
for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
dReal k = dDOT (axis1,q);
dReal a1lo = -lz1;
dReal a1hi = lz1;
dReal a2lo = -lz2 - k;
dReal a2hi = lz2 - k;
dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
if (lo <= hi) {
int num_contacts = flags & NUMC_MASK;
if (num_contacts >= 2 && lo < hi) {
// generate up to two contacts. if one of those contacts is
// not made, fall back on the one-contact strategy.
for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
int n1 = dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
if (n1) {
for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
dContactGeom *c2 = CONTACT(contact,skip);
int n2 = dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius, c2);
if (n2) {
c2->g1 = o1;
c2->g2 = o2;
return 2;
}
}
}
// just one contact to generate, so put it in the middle of
// the range
dReal alpha1 = (lo + hi) * REAL(0.5);
dReal alpha2 = alpha1 + k;
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
return dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
}
}
// use the closest point algorithm
dVector3 a1,a2,b1,b2;
a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1;
a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1;
a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1;
a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1;
a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1;
a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1;
b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2;
b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2;
b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2;
b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2;
b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2;
b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2;
dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2);
return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
}
int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dPlaneClass);
dxCapsule *ccyl = (dxCapsule*) o1;
dxPlane *plane = (dxPlane*) o2;
// collide the deepest capping sphere with the plane
dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
dVector3 p;
p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
dReal k = dDOT (p,plane->p);
dReal depth = plane->p[3] - k + ccyl->radius;
if (depth < 0) return 0;
contact->normal[0] = plane->p[0];
contact->normal[1] = plane->p[1];
contact->normal[2] = plane->p[2];
contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
contact->depth = depth;
int ncontacts = 1;
if ((flags & NUMC_MASK) >= 2) {
// collide the other capping sphere with the plane
p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
k = dDOT (p,plane->p);
depth = plane->p[3] - k + ccyl->radius;
if (depth >= 0) {
dContactGeom *c2 = CONTACT(contact,skip);
c2->normal[0] = plane->p[0];
c2->normal[1] = plane->p[1];
c2->normal[2] = plane->p[2];
c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
c2->depth = depth;
ncontacts = 2;
}
}
for (int i=0; i < ncontacts; i++) {
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return ncontacts;
}

986
ode/src/collision_cylinder_box.cpp Normal file
View File

@ -0,0 +1,986 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
/*
* Cylinder-box collider by Alen Ladavac
* Ported to ODE by Nguyen Binh
*/
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"
static const int MAX_CYLBOX_CLIP_POINTS = 16;
static const int nCYLINDER_AXIS = 2;
// Number of segment of cylinder base circle.
// Must be divisible by 4.
static const int nCYLINDER_SEGMENT = 8;
#define MAX_FLOAT dInfinity
// Data that passed through the collider's functions
typedef struct _sCylinderBoxData
{
// cylinder parameters
dMatrix3 mCylinderRot;
dVector3 vCylinderPos;
dVector3 vCylinderAxis;
dReal fCylinderRadius;
dReal fCylinderSize;
dVector3 avCylinderNormals[nCYLINDER_SEGMENT];
// box parameters
dMatrix3 mBoxRot;
dVector3 vBoxPos;
dVector3 vBoxHalfSize;
// box vertices array : 8 vertices
dVector3 avBoxVertices[8];
// global collider data
dVector3 vDiff;
dVector3 vNormal;
dReal fBestDepth;
dReal fBestrb;
dReal fBestrc;
int iBestAxis;
// contact data
dVector3 vEp0, vEp1;
dReal fDepth0, fDepth1;
// ODE stuff
dGeomID gBox;
dGeomID gCylinder;
dContactGeom* gContact;
int iFlags;
int iSkip;
int nContacts;
} sCylinderBoxData;
// initialize collision data
void _cldInitCylinderBox(sCylinderBoxData& cData)
{
// get cylinder position, orientation
const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
dMatrix3Copy(pRotCyc,cData.mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
dVector3Copy(*pPosCyc,cData.vCylinderPos);
dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
// get cylinder radius and size
dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
// get box position, orientation, size
const dReal* pRotBox = dGeomGetRotation(cData.gBox);
dMatrix3Copy(pRotBox,cData.mBoxRot);
const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
dVector3Copy(*pPosBox,cData.vBoxPos);
dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
cData.vBoxHalfSize[0] *= REAL(0.5);
cData.vBoxHalfSize[1] *= REAL(0.5);
cData.vBoxHalfSize[2] *= REAL(0.5);
// vertex 0
cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
// vertex 1
cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
// vertex 2
cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
// vertex 3
cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
// vertex 4
cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
// vertex 5
cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
// vertex 6
cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
// vertex 7
cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
// temp index
int i = 0;
dVector3 vTempBoxVertices[8];
// transform vertices in absolute space
for(i=0; i < 8; i++)
{
dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
}
// find relative position
dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
cData.fBestDepth = MAX_FLOAT;
cData.vNormal[0] = REAL(0.0);
cData.vNormal[1] = REAL(0.0);
cData.vNormal[2] = REAL(0.0);
// calculate basic angle for nCYLINDER_SEGMENT-gon
dReal fAngle = M_PI/nCYLINDER_SEGMENT;
// calculate angle increment
dReal fAngleIncrement = fAngle * REAL(2.0);
// calculate nCYLINDER_SEGMENT-gon points
for(i = 0; i < nCYLINDER_SEGMENT; i++)
{
cData.avCylinderNormals[i][0] = -dCos(fAngle);
cData.avCylinderNormals[i][1] = -dSin(fAngle);
cData.avCylinderNormals[i][2] = 0;
fAngle += fAngleIncrement;
}
cData.fBestrb = 0;
cData.fBestrc = 0;
cData.iBestAxis = 0;
cData.nContacts = 0;
}
// test for given separating axis
int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis )
{
// check length of input normal
dReal fL = dVector3Length(vInputNormal);
// if not long enough
if ( fL < 1e-5f )
{
// do nothing
return 1;
}
// otherwise make it unit for sure
dNormalize3(vInputNormal);
// project box and Cylinder on mAxis
dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal);
dReal frc;
if (fdot1 > REAL(1.0))
{
fdot1 = REAL(1.0);
frc = dFabs(cData.fCylinderSize*REAL(0.5));
}
// project box and capsule on iAxis
frc = dFabs( fdot1 * (cData.fCylinderSize*REAL(0.5))) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
dVector3 vTemp1;
dReal frb = REAL(0.0);
dMat3GetCol(cData.mBoxRot,0,vTemp1);
frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0];
dMat3GetCol(cData.mBoxRot,1,vTemp1);
frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1];
dMat3GetCol(cData.mBoxRot,2,vTemp1);
frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2];
// project their distance on separating axis
dReal fd = dVector3Dot(cData.vDiff,vInputNormal);
// if they do not overlap exit, we have no intersection
if ( dFabs(fd) > frc+frb )
{
return 0;
}
// get depth
dReal fDepth = - dFabs(fd) + (frc+frb);
// get maximum depth
if ( fDepth < cData.fBestDepth )
{
cData.fBestDepth = fDepth;
dVector3Copy(vInputNormal,cData.vNormal);
cData.iBestAxis = iAxis;
cData.fBestrb = frb;
cData.fBestrc = frc;
// flip normal if interval is wrong faced
if (fd > 0)
{
dVector3Inv(cData.vNormal);
}
}
return 1;
}
// check for separation between box edge and cylinder circle edge
int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
const dVector3 &vCenterPoint,
const dVector3 &vVx0, const dVector3 &vVx1,
int iAxis )
{
// calculate direction of edge
dVector3 vDirEdge;
dVector3Subtract(vVx1,vVx0,vDirEdge);
dNormalize3(vDirEdge);
// starting point of edge
dVector3 vEStart;
dVector3Copy(vVx0,vEStart);;
// calculate angle cosine between cylinder axis and edge
dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);
// if edge is perpendicular to cylinder axis
if(dFabs(fdot2) < 1e-5f)
{
// this can't be separating axis, because edge is parallel to circle plane
return 1;
}
// find point of intersection between edge line and circle plane
dVector3 vTemp1;
dVector3Subtract(vCenterPoint,vEStart,vTemp1);
dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
dVector3 vpnt;
vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2);
vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2);
vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2);
// find tangent vector on circle with same center (vCenterPoint) that
// touches point of intersection (vpnt)
dVector3 vTangent;
dVector3Subtract(vCenterPoint,vpnt,vTemp1);
dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
// find vector orthogonal both to tangent and edge direction
dVector3 vAxis;
dVector3Cross(vTangent,vDirEdge,vAxis);
// use that vector as separating axis
return _cldTestAxis( cData, vAxis, iAxis );
}
// Test separating axis for collision
int _cldTestSeparatingAxes(sCylinderBoxData& cData)
{
// reset best axis
cData.fBestDepth = MAX_FLOAT;
cData.iBestAxis = 0;
cData.fBestrb = 0;
cData.fBestrc = 0;
cData.nContacts = 0;
dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
// Epsilon value for checking axis vector length
const dReal fEpsilon = 1e-6f;
// axis A0
dMat3GetCol(cData.mBoxRot, 0 , vAxis);
if (!_cldTestAxis( cData, vAxis, 1 ))
{
return 0;
}
// axis A1
dMat3GetCol(cData.mBoxRot, 1 , vAxis);
if (!_cldTestAxis( cData, vAxis, 2 ))
{
return 0;
}
// axis A2
dMat3GetCol(cData.mBoxRot, 2 , vAxis);
if (!_cldTestAxis( cData, vAxis, 3 ))
{
return 0;
}
// axis C - Cylinder Axis
//vAxis = vCylinderAxis;
dVector3Copy(cData.vCylinderAxis , vAxis);
if (!_cldTestAxis( cData, vAxis, 4 ))
{
return 0;
}
// axis CxA0
//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 ));
dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis);
if(dVector3Length2( vAxis ) > fEpsilon )
{
if (!_cldTestAxis( cData, vAxis, 5 ))
{
return 0;
}
}
// axis CxA1
//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 ));
dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis);
if(dVector3Length2( vAxis ) > fEpsilon )
{
if (!_cldTestAxis( cData, vAxis, 6 ))
{
return 0;
}
}
// axis CxA2
//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 ));
dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis);
if(dVector3Length2( vAxis ) > fEpsilon )
{
if (!_cldTestAxis( cData, vAxis, 7 ))
{
return 0;
}
}
int i = 0;
dVector3 vTemp1;
dVector3 vTemp2;
// here we check box's vertices axis
for(i=0; i< 8; i++)
{
//vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos));
dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1);
dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2);
//vAxis = ( vCylinderAxis cross vAxis );
dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis);
if(dVector3Length2( vAxis ) > fEpsilon )
{
if (!_cldTestAxis( cData, vAxis, 8 + i ))
{
return 0;
}
}
}
// ************************************
// this is defined for first 12 axes
// normal of plane that contains top circle of cylinder
// center of top circle of cylinder
dVector3 vcc;
vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
// ************************************
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27))
{
return 0;
}
// ************************************
// this is defined for second 12 axes
// normal of plane that contains bottom circle of cylinder
// center of bottom circle of cylinder
// vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5));
vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
// ************************************
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38))
{
return 0;
}
if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39))
{
return 0;
}
return 1;
}
int _cldClipCylinderToBox(sCylinderBoxData& cData)
{
// calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
dVector3 vN;
dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal);
vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1;
vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1;
vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1;
// normalize that vector
dNormalize3(vN);
// translate cylinder end points by the vector
dVector3 vCposTrans;
vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius;
vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius;
vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius;
cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
// transform edge points in box space
cData.vEp0[0] -= cData.vBoxPos[0];
cData.vEp0[1] -= cData.vBoxPos[1];
cData.vEp0[2] -= cData.vBoxPos[2];
cData.vEp1[0] -= cData.vBoxPos[0];
cData.vEp1[1] -= cData.vBoxPos[1];
cData.vEp1[2] -= cData.vBoxPos[2];
dVector3 vTemp1;
// clip the edge to box
dVector4 plPlane;
// plane 0 +x
dMat3GetCol(cData.mBoxRot,0,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 1 +y
dMat3GetCol(cData.mBoxRot,1,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 2 +z
dMat3GetCol(cData.mBoxRot,2,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 3 -x
dMat3GetCol(cData.mBoxRot,0,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 4 -y
dMat3GetCol(cData.mBoxRot,1,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 5 -z
dMat3GetCol(cData.mBoxRot,2,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// calculate depths for both contact points
cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal);
cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal);
// clamp depths to 0
if(cData.fDepth0<0)
{
cData.fDepth0 = REAL(0.0);
}
if(cData.fDepth1<0)
{
cData.fDepth1 = REAL(0.0);
}
// back transform edge points from box to absolute space
cData.vEp0[0] += cData.vBoxPos[0];
cData.vEp0[1] += cData.vBoxPos[1];
cData.vEp0[2] += cData.vBoxPos[2];
cData.vEp1[0] += cData.vBoxPos[0];
cData.vEp1[1] += cData.vBoxPos[1];
cData.vEp1[2] += cData.vBoxPos[2];
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = cData.fDepth0;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(cData.vEp0,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact1->depth = cData.fDepth1;
dVector3Copy(cData.vNormal,Contact1->normal);
dVector3Copy(cData.vEp1,Contact1->pos);
Contact1->g1 = cData.gCylinder;
Contact1->g2 = cData.gBox;
dVector3Inv(Contact1->normal);
cData.nContacts++;
return 1;
}
void _cldClipBoxToCylinder(sCylinderBoxData& cData )
{
dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
// check which circle from cylinder we take for clipping
if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) )
{
// get top circle
vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
vCylinderCircleNormal_Rel[0] = REAL(0.0);
vCylinderCircleNormal_Rel[1] = REAL(0.0);
vCylinderCircleNormal_Rel[2] = REAL(0.0);
vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
}
else
{
// get bottom circle
vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
vCylinderCircleNormal_Rel[0] = REAL(0.0);
vCylinderCircleNormal_Rel[1] = REAL(0.0);
vCylinderCircleNormal_Rel[2] = REAL(0.0);
vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
}
// vNr is normal in Box frame, pointing from Cylinder to Box
dVector3 vNr;
dMatrix3 mBoxInv;
// Find a way to use quaternion
dMatrix3Inv(cData.mBoxRot,mBoxInv);
dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr);
dVector3 vAbsNormal;
vAbsNormal[0] = dFabs( vNr[0] );
vAbsNormal[1] = dFabs( vNr[1] );
vAbsNormal[2] = dFabs( vNr[2] );
// find which face in box is closest to cylinder
int iB0, iB1, iB2;
// Different from Croteam's code
if (vAbsNormal[1] > vAbsNormal[0])
{
// 1 > 0
if (vAbsNormal[0]> vAbsNormal[2])
{
// 0 > 2 -> 1 > 0 >2
iB0 = 1; iB1 = 0; iB2 = 2;
}
else
{
// 2 > 0-> Must compare 1 and 2
if (vAbsNormal[1] > vAbsNormal[2])
{
// 1 > 2 -> 1 > 2 > 0
iB0 = 1; iB1 = 2; iB2 = 0;
}
else
{
// 2 > 1 -> 2 > 1 > 0;
iB0 = 2; iB1 = 1; iB2 = 0;
}
}
}
else
{
// 0 > 1
if (vAbsNormal[1] > vAbsNormal[2])
{
// 1 > 2 -> 0 > 1 > 2
iB0 = 0; iB1 = 1; iB2 = 2;
}
else
{
// 2 > 1 -> Must compare 0 and 2
if (vAbsNormal[0] > vAbsNormal[2])
{
// 0 > 2 -> 0 > 2 > 1;
iB0 = 0; iB1 = 2; iB2 = 1;
}
else
{
// 2 > 0 -> 2 > 0 > 1;
iB0 = 2; iB1 = 0; iB2 = 1;
}
}
}
dVector3 vCenter;
// find center of box polygon
dVector3 vTemp;
if (vNr[iB0] > 0)
{
dMat3GetCol(cData.mBoxRot,iB0,vTemp);
vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2];
}
else
{
dMat3GetCol(cData.mBoxRot,iB0,vTemp);
vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2];
}
// find the vertices of box polygon
dVector3 avPoints[4];
dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];
int i=0;
for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++)
{
avTempArray1[i][0] = REAL(0.0);
avTempArray1[i][1] = REAL(0.0);
avTempArray1[i][2] = REAL(0.0);
avTempArray2[i][0] = REAL(0.0);
avTempArray2[i][1] = REAL(0.0);
avTempArray2[i][2] = REAL(0.0);
}
dVector3 vAxis1, vAxis2;
dMat3GetCol(cData.mBoxRot,iB1,vAxis1);
dMat3GetCol(cData.mBoxRot,iB2,vAxis2);
avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
// transform box points to space of cylinder circle
dMatrix3 mCylinderInv;
dMatrix3Inv(cData.mCylinderRot,mCylinderInv);
for(i=0; i<4; i++)
{
dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
}
int iTmpCounter1 = 0;
int iTmpCounter2 = 0;
dVector4 plPlane;
// plane of cylinder that contains circle for intersection
dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);
// Body of base circle of Cylinder
int nCircleSegment = 0;
for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++)
{
dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);
if (0 == (nCircleSegment % 2))
{
dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
}
else
{
dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
}
dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS );
dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS );
}
// back transform clipped points to absolute space
dReal ftmpdot;
dReal fTempDepth;
dVector3 vPoint;
if (nCircleSegment %2)
{
for( i=0; i<iTmpCounter2; i++)
{
dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, cData.vNormal);
fTempDepth = cData.fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
}
}
}
else
{
for( i=0; i<iTmpCounter1; i++)
{
dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, cData.vNormal);
fTempDepth = cData.fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
}
}
}
}
// Cylinder - Box by CroTeam
// Ported by Nguyen Binh
int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
sCylinderBoxData cData;
// Assign ODE stuff
cData.gCylinder = o1;
cData.gBox = o2;
cData.iFlags = flags;
cData.iSkip = skip;
cData.gContact = contact;
// initialize collider
_cldInitCylinderBox( cData );
// do intersection test and find best separating axis
if(!_cldTestSeparatingAxes( cData ) )
{
// if not found do nothing
return 0;
}
// if best separation axis is not found
if ( cData.iBestAxis == 0 )
{
// this should not happen (we should already exit in that case)
dIASSERT(0);
// do nothing
return 0;
}
dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis);
// choose which clipping method are we going to apply
if (dFabs(fdot) < REAL(0.9) )
{
// clip cylinder over box
if(!_cldClipCylinderToBox(cData))
{
return 0;
}
}
else
{
_cldClipBoxToCylinder(cData);
}
return cData.nContacts;
}

293
ode/src/collision_cylinder_plane.cpp Normal file
View File

@ -0,0 +1,293 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
/*
* Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de )
*
* This testing basically comes down to testing the intersection
* of the cylinder caps (discs) with the plane.
*
*/
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include <ode/objects.h>
#include "collision_kernel.h" // for dxGeom
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT ((flags & 0xffff) >= 1);
unsigned char* pContactData = (unsigned char*)contact;
int GeomCount = 0; // count of used contactgeoms
#ifdef dSINGLE
const dReal toleranz = 0.0001f;
#endif
#ifdef dDOUBLE
const dReal toleranz = 0.0000001;
#endif
// Get the properties of the cylinder (length+radius)
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
// and the plane
dVector4 planevec;
dGeomPlaneGetParams(Plane, planevec);
dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * dReal(0.5);
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
// parallel-check
s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
if(s < 0)
s += dReal(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
else
s -= dReal(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
if(s < toleranz && s > (-toleranz))
{
// discs are parallel to the plane
// 1.compute if, and where contacts are
dVector3 P;
s = planevec[3] - planevec[0] * G1Pos1[0] - planevec[1] * G1Pos1[1] - planevec[2] * G1Pos1[2];
dReal t;
t = planevec[3] - planevec[0] * G1Pos2[0] - planevec[1] * G1Pos2[1] - planevec[2] * G1Pos2[2];
if(s >= t) // s == t does never happen,
{
if(s >= 0)
{
// 1. Disc
P[0] = G1Pos1[0];
P[1] = G1Pos1[1];
P[2] = G1Pos1[2];
}
else
return GeomCount; // no contacts
}
else
{
if(t >= 0)
{
// 2. Disc
P[0] = G1Pos2[0];
P[1] = G1Pos2[1];
P[2] = G1Pos2[2];
}
else
return GeomCount; // no contacts
}
// 2. generate a coordinate-system on the disc
dVector3 V1, V2;
if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
{
// not x-axis
V1[0] = vDir1[0] + dReal(1.0); // random value
V1[1] = vDir1[1];
V1[2] = vDir1[2];
}
else
{
// maybe x-axis
V1[0] = vDir1[0];
V1[1] = vDir1[1] + dReal(1.0); // random value
V1[2] = vDir1[2];
}
// V1 is now another direction than vDir1
// Cross-product
V2[0] = V1[1] * vDir1[2] - V1[2] * vDir1[1];
V2[1] = V1[2] * vDir1[0] - V1[0] * vDir1[2];
V2[2] = V1[0] * vDir1[1] - V1[1] * vDir1[0];
// make unit V2
t = dReal(sqrt(V2[0] * V2[0] + V2[1] * V2[1] + V2[2] * V2[2]));
t = radius / t;
V2[0] *= t;
V2[1] *= t;
V2[2] *= t;
// cross again
V1[0] = V2[1] * vDir1[2] - V2[2] * vDir1[1];
V1[1] = V2[2] * vDir1[0] - V2[0] * vDir1[2];
V1[2] = V2[0] * vDir1[1] - V2[1] * vDir1[0];
// |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
// V1 = first axis
// V2 = second axis
// 3. generate contactpoints
// Potential contact 1
contact->pos[0] = P[0] + V1[0];
contact->pos[1] = P[1] + V1[1];
contact->pos[2] = P[2] + V1[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// Potential contact 2
contact->pos[0] = P[0] - V1[0];
contact->pos[1] = P[1] - V1[1];
contact->pos[2] = P[2] - V1[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// Potential contact 3
contact->pos[0] = P[0] + V2[0];
contact->pos[1] = P[1] + V2[1];
contact->pos[2] = P[2] + V2[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// Potential contact 4
contact->pos[0] = P[0] - V2[0];
contact->pos[1] = P[1] - V2[1];
contact->pos[2] = P[2] - V2[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
}
else
{
dReal t = -((-PlaneNormal[0]) * vDir1[0] + (-PlaneNormal[1]) * vDir1[1] + (-PlaneNormal[2]) * vDir1[2]);
C[0] = vDir1[0] * t - PlaneNormal[0];
C[1] = vDir1[1] * t - PlaneNormal[1];
C[2] = vDir1[2] * t - PlaneNormal[2];
s = dReal(sqrt(C[0] * C[0] + C[1] * C[1] + C[2] * C[2]));
// move C onto the circle
s = radius / s;
C[0] *= s;
C[1] *= s;
C[2] *= s;
// deepest point of disc 1
contact->pos[0] = C[0] + G1Pos1[0];
contact->pos[1] = C[1] + G1Pos1[1];
contact->pos[2] = C[2] + G1Pos1[2];
// depth of the deepest point
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth >= 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// C is still computed
// deepest point of disc 2
contact->pos[0] = C[0] + G1Pos2[0];
contact->pos[1] = C[1] + G1Pos2[1];
contact->pos[2] = C[2] + G1Pos2[2];
// depth of the deepest point
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth >= 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
}
return GeomCount;
}

261
ode/src/collision_cylinder_sphere.cpp Normal file
View File

@ -0,0 +1,261 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
/*******************************************************************
* *
* cylinder-sphere collider by Christoph Beyer (boernerb@web.de) *
* *
* In Cylinder/Sphere-collisions, there are three possibilies: *
* 1. collision with the cylinder's nappe *
* 2. collision with one of the cylinder's disc *
* 3. collision with one of the disc's border *
* *
* This collider computes two distances (s, t) and based on them, *
* it decides, which collision we have. *
* This collider always generates 1 (or 0, if we have no collison) *
* contacts. *
* It is able to "separate" cylinder and sphere in all *
* configurations, but it never pays attention to velocity. *
* So, in extrem situations, "tunneling-effect" is possible. *
* *
*******************************************************************/
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include <ode/objects.h>
#include "collision_kernel.h" // for dxGeom
int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT ((flags & 0xffff) >= 1);
unsigned char* pContactData = (unsigned char*)contact;
int GeomCount = 0; // count of used contacts
#ifdef dSINGLE
const dReal toleranz = 0.0001f;
#endif
#ifdef dDOUBLE
const dReal toleranz = 0.0000001;
#endif
// get the data from the geoms
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
const dReal* pfRot1 = dGeomGetRotation(Cylinder);
dReal radius2;
radius2 = dGeomSphereGetRadius(Sphere);
const dReal* SpherePos = dGeomGetPosition(Sphere);
// G1Pos1 is the middle of the first disc
// G1Pos2 is the middle of the second disc
// vDir1 is the unit direction of the cylinderaxis
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * dReal(0.5); // just a precomputed factor
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
dReal t;
// Step 1: compute the two distances 's' and 't'
// 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
if(s < (-radius2) || s > (length + radius2) )
{
// Sphere is too far away from the discs
// no collision
return 0;
}
// C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
// t is the distance from the Sphere-middle to the cylinder-axis!
t = dReal(sqrt(C[0] * C[0] + C[1] * C[1] + C[2] * C[2]) );
if(t > (radius + radius2) )
{
// Sphere is too far away from the cylinder axis!
// no collision
return 0;
}
// decide which kind of collision we have:
if(t > radius && (s < 0 || s > length) )
{
// 3. collision
if(s <= 0)
{
contact->depth = radius2 - dReal(sqrt( (s) * (s) + (t - radius) * (t - radius) ));
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
else
{
// now s is bigger than length here!
contact->depth = radius2 - dReal(sqrt( (s - length) * (s - length) + (t - radius) * (t - radius) ));
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
else if( (radius - t) <= s && (radius - t) <= (length - s) )
{
// 1. collsision
if(t > (radius2 + toleranz))
{
// cylinder-axis is outside the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
C[0] /= t;
C[1] /= t;
C[2] /= t;
contact->pos[0] = C[0] * radius2 + SpherePos[0];
contact->pos[1] = C[1] * radius2 + SpherePos[1];
contact->pos[2] = C[2] * radius2 + SpherePos[2];
contact->normal[0] = C[0];
contact->normal[1] = C[1];
contact->normal[2] = C[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
else
{
// cylinder-axis is outside of the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
contact->pos[0] = C[0] + SpherePos[0];
contact->pos[1] = C[1] + SpherePos[1];
contact->pos[2] = C[2] + SpherePos[2];
contact->normal[0] = C[0] / t;
contact->normal[1] = C[1] / t;
contact->normal[2] = C[2] / t;
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
}
else
{
// 2. collision
if(s <= (length * dReal(0.5)) )
{
// collsision with the first disc
contact->depth = s + radius2;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
contact->normal[0] = vDir1[0];
contact->normal[1] = vDir1[1];
contact->normal[2] = vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
else
{
// collsision with the second disc
contact->depth = (radius2 + length - s);
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
contact->normal[0] = -vDir1[0];
contact->normal[1] = -vDir1[1];
contact->normal[2] = -vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
GeomCount++;
return GeomCount;
}
}
return GeomCount;
}

1125
ode/src/collision_cylinder_trimesh.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1091
ode/src/collision_kernel.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

214
ode/src/collision_kernel.h Normal file
View File

@ -0,0 +1,214 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
/*
internal data structures and functions for collision detection.
*/
#ifndef _ODE_COLLISION_KERNEL_H_
#define _ODE_COLLISION_KERNEL_H_
#include <ode/common.h>
#include <ode/contact.h>
#include <ode/collision.h>
#include "objects.h"
//****************************************************************************
// constants and macros
// mask for the number-of-contacts field in the dCollide() flags parameter
#define NUMC_MASK (0xffff)
#define IS_SPACE(geom) \
((geom)->type >= dFirstSpaceClass && (geom)->type <= dLastSpaceClass)
//****************************************************************************
// geometry object base class
// geom flags.
//
// GEOM_DIRTY means that the space data structures for this geom are
// potentially not up to date. NOTE THAT all space parents of a dirty geom
// are themselves dirty. this is an invariant that must be enforced.
//
// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date.
// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might
// recalculate its own AABB but does not know how to update the space data
// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY.
// the valid combinations are:
// 0
// GEOM_DIRTY
// GEOM_DIRTY|GEOM_AABB_BAD
// GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD
enum {
GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown
GEOM_POSR_BAD = 2, // geom's final posr is not valid
GEOM_AABB_BAD = 4, // geom's AABB is not valid
GEOM_PLACEABLE = 8, // geom is placeable
GEOM_ENABLED = 16, // geom is enabled
// Ray specific
RAY_FIRSTCONTACT = 0x10000,
RAY_BACKFACECULL = 0x20000,
RAY_CLOSEST_HIT = 0x40000
};
// geometry object base class. pos and R will either point to a separately
// allocated buffer (if body is 0 - pos points to the dxPosR object) or to
// the pos and R of the body (if body nonzero).
// a dGeomID is a pointer to this object.
struct dxGeom : public dBase {
int type; // geom type number, set by subclass constructor
int gflags; // flags used by geom and space
void *data; // user-defined data pointer
dBodyID body; // dynamics body associated with this object (if any)
dxGeom *body_next; // next geom in body's linked list of associated geoms
dxPosR *final_posr; // final position of the geom in world coordinates
dxPosR *offset_posr; // offset from body in local coordinates
// information used by spaces
dxGeom *next; // next geom in linked list of geoms
dxGeom **tome; // linked list backpointer
dxSpace *parent_space;// the space this geom is contained in, 0 if none
dReal aabb[6]; // cached AABB for this space
unsigned long category_bits,collide_bits;
dxGeom (dSpaceID _space, int is_placeable);
virtual ~dxGeom();
// calculate our new final position from our offset and body
void computePosr();
// recalculate our new final position if needed
void recomputePosr()
{
if (gflags & GEOM_POSR_BAD) {
computePosr();
gflags &= ~GEOM_POSR_BAD;
}
}
virtual void computeAABB()=0;
// compute the AABB for this object and put it in aabb. this function
// always performs a fresh computation, it does not inspect the
// GEOM_AABB_BAD flag.
virtual int AABBTest (dxGeom *o, dReal aabb[6]);
// test whether the given AABB object intersects with this object, return
// 1=yes, 0=no. this is used as an early-exit test in the space collision
// functions. the default implementation returns 1, which is the correct
// behavior if no more detailed implementation can be provided.
// utility functions
// compute the AABB only if it is not current. this function manipulates
// the GEOM_AABB_BAD flag.
void recomputeAABB() {
if (gflags & GEOM_AABB_BAD) {
// our aabb functions assume final_posr is up to date
recomputePosr();
computeAABB();
gflags &= ~GEOM_AABB_BAD;
}
}
// add and remove this geom from a linked list maintained by a space.
void spaceAdd (dxGeom **first_ptr) {
next = *first_ptr;
tome = first_ptr;
if (*first_ptr) (*first_ptr)->tome = &next;
*first_ptr = this;
}
void spaceRemove() {
if (next) next->tome = tome;
*tome = next;
}
// add and remove this geom from a linked list maintained by a body.
void bodyAdd (dxBody *b) {
body = b;
body_next = b->geom;
b->geom = this;
}
void bodyRemove();
};
//****************************************************************************
// the base space class
//
// the contained geoms are divided into two kinds: clean and dirty.
// the clean geoms have not moved since they were put in the list,
// and their AABBs are valid. the dirty geoms have changed position, and
// their AABBs are may not be valid. the two types are distinguished by the
// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list.
struct dxSpace : public dxGeom {
int count; // number of geoms in this space
dxGeom *first; // first geom in list
int cleanup; // cleanup mode, 1=destroy geoms on exit
// cached state for getGeom()
int current_index; // only valid if current_geom != 0
dxGeom *current_geom; // if 0 then there is no information
// locking stuff. the space is locked when it is currently traversing its
// internal data structures, e.g. in collide() and collide2(). operations
// that modify the contents of the space are not permitted when the space
// is locked.
int lock_count;
dxSpace (dSpaceID _space);
~dxSpace();
void computeAABB();
void setCleanup (int mode);
int getCleanup();
int query (dxGeom *geom);
int getNumGeoms();
virtual dxGeom *getGeom (int i);
virtual void add (dxGeom *);
virtual void remove (dxGeom *);
virtual void dirty (dxGeom *);
virtual void cleanGeoms()=0;
// turn all dirty geoms into clean geoms by computing their AABBs and any
// other space data structures that are required. this should clear the
// GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms.
virtual void collide (void *data, dNearCallback *callback)=0;
virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0;
};
#endif

584
ode/src/collision_quadtreespace.cpp Normal file
View File

@ -0,0 +1,584 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// QuadTreeSpace by Erwin de Vries.
#include <ode/common.h>
#include <ode/matrix.h>
#include <ode/collision_space.h>
#include <ode/collision.h>
#include "collision_kernel.h"
#include "collision_space_internal.h"
#define AXIS0 0
#define AXIS1 1
#define UP 2
//#define DRAWBLOCKS
const int SPLITAXIS = 2;
const int SPLITS = SPLITAXIS * SPLITAXIS;
#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED
class Block{
public:
dReal MinX, MaxX;
dReal MinZ, MaxZ;
dGeomID First;
int GeomCount;
Block* Parent;
Block* Children;
void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks);
void Collide(void* UserData, dNearCallback* Callback);
void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback);
void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback);
void AddObject(dGeomID Object);
void DelObject(dGeomID Object);
void Traverse(dGeomID Object);
bool Inside(const dReal* AABB);
Block* GetBlock(const dReal* AABB);
Block* GetBlockChild(const dReal* AABB);
};
#ifdef DRAWBLOCKS
#include "..\..\Include\drawstuff\\drawstuff.h"
static void DrawBlock(Block* Block){
dVector3 v[8];
v[0][AXIS0] = Block->MinX;
v[0][UP] = REAL(-1.0);
v[0][AXIS1] = Block->MinZ;
v[1][AXIS0] = Block->MinX;
v[1][UP] = REAL(-1.0);
v[1][AXIS1] = Block->MaxZ;
v[2][AXIS0] = Block->MaxX;
v[2][UP] = REAL(-1.0);
v[2][AXIS1] = Block->MinZ;
v[3][AXIS0] = Block->MaxX;
v[3][UP] = REAL(-1.0);
v[3][AXIS1] = Block->MaxZ;
v[4][AXIS0] = Block->MinX;
v[4][UP] = REAL(1.0);
v[4][AXIS1] = Block->MinZ;
v[5][AXIS0] = Block->MinX;
v[5][UP] = REAL(1.0);
v[5][AXIS1] = Block->MaxZ;
v[6][AXIS0] = Block->MaxX;
v[6][UP] = REAL(1.0);
v[6][AXIS1] = Block->MinZ;
v[7][AXIS0] = Block->MaxX;
v[7][UP] = REAL(1.0);
v[7][AXIS1] = Block->MaxZ;
// Bottom
dsDrawLine(v[0], v[1]);
dsDrawLine(v[1], v[3]);
dsDrawLine(v[3], v[2]);
dsDrawLine(v[2], v[0]);
// Top
dsDrawLine(v[4], v[5]);
dsDrawLine(v[5], v[7]);
dsDrawLine(v[7], v[6]);
dsDrawLine(v[6], v[4]);
// Sides
dsDrawLine(v[0], v[4]);
dsDrawLine(v[1], v[5]);
dsDrawLine(v[2], v[6]);
dsDrawLine(v[3], v[7]);
}
#endif //DRAWBLOCKS
void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){
GeomCount = 0;
First = 0;
MinX = Center[AXIS0] - Extents[AXIS0];
MaxX = Center[AXIS0] + Extents[AXIS0];
MinZ = Center[AXIS1] - Extents[AXIS1];
MaxZ = Center[AXIS1] + Extents[AXIS1];
this->Parent = Parent;
if (Depth > 0){
Children = Blocks;
Blocks += SPLITS;
dVector3 ChildExtents;
ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS;
ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS;
ChildExtents[UP] = Extents[UP];
for (int i = 0; i < SPLITAXIS; i++){
for (int j = 0; j < SPLITAXIS; j++){
int Index = i * SPLITAXIS + j;
dVector3 ChildCenter;
ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2);
ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2);
ChildCenter[UP] = Center[UP];
Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks);
}
}
}
else Children = 0;
}
void Block::Collide(void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
DrawBlock(this);
#endif
// Collide the local list
dxGeom* g = First;
while (g){
if (GEOM_ENABLED(g)){
Collide(g, g->next, UserData, Callback);
}
g = g->next;
}
// Recurse for children
if (Children){
for (int i = 0; i < SPLITS; i++){
if (Children[i].GeomCount <= 1){ // Early out
continue;
}
Children[i].Collide(UserData, Callback);
}
}
}
void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
DrawBlock(this);
#endif
// Collide against local list
while (g2){
if (GEOM_ENABLED(g2)){
collideAABBs (g1, g2, UserData, Callback);
}
g2 = g2->next;
}
// Collide against children
if (Children){
for (int i = 0; i < SPLITS; i++){
// Early out for empty blocks
if (Children[i].GeomCount == 0){
continue;
}
// Does the geom's AABB collide with the block?
// Dont do AABB tests for single geom blocks.
if (Children[i].GeomCount == 1 && Children[i].First){
//
}
else if (true){
if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||
g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||
g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||
g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;
}
Children[i].Collide(g1, Children[i].First, UserData, Callback);
}
}
}
void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){
// Collide against local list
dxGeom* g2 = First;
while (g2){
if (GEOM_ENABLED(g2)){
collideAABBs (g1, g2, UserData, Callback);
}
g2 = g2->next;
}
}
void Block::AddObject(dGeomID Object){
// Add the geom
Object->next = First;
First = Object;
Object->tome = (dxGeom**)this;
// Now traverse upwards to tell that we have a geom
Block* Block = this;
do{
Block->GeomCount++;
Block = Block->Parent;
}
while (Block);
}
void Block::DelObject(dGeomID Object){
// Del the geom
dxGeom* g = First;
dxGeom* Last = 0;
while (g){
if (g == Object){
if (Last){
Last->next = g->next;
}
else First = g->next;
break;
}
Last = g;
g = g->next;
}
Object->tome = 0;
// Now traverse upwards to tell that we have lost a geom
Block* Block = this;
do{
Block->GeomCount--;
Block = Block->Parent;
}
while (Block);
}
void Block::Traverse(dGeomID Object){
Block* NewBlock = GetBlock(Object->aabb);
if (NewBlock != this){
// Remove the geom from the old block and add it to the new block.
// This could be more optimal, but the loss should be very small.
DelObject(Object);
NewBlock->AddObject(Object);
}
}
bool Block::Inside(const dReal* AABB){
return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ;
}
Block* Block::GetBlock(const dReal* AABB){
if (Inside(AABB)){
return GetBlockChild(AABB); // Child or this will have a good block
}
else if (Parent){
return Parent->GetBlock(AABB); // Parent has a good block
}
else return this; // We are at the root, so we have little choice
}
Block* Block::GetBlockChild(const dReal* AABB){
if (Children){
for (int i = 0; i < SPLITS; i++){
if (Children[i].Inside(AABB)){
return Children[i].GetBlockChild(AABB); // Child will have good block
}
}
}
return this; // This is the best block
}
//****************************************************************************
// quadtree space
struct dxQuadTreeSpace : public dxSpace{
Block* Blocks; // Blocks[0] is the root
dArray<dxGeom*> DirtyList;
dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth);
~dxQuadTreeSpace();
dxGeom* getGeom(int i);
void add(dxGeom* g);
void remove(dxGeom* g);
void dirty(dxGeom* g);
void computeAABB();
void cleanGeoms();
void collide(void* UserData, dNearCallback* Callback);
void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
// Temp data
Block* CurrentBlock; // Only used while enumerating
int* CurrentChild; // Only used while enumerating
int CurrentLevel; // Only used while enumerating
dxGeom* CurrentObject; // Only used while enumerating
int CurrentIndex;
};
dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){
type = dQuadTreeSpaceClass;
int BlockCount = 0;
for (int i = 0; i <= Depth; i++){
BlockCount += (int)pow((dReal)SPLITS, i);
}
Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
Block* Blocks = this->Blocks + 1; // This pointer gets modified!
this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);
CurrentBlock = 0;
CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
CurrentLevel = 0;
CurrentObject = 0;
CurrentIndex = -1;
// Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
aabb[0] = -dInfinity;
aabb[1] = dInfinity;
aabb[2] = -dInfinity;
aabb[3] = dInfinity;
aabb[4] = -dInfinity;
aabb[5] = dInfinity;
}
dxQuadTreeSpace::~dxQuadTreeSpace(){
int Depth = 0;
Block* Current = &Blocks[0];
while (Current){
Depth++;
Current = Current->Children;
}
int BlockCount = 0;
for (int i = 0; i < Depth; i++){
BlockCount += (int)pow((dReal)SPLITS, i);
}
dFree(Blocks, BlockCount * sizeof(Block));
dFree(CurrentChild, (Depth + 1) * sizeof(int));
}
dxGeom* dxQuadTreeSpace::getGeom(int Index){
dUASSERT(Index >= 0 && Index < count, "index out of range");
//@@@
dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
return 0;
// This doesnt work
/*if (CurrentIndex == Index){
// Loop through all objects in the local list
CHILDRECURSE:
if (CurrentObject){
dGeomID g = CurrentObject;
CurrentObject = CurrentObject->next;
CurrentIndex++;
#ifdef DRAWBLOCKS
DrawBlock(CurrentBlock);
#endif //DRAWBLOCKS
return g;
}
else{
// Now lets loop through our children. Starting at index 0.
if (CurrentBlock->Children){
CurrentChild[CurrentLevel] = 0;
PARENTRECURSE:
for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
if (CurrentBlock->Children[i].GeomCount == 0){
continue;
}
CurrentBlock = &CurrentBlock->Children[i];
CurrentObject = CurrentBlock->First;
i++;
CurrentLevel++;
goto CHILDRECURSE;
}
}
}
// Now lets go back to the parent so it can continue processing its other children.
if (CurrentBlock->Parent){
CurrentBlock = CurrentBlock->Parent;
CurrentLevel--;
goto PARENTRECURSE;
}
}
else{
CurrentBlock = &Blocks[0];
CurrentLevel = 0;
CurrentObject = CurrentObject;
CurrentIndex = 0;
// Other states are already set
CurrentObject = CurrentBlock->First;
}
if (current_geom && current_index == Index - 1){
//current_geom = current_geom->next; // next
current_index = Index;
return current_geom;
}
else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow
getGeom(i);
}*/
return 0;
}
void dxQuadTreeSpace::add(dxGeom* g){
CHECK_NOT_LOCKED (this);
dAASSERT(g);
dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");
g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
DirtyList.push(g);
// add
g->parent_space = this;
Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block
count++;
// enumerator has been invalidated
current_geom = 0;
dGeomMoved(this);
}
void dxQuadTreeSpace::remove(dxGeom* g){
CHECK_NOT_LOCKED(this);
dAASSERT(g);
dUASSERT(g->parent_space == this,"object is not in this space");
// remove
((Block*)g->tome)->DelObject(g);
count--;
for (int i = 0; i < DirtyList.size(); i++){
if (DirtyList[i] == g){
DirtyList.remove(i);
// (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i
--i;
}
}
// safeguard
g->next = 0;
g->tome = 0;
g->parent_space = 0;
// enumerator has been invalidated
current_geom = 0;
// the bounding box of this space (and that of all the parents) may have
// changed as a consequence of the removal.
dGeomMoved(this);
}
void dxQuadTreeSpace::dirty(dxGeom* g){
DirtyList.push(g);
}
void dxQuadTreeSpace::computeAABB(){
//
}
void dxQuadTreeSpace::cleanGeoms(){
// compute the AABBs of all dirty geoms, and clear the dirty flags
lock_count++;
for (int i = 0; i < DirtyList.size(); i++){
dxGeom* g = DirtyList[i];
if (IS_SPACE(g)){
((dxSpace*)g)->cleanGeoms();
}
g->recomputeAABB();
g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
((Block*)g->tome)->Traverse(g);
}
DirtyList.setSize(0);
lock_count--;
}
void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
dAASSERT(Callback);
lock_count++;
cleanGeoms();
Blocks[0].Collide(UserData, Callback);
lock_count--;
}
void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){
dAASSERT(g1 && Callback);
lock_count++;
cleanGeoms();
g1->recomputeAABB();
if (g1->parent_space == this){
// The block the geom is in
Block* CurrentBlock = (Block*)g1->tome;
// Collide against block and its children
CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback);
// Collide against parents
while (true){
CurrentBlock = CurrentBlock->Parent;
if (!CurrentBlock){
break;
}
CurrentBlock->CollideLocal(g1, UserData, Callback);
}
}
else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback);
lock_count--;
}
dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){
return new dxQuadTreeSpace(space, Center, Extents, Depth);
}

790
ode/src/collision_space.cpp Normal file
View File

@ -0,0 +1,790 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
/*
spaces
*/
#include <ode/common.h>
#include <ode/matrix.h>
#include <ode/collision_space.h>
#include <ode/collision.h>
#include "collision_kernel.h"
#include "collision_space_internal.h"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
// and moving it to the front of the space's list. all the parents of a
// dirty geom also become dirty.
void dGeomMoved (dxGeom *geom)
{
dAASSERT (geom);
// if geom is offset, mark it as needing a calculate
if (geom->offset_posr) {
geom->gflags |= GEOM_POSR_BAD;
}
// from the bottom of the space heirarchy up, process all clean geoms
// turning them into dirty geoms.
dxSpace *parent = geom->parent_space;
while (parent && (geom->gflags & GEOM_DIRTY)==0) {
CHECK_NOT_LOCKED (parent);
geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
parent->dirty (geom);
geom = parent;
parent = parent->parent_space;
}
// all the remaining dirty geoms must have their AABB_BAD flags set, to
// ensure that their AABBs get recomputed
while (geom) {
geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
CHECK_NOT_LOCKED (geom->parent_space);
geom = geom->parent_space;
}
}
#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED)
//****************************************************************************
// dxSpace
dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)
{
count = 0;
first = 0;
cleanup = 1;
current_index = 0;
current_geom = 0;
lock_count = 0;
}
dxSpace::~dxSpace()
{
CHECK_NOT_LOCKED (this);
if (cleanup) {
// note that destroying each geom will call remove()
dxGeom *g,*n;
for (g = first; g; g=n) {
n = g->next;
dGeomDestroy (g);
}
}
else {
dxGeom *g,*n;
for (g = first; g; g=n) {
n = g->next;
remove (g);
}
}
}
void dxSpace::computeAABB()
{
if (first) {
int i;
dReal a[6];
a[0] = dInfinity;
a[1] = -dInfinity;
a[2] = dInfinity;
a[3] = -dInfinity;
a[4] = dInfinity;
a[5] = -dInfinity;
for (dxGeom *g=first; g; g=g->next) {
g->recomputeAABB();
for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];
for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];
}
memcpy(aabb,a,6*sizeof(dReal));
}
else {
dSetZero (aabb,6);
}
}
void dxSpace::setCleanup (int mode)
{
cleanup = (mode != 0);
}
int dxSpace::getCleanup()
{
return cleanup;
}
int dxSpace::query (dxGeom *geom)
{
dAASSERT (geom);
return (geom->parent_space == this);
}
int dxSpace::getNumGeoms()
{
return count;
}
// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1
dxGeom *dxSpace::getGeom (int i)
{
dUASSERT (i >= 0 && i < count,"index out of range");
if (current_geom && current_index == i-1) {
current_geom = current_geom->next;
current_index = i;
return current_geom;
}
else {
dxGeom *g=first;
for (int j=0; j<i; j++) {
if (g) g = g->next; else return 0;
}
current_geom = g;
current_index = i;
return g;
}
}
void dxSpace::add (dxGeom *geom)
{
CHECK_NOT_LOCKED (this);
dAASSERT (geom);
dUASSERT (geom->parent_space == 0 && geom->next == 0,
"geom is already in a space");
// add
geom->parent_space = this;
geom->spaceAdd (&first);
count++;
// enumerator has been invalidated
current_geom = 0;
// new geoms are added to the front of the list and are always
// considered to be dirty. as a consequence, this space and all its
// parents are dirty too.
geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
dGeomMoved (this);
}
void dxSpace::remove (dxGeom *geom)
{
CHECK_NOT_LOCKED (this);
dAASSERT (geom);
dUASSERT (geom->parent_space == this,"object is not in this space");
// remove
geom->spaceRemove();
count--;
// safeguard
geom->next = 0;
geom->tome = 0;
geom->parent_space = 0;
// enumerator has been invalidated
current_geom = 0;
// the bounding box of this space (and that of all the parents) may have
// changed as a consequence of the removal.
dGeomMoved (this);
}
void dxSpace::dirty (dxGeom *geom)
{
geom->spaceRemove();
geom->spaceAdd (&first);
}
//****************************************************************************
// simple space - reports all n^2 object intersections
struct dxSimpleSpace : public dxSpace {
dxSimpleSpace (dSpaceID _space);
void cleanGeoms();
void collide (void *data, dNearCallback *callback);
void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
};
dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)
{
type = dSimpleSpaceClass;
}
void dxSimpleSpace::cleanGeoms()
{
// compute the AABBs of all dirty geoms, and clear the dirty flags
lock_count++;
for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
if (IS_SPACE(g)) {
((dxSpace*)g)->cleanGeoms();
}
g->recomputeAABB();
g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
}
lock_count--;
}
void dxSimpleSpace::collide (void *data, dNearCallback *callback)
{
dAASSERT (callback);
lock_count++;
cleanGeoms();
// intersect all bounding boxes
for (dxGeom *g1=first; g1; g1=g1->next) {
if (GEOM_ENABLED(g1)){
for (dxGeom *g2=g1->next; g2; g2=g2->next) {
if (GEOM_ENABLED(g2)){
collideAABBs (g1,g2,data,callback);
}
}
}
}
lock_count--;
}
void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
dNearCallback *callback)
{
dAASSERT (geom && callback);
lock_count++;
cleanGeoms();
geom->recomputeAABB();
// intersect bounding boxes
for (dxGeom *g=first; g; g=g->next) {
if (GEOM_ENABLED(g)){
collideAABBs (g,geom,data,callback);
}
}
lock_count--;
}
//****************************************************************************
// utility stuff for hash table space
// kind of silly, but oh well...
#ifndef MAXINT
#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))
#endif
// prime[i] is the largest prime smaller than 2^i
#define NUM_PRIMES 31
static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
67108859L,134217689L,268435399L,536870909L,1073741789L};
// an axis aligned bounding box in the hash table
struct dxAABB {
dxAABB *next; // next in the list of all AABBs
int level; // the level this is stored in (cell size = 2^level)
int dbounds[6]; // AABB bounds, discretized to cell size
dxGeom *geom; // corresponding geometry object (AABB stored here)
int index; // index of this AABB, starting from 0
};
// a hash table node that represents an AABB that intersects a particular cell
// at a particular level
struct Node {
Node *next; // next node in hash table collision list, 0 if none
int x,y,z; // cell position in space, discretized to cell size
dxAABB *aabb; // axis aligned bounding box that intersects this cell
};
// return the `level' of an AABB. the AABB will be put into cells at this
// level - the cell size will be 2^level. the level is chosen to be the
// smallest value such that the AABB occupies no more than 8 cells, regardless
// of its placement. this means that:
// size/2 < q <= size
// where q is the maximum AABB dimension.
static int findLevel (dReal bounds[6])
{
if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
return MAXINT;
}
// compute q
dReal q,q2;
q = bounds[1] - bounds[0]; // x bounds
q2 = bounds[3] - bounds[2]; // y bounds
if (q2 > q) q = q2;
q2 = bounds[5] - bounds[4]; // z bounds
if (q2 > q) q = q2;
// find level such that 0.5 * 2^level < q <= 2^level
int level;
frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp)
return level;
}
// find a virtual memory address for a cell at the given level and x,y,z
// position.
// @@@ currently this is not very sophisticated, e.g. the scaling
// factors could be better designed to avoid collisions, and they should
// probably depend on the hash table physical size.
static unsigned long getVirtualAddress (int level, int x, int y, int z)
{
return level*1000 + x*100 + y*10 + z;
}
//****************************************************************************
// hash space
struct dxHashSpace : public dxSpace {
int global_minlevel; // smallest hash table level to put AABBs in
int global_maxlevel; // objects that need a level larger than this will be
// put in a "big objects" list instead of a hash table
dxHashSpace (dSpaceID _space);
void setLevels (int minlevel, int maxlevel);
void getLevels (int *minlevel, int *maxlevel);
void cleanGeoms();
void collide (void *data, dNearCallback *callback);
void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
};
dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space)
{
type = dHashSpaceClass;
global_minlevel = -3;
global_maxlevel = 10;
}
void dxHashSpace::setLevels (int minlevel, int maxlevel)
{
dAASSERT (minlevel <= maxlevel);
global_minlevel = minlevel;
global_maxlevel = maxlevel;
}
void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
{
if (minlevel) *minlevel = global_minlevel;
if (maxlevel) *maxlevel = global_maxlevel;
}
void dxHashSpace::cleanGeoms()
{
// compute the AABBs of all dirty geoms, and clear the dirty flags
lock_count++;
for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
if (IS_SPACE(g)) {
((dxSpace*)g)->cleanGeoms();
}
g->recomputeAABB();
g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
}
lock_count--;
}
void dxHashSpace::collide (void *data, dNearCallback *callback)
{
dAASSERT(this && callback);
dxGeom *geom;
dxAABB *aabb;
int i,maxlevel;
// 0 or 1 geoms can't collide with anything
if (count < 2) return;
lock_count++;
cleanGeoms();
// create a list of auxiliary information for all geom axis aligned bounding
// boxes. set the level for all AABBs. put AABBs larger than the space's
// global_maxlevel in the big_boxes list, check everything else against
// that list at the end. for AABBs that are not too big, record the maximum
// level that we need.
int n = 0; // number of AABBs in main list
dxAABB *first_aabb = 0; // list of AABBs in hash table
dxAABB *big_boxes = 0; // list of AABBs too big for hash table
maxlevel = global_minlevel - 1;
for (geom = first; geom; geom=geom->next) {
if (!GEOM_ENABLED(geom)){
continue;
}
dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB));
aabb->geom = geom;
// compute level, but prevent cells from getting too small
int level = findLevel (geom->aabb);
if (level < global_minlevel) level = global_minlevel;
if (level <= global_maxlevel) {
// aabb goes in main list
aabb->next = first_aabb;
first_aabb = aabb;
aabb->level = level;
if (level > maxlevel) maxlevel = level;
// cellsize = 2^level
dReal cellsize = (dReal) ldexp (1.0,level);
// discretize AABB position to cell size
for (i=0; i < 6; i++) aabb->dbounds[i] = (int)
floor (geom->aabb[i]/cellsize);
// set AABB index
aabb->index = n;
n++;
}
else {
// aabb is too big, put it in the big_boxes list. we don't care about
// setting level, dbounds, index, or the maxlevel
aabb->next = big_boxes;
big_boxes = aabb;
}
}
// for `n' objects, an n*n array of bits is used to record if those objects
// have been intersection-tested against each other yet. this array can
// grow large with high n, but oh well...
int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits
unsigned char *tested = (unsigned char *) alloca (n * tested_rowsize);
memset (tested,0,n * tested_rowsize);
// create a hash table to store all AABBs. each AABB may take up to 8 cells.
// we use chaining to resolve collisions, but we use a relatively large table
// to reduce the chance of collisions.
// compute hash table size sz to be a prime > 8*n
for (i=0; i<NUM_PRIMES; i++) {
if (prime[i] >= (8*n)) break;
}
if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless
int sz = prime[i];
// allocate and initialize hash table node pointers
Node **table = (Node **) ALLOCA (sizeof(Node*) * sz);
for (i=0; i<sz; i++) table[i] = 0;
// add each AABB to the hash table (may need to add it to up to 8 cells)
for (aabb=first_aabb; aabb; aabb=aabb->next) {
int *dbounds = aabb->dbounds;
for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) {
for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) {
for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) {
// get the hash index
unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz;
// add a new node to the hash table
Node *node = (Node*) alloca (sizeof (Node));
node->x = xi;
node->y = yi;
node->z = zi;
node->aabb = aabb;
node->next = table[hi];
table[hi] = node;
}
}
}
}
// now that all AABBs are loaded into the hash table, we do the actual
// collision detection. for all AABBs, check for other AABBs in the
// same cells for collisions, and then check for other AABBs in all
// intersecting higher level cells.
int db[6]; // discrete bounds at current level
for (aabb=first_aabb; aabb; aabb=aabb->next) {
// we are searching for collisions with aabb
for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
for (int level = aabb->level; level <= maxlevel; level++) {
for (int xi = db[0]; xi <= db[1]; xi++) {
for (int yi = db[2]; yi <= db[3]; yi++) {
for (int zi = db[4]; zi <= db[5]; zi++) {
// get the hash index
unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
// search all nodes at this index
Node *node;
for (node = table[hi]; node; node=node->next) {
// node points to an AABB that may intersect aabb
if (node->aabb == aabb) continue;
if (node->aabb->level == level &&
node->x == xi && node->y == yi && node->z == zi) {
// see if aabb and node->aabb have already been tested
// against each other
unsigned char mask;
if (aabb->index <= node->aabb->index) {
i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
mask = 1 << (node->aabb->index & 7);
}
else {
i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
mask = 1 << (aabb->index & 7);
}
dIASSERT (i >= 0 && i < (tested_rowsize*n));
if ((tested[i] & mask)==0) {
collideAABBs (aabb->geom,node->aabb->geom,data,callback);
}
tested[i] |= mask;
}
}
}
}
}
// get the discrete bounds for the next level up
for (i=0; i<6; i++) db[i] >>= 1;
}
}
// every AABB in the normal list must now be intersected against every
// AABB in the big_boxes list. so let's hope there are not too many objects
// in the big_boxes list.
for (aabb=first_aabb; aabb; aabb=aabb->next) {
for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) {
collideAABBs (aabb->geom,aabb2->geom,data,callback);
}
}
// intersected all AABBs in the big_boxes list together
for (aabb=big_boxes; aabb; aabb=aabb->next) {
for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) {
collideAABBs (aabb->geom,aabb2->geom,data,callback);
}
}
lock_count--;
}
void dxHashSpace::collide2 (void *data, dxGeom *geom,
dNearCallback *callback)
{
dAASSERT (geom && callback);
// this could take advantage of the hash structure to avoid
// O(n2) complexity, but it does not yet.
lock_count++;
cleanGeoms();
geom->recomputeAABB();
// intersect bounding boxes
for (dxGeom *g=first; g; g=g->next) {
collideAABBs (g,geom,data,callback);
}
lock_count--;
}
//****************************************************************************
// space functions
dxSpace *dSimpleSpaceCreate (dxSpace *space)
{
return new dxSimpleSpace (space);
}
dxSpace *dHashSpaceCreate (dxSpace *space)
{
return new dxHashSpace (space);
}
void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
{
dAASSERT (space);
dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
dxHashSpace *hspace = (dxHashSpace*) space;
hspace->setLevels (minlevel,maxlevel);
}
void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel)
{
dAASSERT (space);
dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
dxHashSpace *hspace = (dxHashSpace*) space;
hspace->getLevels (minlevel,maxlevel);
}
void dSpaceDestroy (dxSpace *space)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
dGeomDestroy (space);
}
void dSpaceSetCleanup (dxSpace *space, int mode)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
space->setCleanup (mode);
}
int dSpaceGetCleanup (dxSpace *space)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
return space->getCleanup();
}
void dSpaceAdd (dxSpace *space, dxGeom *g)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
CHECK_NOT_LOCKED (space);
space->add (g);
}
void dSpaceRemove (dxSpace *space, dxGeom *g)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
CHECK_NOT_LOCKED (space);
space->remove (g);
}
int dSpaceQuery (dxSpace *space, dxGeom *g)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
return space->query (g);
}
void dSpaceClean (dxSpace *space){
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
space->cleanGeoms();
}
int dSpaceGetNumGeoms (dxSpace *space)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
return space->getNumGeoms();
}
dGeomID dSpaceGetGeom (dxSpace *space, int i)
{
dAASSERT (space);
dUASSERT (dGeomIsSpace(space),"argument not a space");
return space->getGeom (i);
}
void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
{
dAASSERT (space && callback);
dUASSERT (dGeomIsSpace(space),"argument not a space");
space->collide (data,callback);
}
void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,
dNearCallback *callback)
{
dAASSERT (g1 && g2 && callback);
dxSpace *s1,*s2;
// see if either geom is a space
if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;
if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;
// handle the four space/geom cases
if (s1) {
if (s2) {
// g1 and g2 are spaces.
if (s1==s2) {
// collide a space with itself --> interior collision
s1->collide (data,callback);
}
else {
// iterate through the space that has the fewest geoms, calling
// collide2 in the other space for each one.
if (s1->count < s2->count) {
for (dxGeom *g = s1->first; g; g=g->next) {
s2->collide2 (data,g,callback);
}
}
else {
for (dxGeom *g = s2->first; g; g=g->next) {
s1->collide2 (data,g,callback);
}
}
}
}
else {
// g1 is a space, g2 is a geom
s1->collide2 (data,g2,callback);
}
}
else {
if (s2) {
// g1 is a geom, g2 is a space
s2->collide2 (data,g1,callback);
}
else {
// g1 and g2 are geoms, call the callback directly
callback (data,g1,g2);
}
}
}

84
ode/src/collision_space_internal.h Normal file
View File

@ -0,0 +1,84 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
/*
stuff common to all spaces
*/
#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_
#define _ODE_COLLISION_SPACE_INTERNAL_H_
#define ALLOCA(x) dALLOCA16(x)
#define CHECK_NOT_LOCKED(space) \
dUASSERT ((space)==0 || (space)->lock_count==0, \
"invalid operation for locked space");
// collide two geoms together. for the hash table space, this is
// called if the two AABBs inhabit the same hash table cells.
// this only calls the callback function if the AABBs actually
// intersect. if a geom has an AABB test function, that is called to
// provide a further refinement of the intersection.
//
// NOTE: this assumes that the geom AABBs are valid on entry
// and that both geoms are enabled.
static void collideAABBs (dxGeom *g1, dxGeom *g2,
void *data, dNearCallback *callback)
{
dIASSERT((g1->gflags & GEOM_AABB_BAD)==0);
dIASSERT((g2->gflags & GEOM_AABB_BAD)==0);
// no contacts if both geoms on the same body, and the body is not 0
if (g1->body == g2->body && g1->body) return;
// test if the category and collide bitfields match
if ( ((g1->category_bits & g2->collide_bits) ||
(g2->category_bits & g1->collide_bits)) == 0) {
return;
}
// if the bounding boxes are disjoint then don't do anything
dReal *bounds1 = g1->aabb;
dReal *bounds2 = g2->aabb;
if (bounds1[0] > bounds2[1] ||
bounds1[1] < bounds2[0] ||
bounds1[2] > bounds2[3] ||
bounds1[3] < bounds2[2] ||
bounds1[4] > bounds2[5] ||
bounds1[5] < bounds2[4]) {
return;
}
// check if either object is able to prove that it doesn't intersect the
// AABB of the other
if (g1->AABBTest (g2,bounds2) == 0) return;
if (g2->AABBTest (g1,bounds1) == 0) return;
// the objects might actually intersect - call the space callback function
callback (data,g1,g2);
}
#endif

172
ode/src/collision_std.h Normal file
View File

@ -0,0 +1,172 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
the standard ODE geometry primitives.
*/
#ifndef _ODE_COLLISION_STD_H_
#define _ODE_COLLISION_STD_H_
#include <set>
#include <ode/common.h>
#include "collision_kernel.h"
// primitive collision functions - these have the dColliderFn interface, i.e.
// the same interface as dCollide(). the first and second geom arguments must
// have the specified types.
int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip);
int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip);
int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip);
int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideRayCylinder (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
// Cylinder - Box/Sphere by (C) CroTeam
// Ported by Nguyen Binh
int dCollideCylinderBox(dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip);
int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere,
int flags, dContactGeom *contact, int skip);
int dCollideCylinderPlane(dxGeom *gCylinder, dxGeom *gPlane,
int flags, dContactGeom *contact, int skip);
//--> Convex Collision
int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip);
int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
//<-- Convex Collision
// dHeightfield
int dCollideHeightfield( dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip );
//****************************************************************************
// the basic geometry objects
struct dxSphere : public dxGeom {
dReal radius; // sphere radius
dxSphere (dSpaceID space, dReal _radius);
void computeAABB();
};
struct dxBox : public dxGeom {
dVector3 side; // side lengths (x,y,z)
dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
void computeAABB();
};
struct dxCapsule : public dxGeom {
dReal radius,lz; // radius, length along z axis
dxCapsule (dSpaceID space, dReal _radius, dReal _length);
void computeAABB();
};
struct dxCylinder : public dxGeom {
dReal radius,lz; // radius, length along z axis
dxCylinder (dSpaceID space, dReal _radius, dReal _length);
void computeAABB();
};
struct dxPlane : public dxGeom {
dReal p[4];
dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
void computeAABB();
};
struct dxRay : public dxGeom {
dReal length;
dxRay (dSpaceID space, dReal _length);
void computeAABB();
};
typedef std::pair<unsigned int,unsigned int> edge; /*!< Used to descrive a convex hull edge, an edge is a pair or indices into the hull's points */
struct dxConvex : public dxGeom
{
dReal *planes; /*!< An array of planes in the form:
normal X, normal Y, normal Z,Distance
*/
dReal *points; /*!< An array of points X,Y,Z */
unsigned int *polygons; /*! An array of indices to the points of each polygon, it should be the number of vertices followed by that amount of indices to "points" in counter clockwise order*/
unsigned int planecount; /*!< Amount of planes in planes */
unsigned int pointcount;/*!< Amount of points in points */
dReal saabb[6];/*!< Static AABB */
std::set<edge> edges;
dxConvex(dSpaceID space,
dReal *planes,
unsigned int planecount,
dReal *points,
unsigned int pointcount,
unsigned int *polygons);
~dxConvex()
{
//fprintf(stdout,"dxConvex Destroy\n");
}
void computeAABB();
private:
// For Internal Use Only
void FillEdges();
};
#endif

232
ode/src/collision_transform.cpp Normal file
View File

@ -0,0 +1,232 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
geom transform
*/
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_transform.h"
#include "collision_util.h"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// dxGeomTransform class
struct dxGeomTransform : public dxGeom {
dxGeom *obj; // object that is being transformed
int cleanup; // 1 to destroy obj when destroyed
int infomode; // 1 to put Tx geom in dContactGeom g1
// cached final object transform (body tx + relative tx). this is set by
// computeAABB(), and it is valid while the AABB is valid.
dxPosR transform_posr;
dxGeomTransform (dSpaceID space);
~dxGeomTransform();
void computeAABB();
void computeFinalTx();
};
/*
void RunMe()
{
printf("sizeof body = %i\n", sizeof(dxBody));
printf("sizeof geom = %i\n", sizeof(dxGeom));
printf("sizeof geomtransform = %i\n", sizeof(dxGeomTransform));
printf("sizeof posr = %i\n", sizeof(dxPosR));
}
*/
dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1)
{
type = dGeomTransformClass;
obj = 0;
cleanup = 0;
infomode = 0;
dSetZero (transform_posr.pos,4);
dRSetIdentity (transform_posr.R);
}
dxGeomTransform::~dxGeomTransform()
{
if (obj && cleanup) delete obj;
}
void dxGeomTransform::computeAABB()
{
if (!obj) {
dSetZero (aabb,6);
return;
}
// backup the relative pos and R pointers of the encapsulated geom object
dxPosR* posr_bak = obj->final_posr;
// compute temporary pos and R for the encapsulated geom object
computeFinalTx();
obj->final_posr = &transform_posr;
// compute the AABB
obj->computeAABB();
memcpy (aabb,obj->aabb,6*sizeof(dReal));
// restore the pos and R
obj->final_posr = posr_bak;
}
// utility function for dCollideTransform() : compute final pos and R
// for the encapsulated geom object
void dxGeomTransform::computeFinalTx()
{
dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos);
transform_posr.pos[0] += final_posr->pos[0];
transform_posr.pos[1] += final_posr->pos[1];
transform_posr.pos[2] += final_posr->pos[2];
dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R);
}
//****************************************************************************
// collider function:
// this collides a transformed geom with another geom. the other geom can
// also be a transformed geom, but this case is not handled specially.
int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dGeomTransformClass);
dxGeomTransform *tr = (dxGeomTransform*) o1;
if (!tr->obj) return 0;
dUASSERT (tr->obj->parent_space==0,
"GeomTransform encapsulated object must not be in a space");
dUASSERT (tr->obj->body==0,
"GeomTransform encapsulated object must not be attached "
"to a body");
// backup the relative pos and R pointers of the encapsulated geom object,
// and the body pointer
dxPosR *posr_bak = tr->obj->final_posr;
dxBody *bodybak = tr->obj->body;
// compute temporary pos and R for the encapsulated geom object.
// note that final_pos and final_R are valid if no GEOM_AABB_BAD flag,
// because computeFinalTx() will have already been called in
// dxGeomTransform::computeAABB()
if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx();
tr->obj->final_posr = &tr->transform_posr;
tr->obj->body = o1->body;
// do the collision
int n = dCollide (tr->obj,o2,flags,contact,skip);
// if required, adjust the 'g1' values in the generated contacts so that
// thay indicated the GeomTransform object instead of the encapsulated
// object.
if (tr->infomode) {
for (int i=0; i<n; i++) {
dContactGeom *c = CONTACT(contact,skip*i);
c->g1 = o1;
}
}
// restore the pos, R and body
tr->obj->final_posr = posr_bak;
tr->obj->body = bodybak;
return n;
}
//****************************************************************************
// public API
dGeomID dCreateGeomTransform (dSpaceID space)
{
return new dxGeomTransform (space);
}
void dGeomTransformSetGeom (dGeomID g, dGeomID obj)
{
dUASSERT (g && g->type == dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr = (dxGeomTransform*) g;
if (tr->obj && tr->cleanup) delete tr->obj;
tr->obj = obj;
}
dGeomID dGeomTransformGetGeom (dGeomID g)
{
dUASSERT (g && g->type == dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr = (dxGeomTransform*) g;
return tr->obj;
}
void dGeomTransformSetCleanup (dGeomID g, int mode)
{
dUASSERT (g && g->type == dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr = (dxGeomTransform*) g;
tr->cleanup = mode;
}
int dGeomTransformGetCleanup (dGeomID g)
{
dUASSERT (g && g->type == dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr = (dxGeomTransform*) g;
return tr->cleanup;
}
void dGeomTransformSetInfo (dGeomID g, int mode)
{
dUASSERT (g && g->type == dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr = (dxGeomTransform*) g;
tr->infomode = mode;
}
int dGeomTransformGetInfo (dGeomID g)
{
dUASSERT (g && g->type == dGeomTransformClass,
"argument not a geom transform");
dxGeomTransform *tr = (dxGeomTransform*) g;
return tr->infomode;
}

40
ode/src/collision_transform.h Normal file
View File

@ -0,0 +1,40 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
geom transform
*/
#ifndef _ODE_COLLISION_TRANSFORM_H_
#define _ODE_COLLISION_TRANSFORM_H_
#include <ode/common.h>
#include "collision_kernel.h"
int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip);
#endif

1423
ode/src/collision_trimesh_box.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1161
ode/src/collision_trimesh_ccylinder.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1255
ode/src/collision_trimesh_distance.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

521
ode/src/collision_trimesh_gimpact.cpp Normal file
View File

@ -0,0 +1,521 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// TriMesh code by Erwin de Vries.
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"
#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"
#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
void dxTriMeshData::Preprocess()
{
// If this mesh has already been preprocessed, exit
// if (UseFlags)
// return;
//
// udword numTris = Mesh.GetNbTriangles();
// udword numEdges = numTris * 3;
//
// UseFlags = new uint8[numTris];
// memset(UseFlags, 0, sizeof(uint8) * numTris);
//
// EdgeRecord* records = new EdgeRecord[numEdges];
//
// // Make a list of every edge in the mesh
// const IndexedTriangle* tris = Mesh.GetTris();
// for (unsigned int i = 0; i < numTris; i++)
// {
// SetupEdge(&records[i*3], 0, i, tris->mVRef);
// SetupEdge(&records[i*3+1], 1, i, tris->mVRef);
// SetupEdge(&records[i*3+2], 2, i, tris->mVRef);
//
// tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride());
// }
//
// // Sort the edges, so the ones sharing the same verts are beside each other
// qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare);
//
// // Go through the sorted list of edges and flag all the edges and vertices that we need to use
// for (unsigned int i = 0; i < numEdges; i++)
// {
// EdgeRecord* rec1 = &records[i];
// EdgeRecord* rec2 = 0;
// if (i < numEdges - 1)
// rec2 = &records[i+1];
//
// if (rec2 &&
// rec1->VertIdx1 == rec2->VertIdx1 &&
// rec1->VertIdx2 == rec2->VertIdx2)
// {
// VertexPointers vp;
// Mesh.GetTriangle(vp, rec1->TriIdx);
//
// // Get the normal of the first triangle
// Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]);
// triNorm.Normalize();
//
// // Get the vert opposite this edge in the first triangle
// Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex);
//
// // Get the vert opposite this edge in the second triangle
// Mesh.GetTriangle(vp, rec2->TriIdx);
// Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex);
//
// float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize());
//
// // We let the dot threshold for concavity get slightly negative to allow for rounding errors
// static const float kConcaveThresh = -0.000001f;
//
// // This is a concave edge, leave it for the next pass
// if (dot >= kConcaveThresh)
// rec1->Concave = true;
// // If this is a convex edge, mark its vertices and edge as used
// else
// UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
//
// // Skip the second edge
// i++;
// }
// // This is a boundary edge
// else
// {
// UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
// }
// }
//
// // Go through the list once more, and take any edge we marked as concave and
// // clear it's vertices flags in any triangles they're used in
// for (unsigned int i = 0; i < numEdges; i++)
// {
// EdgeRecord& er = records[i];
//
// if (er.Concave)
// {
// for (unsigned int j = 0; j < numEdges; j++)
// {
// EdgeRecord& curER = records[j];
//
// if (curER.VertIdx1 == er.VertIdx1 ||
// curER.VertIdx1 == er.VertIdx2)
// UseFlags[curER.TriIdx] &= ~curER.Vert1Flags;
//
// if (curER.VertIdx2 == er.VertIdx1 ||
// curER.VertIdx2 == er.VertIdx2)
// UseFlags[curER.TriIdx] &= ~curER.Vert2Flags;
// }
// }
// }
//
// delete [] records;
}
dTriMeshDataID dGeomTriMeshDataCreate(){
return new dxTriMeshData();
}
void dGeomTriMeshDataDestroy(dTriMeshDataID g){
delete g;
}
void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans )
{
// dAASSERT(g)
// dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
// for (int i=0; i<16; i++)
// (((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ];
// return;
}
//dReal* dGeomTriMeshGetLastTransform( dxGeom* g )
//{
// dAASSERT(g)
// dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
//
// return (dReal*)(((dxTriMesh*)g)->last_trans);
//}
void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
{
// dUASSERT(g, "argument not trimesh data");
//
// double *elem;
//
// switch (data_id) {
// case TRIMESH_FACE_NORMALS:
// g->Normals = (dReal *) in_data;
// break;
//
// case TRIMESH_LAST_TRANSFORMATION:
// elem = (double *) in_data;
// for (int i=0; i<16; i++)
// g->last_trans[i] = (dReal) elem[i];
//
// break;
// default:
// dUASSERT(data_id, "invalid data type");
// break;
// }
//
// return;
}
void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id)
{
dUASSERT(g, "argument not trimesh data");
// switch (data_id) {
// case TRIMESH_FACE_NORMALS:
// return NULL;
// break;
//
// case TRIMESH_LAST_TRANSFORMATION:
// return NULL;
// break;
// default:
// dUASSERT(data_id, "invalid data type");
// break;
// }
return NULL;
}
void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* Normals)
{
dUASSERT(g, "argument not trimesh data");
g->Build(Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride,
Normals,
true);
}
void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride)
{
dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride, (void*)NULL);
}
void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* Normals)
{
dUASSERT(g, "argument not trimesh data");
g->Build(Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride,
Normals,
false);
}
void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride) {
dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride, NULL);
}
void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
const dReal* Vertices, int VertexCount,
const int* Indices, int IndexCount,
const int* Normals){
#ifdef dSINGLE
dGeomTriMeshDataBuildSingle1(g,
Vertices, 4 * sizeof(dReal), VertexCount,
Indices, IndexCount, 3 * sizeof(unsigned int),
Normals);
#else
dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount,
Indices, IndexCount, 3 * sizeof(unsigned int),
Normals);
#endif
}
void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
const dReal* Vertices, int VertexCount,
const int* Indices, int IndexCount) {
dGeomTriMeshDataBuildSimple1(g,
Vertices, VertexCount, Indices, IndexCount,
(const int*)NULL);
}
void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
{
dUASSERT(g, "argument not trimesh data");
g->Preprocess();
}
void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
{
dUASSERT(g, "argument not trimesh data");
*buf = NULL;
*bufLen = 0;
}
void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
{
dUASSERT(g, "argument not trimesh data");
// g->UseFlags = buf;
}
// Trimesh
dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1){
type = dTriMeshClass;
this->Data = Data;
//Create trimesh
gim_trimesh_create_from_data(&m_collision_trimesh,( vec3f *)(&Data->m_Vertices[0]), Data->m_VertexCount ,0, ( GUINT *)(&Data->m_Indices[0]), Data->m_TriangleCount*3,0,1);
/* TC has speed/space 'issues' that don't make it a clear
win by default on spheres/boxes. */
this->doSphereTC = true;
this->doBoxTC = true;
this->doCapsuleTC = true;
}
dxTriMesh::~dxTriMesh(){
//Terminate Trimesh
gim_trimesh_destroy(&m_collision_trimesh);
}
void dxTriMesh::ClearTCCache(){
}
int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
return 1;
}
void dxTriMesh::computeAABB()
{
//update trimesh transform
mat4f transform;
IDENTIFY_MATRIX_4X4(transform);
MakeMatrix(this, transform);
gim_trimesh_set_tranform(&m_collision_trimesh,transform);
//Update trimesh boxes
gim_trimesh_update(&m_collision_trimesh);
memcpy(aabb,&m_collision_trimesh.m_aabbset.m_global_bound,6*sizeof(GREAL));
}
void dxTriMeshData::UpdateData()
{
// BVTree.Refit();
}
dGeomID dCreateTriMesh(dSpaceID space,
dTriMeshDataID Data,
dTriCallback* Callback,
dTriArrayCallback* ArrayCallback,
dTriRayCallback* RayCallback)
{
dxTriMesh* Geom = new dxTriMesh(space, Data);
Geom->Callback = Callback;
Geom->ArrayCallback = ArrayCallback;
Geom->RayCallback = RayCallback;
return Geom;
}
void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->Callback = Callback;
}
dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->Callback;
}
void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
}
dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->ArrayCallback;
}
void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->RayCallback = Callback;
}
dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->RayCallback;
}
void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->Data = Data;
}
dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->Data;
}
void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
switch (geomClass)
{
case dSphereClass:
((dxTriMesh*)g)->doSphereTC = (1 == enable);
break;
case dBoxClass:
((dxTriMesh*)g)->doBoxTC = (1 == enable);
break;
case dCapsuleClass:
// case dCCylinderClass:
((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
break;
}
}
int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
switch (geomClass)
{
case dSphereClass:
if (((dxTriMesh*)g)->doSphereTC)
return 1;
break;
case dBoxClass:
if (((dxTriMesh*)g)->doBoxTC)
return 1;
break;
case dCapsuleClass:
if (((dxTriMesh*)g)->doCapsuleTC)
return 1;
break;
}
return 0;
}
void dGeomTriMeshClearTCCache(dGeomID g){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
Geom->ClearTCCache();
}
/*
* returns the TriMeshDataID
*/
dTriMeshDataID
dGeomTriMeshGetTriMeshDataID(dGeomID g)
{
dxTriMesh* Geom = (dxTriMesh*) g;
return Geom->Data;
}
// Getting data
void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, (*v0),(*v1),(*v2));
gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
}
void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
dVector3 dv[3];
gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, dv[0],dv[1],dv[2]);
GetPointFromBarycentric(dv, u, v, Out);
gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
}
int dGeomTriMeshGetTriangleCount (dGeomID g)
{
dxTriMesh* Geom = (dxTriMesh*)g;
return gim_trimesh_get_triangle_count(&Geom->m_collision_trimesh);
}
void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
dUASSERT(g, "argument not trimesh data");
g->UpdateData();
}
#endif

489
ode/src/collision_trimesh_internal.h Normal file
View File

@ -0,0 +1,489 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// TriMesh code by Erwin de Vries.
// Modified for FreeSOLID Compatibility by Rodrigo Hernandez
#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
#define _ODE_COLLISION_TRIMESH_INTERNAL_H_
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags,
dContactGeom *contact, int skip )
{
// Swapped case, for code that needs it (heightfield initially)
// The other ray-geom colliders take geoms in a swapped order to the
// dCollideRTL function which is annoying when using function pointers.
return dCollideRTL( trimesh, ray, flags, contact, skip );
}
//****************************************************************************
// dxTriMesh class
#ifdef TRIMESH_INTERNAL
#include "collision_kernel.h"
#include <ode/collision_trimesh.h>
#if dTRIMESH_OPCODE
#define BAN_OPCODE_AUTOLINK
#include "Opcode.h"
using namespace Opcode;
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
#include <GIMPACT/gimpact.h>
#endif
struct dxTriMeshData : public dBase
{
/* Array of flags for which edges and verts should be used on each triangle */
enum UseFlags
{
kEdge0 = 0x1,
kEdge1 = 0x2,
kEdge2 = 0x4,
kVert0 = 0x8,
kVert1 = 0x10,
kVert2 = 0x20,
kUseAll = 0xFF,
};
/* Setup the UseFlags array */
void Preprocess();
/* For when app changes the vertices */
void UpdateData();
#if dTRIMESH_OPCODE
Model BVTree;
MeshInterface Mesh;
dxTriMeshData();
~dxTriMeshData();
void Build(const void* Vertices, int VertexStide, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* Normals,
bool Single);
/* aabb in model space */
dVector3 AABBCenter;
dVector3 AABBExtents;
// data for use in collision resolution
const void* Normals;
uint8* UseFlags;
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
const char* m_Vertices;
int m_VertexStride;
int m_VertexCount;
const char* m_Indices;
int m_TriangleCount;
int m_TriStride;
bool m_single;
dxTriMeshData()
{
m_Vertices=NULL;
m_VertexStride = 12;
m_VertexCount = 0;
m_Indices = 0;
m_TriangleCount = 0;
m_TriStride = 12;
m_single = true;
}
void Build(const void* Vertices, int VertexStide, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* Normals,
bool Single)
{
m_Vertices=(const char *)Vertices;
m_VertexStride = VertexStide;
m_VertexCount = VertexCount;
m_Indices = (const char *)Indices;
m_TriangleCount = IndexCount/3;
m_TriStride = TriStride;
m_single = Single;
}
inline void GetVertex(unsigned int i, dVector3 Out)
{
if(m_single)
{
const float * fverts = (const float * )(m_Vertices + m_VertexStride*i);
Out[0] = fverts[0];
Out[1] = fverts[1];
Out[2] = fverts[2];
Out[3] = 1.0f;
}
else
{
const double * dverts = (const double * )(m_Vertices + m_VertexStride*i);
Out[0] = (float)dverts[0];
Out[1] = (float)dverts[1];
Out[2] = (float)dverts[2];
Out[3] = 1.0f;
}
}
inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3])
{
const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle);
triindices[0] = ind[0];
triindices[1] = ind[1];
triindices[2] = ind[2];
}
#endif // dTRIMESH_GIMPACT
};
struct dxTriMesh : public dxGeom{
// Callbacks
dTriCallback* Callback;
dTriArrayCallback* ArrayCallback;
dTriRayCallback* RayCallback;
// Data types
dxTriMeshData* Data;
bool doSphereTC;
bool doBoxTC;
bool doCapsuleTC;
// Functions
dxTriMesh(dSpaceID Space, dTriMeshDataID Data);
~dxTriMesh();
void ClearTCCache();
int AABBTest(dxGeom* g, dReal aabb[6]);
void computeAABB();
#if dTRIMESH_OPCODE
// Instance data for last transform.
dMatrix4 last_trans;
// Colliders
static PlanesCollider _PlanesCollider;
static SphereCollider _SphereCollider;
static OBBCollider _OBBCollider;
static RayCollider _RayCollider;
static AABBTreeCollider _AABBTreeCollider;
static LSSCollider _LSSCollider;
// Some constants
static CollisionFaces Faces;
// Temporal coherence
struct SphereTC : public SphereCache{
dxGeom* Geom;
};
dArray<SphereTC> SphereTCCache;
static SphereCache defaultSphereCache;
struct BoxTC : public OBBCache{
dxGeom* Geom;
};
dArray<BoxTC> BoxTCCache;
static OBBCache defaultBoxCache;
struct CapsuleTC : public LSSCache{
dxGeom* Geom;
};
dArray<CapsuleTC> CapsuleTCCache;
static LSSCache defaultCapsuleCache;
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
GIM_TRIMESH m_collision_trimesh;
#endif // dTRIMESH_GIMPACT
};
#if 0
// Fetches a contact
inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff));
return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
}
#endif
#if dTRIMESH_OPCODE
inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
VertexPointers VP;
TriMesh->Data->Mesh.GetTriangle(VP, Index);
for (int i = 0; i < 3; i++){
Out[i][0] = VP.Vertex[i]->x;
Out[i][1] = VP.Vertex[i]->y;
Out[i][2] = VP.Vertex[i]->z;
Out[i][3] = 0;
}
}
inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
VertexPointers VP;
TriMesh->Data->Mesh.GetTriangle(VP, Index);
for (int i = 0; i < 3; i++){
dVector3 v;
v[0] = VP.Vertex[i]->x;
v[1] = VP.Vertex[i]->y;
v[2] = VP.Vertex[i]->z;
v[3] = 0;
dMULTIPLY0_331(Out[i], Rotation, v);
Out[i][0] += Position[0];
Out[i][1] += Position[1];
Out[i][2] += Position[2];
Out[i][3] = 0;
}
}
inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
Out.m[0][0] = (float) Rotation[0];
Out.m[1][0] = (float) Rotation[1];
Out.m[2][0] = (float) Rotation[2];
Out.m[0][1] = (float) Rotation[4];
Out.m[1][1] = (float) Rotation[5];
Out.m[2][1] = (float) Rotation[6];
Out.m[0][2] = (float) Rotation[8];
Out.m[1][2] = (float) Rotation[9];
Out.m[2][2] = (float) Rotation[10];
Out.m[3][0] = (float) Position[0];
Out.m[3][1] = (float) Position[1];
Out.m[3][2] = (float) Position[2];
Out.m[0][3] = 0.0f;
Out.m[1][3] = 0.0f;
Out.m[2][3] = 0.0f;
Out.m[3][3] = 1.0f;
return Out;
}
inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
return MakeMatrix(Position, Rotation, Out);
}
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
// why is this not implemented?
dAASSERT(false);
}
inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
{
m[0][0] = (float) Rotation[0];
m[0][1] = (float) Rotation[1];
m[0][2] = (float) Rotation[2];
m[1][0] = (float) Rotation[4];
m[1][1] = (float) Rotation[5];
m[1][2] = (float) Rotation[6];
m[2][0] = (float) Rotation[8];
m[2][1] = (float) Rotation[9];
m[2][2] = (float) Rotation[10];
m[0][3] = (float) Position[0];
m[1][3] = (float) Position[1];
m[2][3] = (float) Position[2];
}
inline void MakeMatrix(dxGeom* g, mat4f Out){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
MakeMatrix(Position, Rotation, Out);
}
#endif // dTRIMESH_GIMPACT
// Outputs a matrix to 3 vectors
inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
Right[0] = Matrix[0 * 4 + 0];
Right[1] = Matrix[1 * 4 + 0];
Right[2] = Matrix[2 * 4 + 0];
Right[3] = REAL(0.0);
Up[0] = Matrix[0 * 4 + 1];
Up[1] = Matrix[1 * 4 + 1];
Up[2] = Matrix[2 * 4 + 1];
Up[3] = REAL(0.0);
Direction[0] = Matrix[0 * 4 + 2];
Direction[1] = Matrix[1 * 4 + 2];
Direction[2] = Matrix[2 * 4 + 2];
Direction[3] = REAL(0.0);
}
// Outputs a matrix to 3 vectors
inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
}
// Finds barycentric
inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
dReal w = REAL(1.0) - u - v;
Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
}
// Performs a callback
inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
if (TriMesh->Callback != NULL){
return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0);
}
else return true;
}
// Some utilities
template<class T> const T& dcMAX(const T& x, const T& y){
return x > y ? x : y;
}
template<class T> const T& dcMIN(const T& x, const T& y){
return x < y ? x : y;
}
dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin,
const dVector3 triEdge1, const dVector3 triEdge2,
dReal* pfSParam = 0, dReal* pfTParam = 0 );
dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction,
const dVector3 seg2Origin, const dVector3 seg2Direction,
dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 );
dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd,
const dVector3 triOrigin,
const dVector3 triEdge1, const dVector3 triEdge2,
dReal* t = 0, dReal* u = 0, dReal* v = 0 );
inline
void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
{
result[0] = left[0] - right[0];
result[1] = left[1] - right[1];
result[2] = left[2] - right[2];
result[3] = REAL(0.0);
}
inline
void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
{
result[0] = left[0] + right[0];
result[1] = left[1] + right[1];
result[2] = left[2] + right[2];
result[3] = REAL(0.0);
}
inline
void Vector3Negate( const dVector3 in, dVector3 out )
{
out[0] = -in[0];
out[1] = -in[1];
out[2] = -in[2];
out[3] = REAL(0.0);
}
inline
void Vector3Copy( const dVector3 in, dVector3 out )
{
out[0] = in[0];
out[1] = in[1];
out[2] = in[2];
out[3] = REAL(0.0);
}
inline
void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
{
out[0] = in[0] * scalar;
out[1] = in[1] * scalar;
out[2] = in[2] * scalar;
out[3] = REAL(0.0);
}
inline
void TransformVector3( const dVector3 in,
const dMatrix3 orientation, const dVector3 position,
dVector3 out )
{
dMULTIPLY0_331( out, orientation, in );
out[0] += position[0];
out[1] += position[1];
out[2] += position[2];
}
//------------------------------------------------------------------------------
/**
@brief Check for intersection between triangle and capsule.
@param dist [out] Shortest distance squared between the triangle and
the capsule segment (central axis).
@param t [out] t value of point on segment that's the shortest distance
away from the triangle, the coordinates of this point
can be found by (cap.seg.end - cap.seg.start) * t,
or cap.seg.ipol(t).
@param u [out] Barycentric coord on triangle.
@param v [out] Barycentric coord on triangle.
@return True if intersection exists.
The third Barycentric coord is implicit, ie. w = 1.0 - u - v
The Barycentric coords give the location of the point on the triangle
closest to the capsule (where the distance between the two shapes
is the shortest).
*/
inline
bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd,
const dReal radius, const dVector3 triOrigin,
const dVector3 triEdge0, const dVector3 triEdge1,
dReal* dist, dReal* t, dReal* u, dReal* v )
{
dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1,
t, u, v );
if ( dist )
*dist = sqrDist;
return ( sqrDist <= (radius * radius) );
}
#endif //TRIMESH_INTERNAL
#endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_

829
ode/src/collision_trimesh_opcode.cpp Normal file
View File

@ -0,0 +1,829 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// TriMesh code by Erwin de Vries.
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"
#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"
#if dTRIMESH_ENABLED && dTRIMESH_OPCODE
// Trimesh data
dxTriMeshData::dxTriMeshData() : UseFlags( NULL )
{
#if !dTRIMESH_ENABLED
dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work");
#endif
}
dxTriMeshData::~dxTriMeshData()
{
if ( UseFlags )
delete [] UseFlags;
}
void
dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* in_Normals,
bool Single)
{
#if dTRIMESH_ENABLED
Mesh.SetNbTriangles(IndexCount / 3);
Mesh.SetNbVertices(VertexCount);
Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices);
Mesh.SetStrides(TriStride, VertexStide);
Mesh.Single = Single;
// Build tree
BuildSettings Settings;
// recommended in Opcode User Manual
//Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER;
// used in ODE, why?
//Settings.mRules = SPLIT_BEST_AXIS;
// best compromise?
Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
OPCODECREATE TreeBuilder;
TreeBuilder.mIMesh = &Mesh;
TreeBuilder.mSettings = Settings;
TreeBuilder.mNoLeaf = true;
TreeBuilder.mQuantized = false;
TreeBuilder.mKeepOriginal = false;
TreeBuilder.mCanRemap = false;
BVTree.Build(TreeBuilder);
// compute model space AABB
dVector3 AABBMax, AABBMin;
AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity;
AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity;
if( Single ) {
const char* verts = (const char*)Vertices;
for( int i = 0; i < VertexCount; ++i ) {
const float* v = (const float*)verts;
if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
verts += VertexStide;
}
} else {
const char* verts = (const char*)Vertices;
for( int i = 0; i < VertexCount; ++i ) {
const double* v = (const double*)verts;
if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0];
if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1];
if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2];
if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0];
if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1];
if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2];
verts += VertexStide;
}
}
AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5);
AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5);
AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5);
AABBExtents[0] = AABBMax[0] - AABBCenter[0];
AABBExtents[1] = AABBMax[1] - AABBCenter[1];
AABBExtents[2] = AABBMax[2] - AABBCenter[2];
// user data (not used by OPCODE)
Normals = (dReal *) in_Normals;
UseFlags = 0;
#endif // dTRIMESH_ENABLED
}
struct EdgeRecord
{
int VertIdx1; // Index into vertex array for this edges vertices
int VertIdx2;
int TriIdx; // Index into triangle array for triangle this edge belongs to
uint8 EdgeFlags;
uint8 Vert1Flags;
uint8 Vert2Flags;
bool Concave;
};
// Edge comparison function for qsort
static int EdgeCompare(const void* edge1, const void* edge2)
{
EdgeRecord* e1 = (EdgeRecord*)edge1;
EdgeRecord* e2 = (EdgeRecord*)edge2;
if (e1->VertIdx1 == e2->VertIdx1)
return e1->VertIdx2 - e2->VertIdx2;
else
return e1->VertIdx1 - e2->VertIdx1;
}
void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const unsigned int* vertIdxs)
{
if (edgeIdx == 0)
{
edge->EdgeFlags = dxTriMeshData::kEdge0;
edge->Vert1Flags = dxTriMeshData::kVert0;
edge->Vert2Flags = dxTriMeshData::kVert1;
edge->VertIdx1 = vertIdxs[0];
edge->VertIdx2 = vertIdxs[1];
}
else if (edgeIdx == 1)
{
edge->EdgeFlags = dxTriMeshData::kEdge1;
edge->Vert1Flags = dxTriMeshData::kVert1;
edge->Vert2Flags = dxTriMeshData::kVert2;
edge->VertIdx1 = vertIdxs[1];
edge->VertIdx2 = vertIdxs[2];
}
else if (edgeIdx == 2)
{
edge->EdgeFlags = dxTriMeshData::kEdge2;
edge->Vert1Flags = dxTriMeshData::kVert2;
edge->Vert2Flags = dxTriMeshData::kVert0;
edge->VertIdx1 = vertIdxs[2];
edge->VertIdx2 = vertIdxs[0];
}
// Make sure vert index 1 is less than index 2 (for easier sorting)
if (edge->VertIdx1 > edge->VertIdx2)
{
unsigned int tempIdx = edge->VertIdx1;
edge->VertIdx1 = edge->VertIdx2;
edge->VertIdx2 = tempIdx;
uint8 tempFlags = edge->Vert1Flags;
edge->Vert1Flags = edge->Vert2Flags;
edge->Vert2Flags = tempFlags;
}
edge->TriIdx = triIdx;
edge->Concave = false;
}
#if dTRIMESH_ENABLED
// Get the vertex opposite this edge in the triangle
inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[])
{
if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) ||
(edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0))
{
return *vertices[2];
}
else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) ||
(edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1))
{
return *vertices[0];
}
else
return *vertices[1];
}
#endif // dTRIMESH_ENABLED
void dxTriMeshData::Preprocess()
{
#if dTRIMESH_ENABLED
// If this mesh has already been preprocessed, exit
if (UseFlags)
return;
udword numTris = Mesh.GetNbTriangles();
udword numEdges = numTris * 3;
UseFlags = new uint8[numTris];
memset(UseFlags, 0, sizeof(uint8) * numTris);
EdgeRecord* records = new EdgeRecord[numEdges];
// Make a list of every edge in the mesh
const IndexedTriangle* tris = Mesh.GetTris();
for (unsigned int i = 0; i < numTris; i++)
{
SetupEdge(&records[i*3], 0, i, tris->mVRef);
SetupEdge(&records[i*3+1], 1, i, tris->mVRef);
SetupEdge(&records[i*3+2], 2, i, tris->mVRef);
tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride());
}
// Sort the edges, so the ones sharing the same verts are beside each other
qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare);
// Go through the sorted list of edges and flag all the edges and vertices that we need to use
for (unsigned int i = 0; i < numEdges; i++)
{
EdgeRecord* rec1 = &records[i];
EdgeRecord* rec2 = 0;
if (i < numEdges - 1)
rec2 = &records[i+1];
if (rec2 &&
rec1->VertIdx1 == rec2->VertIdx1 &&
rec1->VertIdx2 == rec2->VertIdx2)
{
VertexPointers vp;
Mesh.GetTriangle(vp, rec1->TriIdx);
// Get the normal of the first triangle
Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]);
triNorm.Normalize();
// Get the vert opposite this edge in the first triangle
Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex);
// Get the vert opposite this edge in the second triangle
Mesh.GetTriangle(vp, rec2->TriIdx);
Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex);
float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize());
// We let the dot threshold for concavity get slightly negative to allow for rounding errors
static const float kConcaveThresh = -0.000001f;
// This is a concave edge, leave it for the next pass
if (dot >= kConcaveThresh)
rec1->Concave = true;
// If this is a convex edge, mark its vertices and edge as used
else
UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
// Skip the second edge
i++;
}
// This is a boundary edge
else
{
UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
}
}
// Go through the list once more, and take any edge we marked as concave and
// clear it's vertices flags in any triangles they're used in
for (unsigned int i = 0; i < numEdges; i++)
{
EdgeRecord& er = records[i];
if (er.Concave)
{
for (unsigned int j = 0; j < numEdges; j++)
{
EdgeRecord& curER = records[j];
if (curER.VertIdx1 == er.VertIdx1 ||
curER.VertIdx1 == er.VertIdx2)
UseFlags[curER.TriIdx] &= ~curER.Vert1Flags;
if (curER.VertIdx2 == er.VertIdx1 ||
curER.VertIdx2 == er.VertIdx2)
UseFlags[curER.TriIdx] &= ~curER.Vert2Flags;
}
}
}
delete [] records;
#endif // dTRIMESH_ENABLED
}
dTriMeshDataID dGeomTriMeshDataCreate(){
return new dxTriMeshData();
}
void dGeomTriMeshDataDestroy(dTriMeshDataID g){
delete g;
}
void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans )
{
dAASSERT(g)
dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
for (int i=0; i<16; i++)
(((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ];
return;
}
dReal* dGeomTriMeshGetLastTransform( dxGeom* g )
{
dAASSERT(g)
dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
return (dReal*)(((dxTriMesh*)g)->last_trans);
}
void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
{
dUASSERT(g, "argument not trimesh data");
switch (data_id)
{
case TRIMESH_FACE_NORMALS:
g->Normals = (dReal *) in_data;
break;
default:
dUASSERT(data_id, "invalid data type");
break;
}
return;
}
void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id)
{
dUASSERT(g, "argument not trimesh data");
switch (data_id)
{
case TRIMESH_FACE_NORMALS:
return (void *) g->Normals;
break;
default:
dUASSERT(data_id, "invalid data type");
break;
}
return NULL;
}
void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* Normals)
{
dUASSERT(g, "argument not trimesh data");
g->Build(Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride,
Normals,
true);
}
void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride)
{
dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride, (void*)NULL);
}
void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride,
const void* Normals)
{
dUASSERT(g, "argument not trimesh data");
g->Build(Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride,
Normals,
false);
}
void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
const void* Vertices, int VertexStride, int VertexCount,
const void* Indices, int IndexCount, int TriStride) {
dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
Indices, IndexCount, TriStride, NULL);
}
void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
const dReal* Vertices, int VertexCount,
const int* Indices, int IndexCount,
const int* Normals){
#ifdef dSINGLE
dGeomTriMeshDataBuildSingle1(g,
Vertices, 4 * sizeof(dReal), VertexCount,
Indices, IndexCount, 3 * sizeof(unsigned int),
Normals);
#else
dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount,
Indices, IndexCount, 3 * sizeof(unsigned int),
Normals);
#endif
}
void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
const dReal* Vertices, int VertexCount,
const int* Indices, int IndexCount) {
dGeomTriMeshDataBuildSimple1(g,
Vertices, VertexCount, Indices, IndexCount,
(const int*)NULL);
}
void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
{
dUASSERT(g, "argument not trimesh data");
g->Preprocess();
}
void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
{
dUASSERT(g, "argument not trimesh data");
#if dTRIMESH_ENABLED
*buf = g->UseFlags;
*bufLen = g->Mesh.GetNbTriangles();
#endif // dTRIMESH_ENABLED
}
void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
{
dUASSERT(g, "argument not trimesh data");
g->UseFlags = buf;
}
#if dTRIMESH_ENABLED
// Trimesh Class Statics
PlanesCollider dxTriMesh::_PlanesCollider;
SphereCollider dxTriMesh::_SphereCollider;
OBBCollider dxTriMesh::_OBBCollider;
RayCollider dxTriMesh::_RayCollider;
AABBTreeCollider dxTriMesh::_AABBTreeCollider;
LSSCollider dxTriMesh::_LSSCollider;
SphereCache dxTriMesh::defaultSphereCache;
OBBCache dxTriMesh::defaultBoxCache;
LSSCache dxTriMesh::defaultCapsuleCache;
CollisionFaces dxTriMesh::Faces;
#endif // dTRIMESH_ENABLED
dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1)
{
type = dTriMeshClass;
this->Data = Data;
#if dTRIMESH_ENABLED
_RayCollider.SetDestination(&Faces);
_PlanesCollider.SetTemporalCoherence(true);
_SphereCollider.SetTemporalCoherence(true);
_SphereCollider.SetPrimitiveTests(false);
_OBBCollider.SetTemporalCoherence(true);
// no first-contact test (i.e. return full contact info)
_AABBTreeCollider.SetFirstContact( false );
// temporal coherence only works with "first conact" tests
_AABBTreeCollider.SetTemporalCoherence(false);
// Perform full BV-BV tests (true) or SAT-lite tests (false)
_AABBTreeCollider.SetFullBoxBoxTest( true );
// Perform full Primitive-BV tests (true) or SAT-lite tests (false)
_AABBTreeCollider.SetFullPrimBoxTest( true );
_LSSCollider.SetTemporalCoherence(false);
#endif // dTRIMESH_ENABLED
/* TC has speed/space 'issues' that don't make it a clear
win by default on spheres/boxes. */
this->doSphereTC = false;
this->doBoxTC = false;
this->doCapsuleTC = false;
#if dTRIMESH_ENABLED
const char* msg;
if ((msg =_AABBTreeCollider.ValidateSettings()))
dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__);
_LSSCollider.SetPrimitiveTests(false);
_LSSCollider.SetFirstContact(false);
#endif // dTRIMESH_ENABLED
for (int i=0; i<16; i++)
last_trans[i] = REAL( 0.0 );
}
dxTriMesh::~dxTriMesh(){
//
}
// Cleanup for allocations when shutting down ODE
void opcode_collider_cleanup()
{
#if dTRIMESH_ENABLED
// Clear TC caches
dxTriMesh::Faces.Empty();
dxTriMesh::defaultSphereCache.TouchedPrimitives.Empty();
dxTriMesh::defaultBoxCache.TouchedPrimitives.Empty();
dxTriMesh::defaultCapsuleCache.TouchedPrimitives.Empty();
#endif // dTRIMESH_ENABLED
}
void dxTriMesh::ClearTCCache()
{
#if dTRIMESH_ENABLED
/* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches -
but the destructor isn't called when doing this, so we would leak.
So, call the previous caches' containers' destructors by hand first. */
int i, n;
n = SphereTCCache.size();
for( i = 0; i < n; ++i ) {
SphereTCCache[i].~SphereTC();
}
SphereTCCache.setSize(0);
n = BoxTCCache.size();
for( i = 0; i < n; ++i ) {
BoxTCCache[i].~BoxTC();
}
BoxTCCache.setSize(0);
n = CapsuleTCCache.size();
for( i = 0; i < n; ++i ) {
CapsuleTCCache[i].~CapsuleTC();
}
CapsuleTCCache.setSize(0);
#endif // dTRIMESH_ENABLED
}
int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
return 1;
}
void dxTriMesh::computeAABB() {
const dxTriMeshData* d = Data;
dVector3 c;
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dMULTIPLY0_331( c, R, d->AABBCenter );
dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
dFabs(R[1] * Data->AABBExtents[1]) +
dFabs(R[2] * Data->AABBExtents[2]);
dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
dFabs(R[5] * Data->AABBExtents[1]) +
dFabs(R[6] * Data->AABBExtents[2]);
dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
dFabs(R[9] * Data->AABBExtents[1]) +
dFabs(R[10] * Data->AABBExtents[2]);
aabb[0] = c[0] + pos[0] - xrange;
aabb[1] = c[0] + pos[0] + xrange;
aabb[2] = c[1] + pos[1] - yrange;
aabb[3] = c[1] + pos[1] + yrange;
aabb[4] = c[2] + pos[2] - zrange;
aabb[5] = c[2] + pos[2] + zrange;
}
void dxTriMeshData::UpdateData()
{
#if dTRIMESH_ENABLED
BVTree.Refit();
#endif // dTRIMESH_ENABLED
}
dGeomID dCreateTriMesh(dSpaceID space,
dTriMeshDataID Data,
dTriCallback* Callback,
dTriArrayCallback* ArrayCallback,
dTriRayCallback* RayCallback)
{
dxTriMesh* Geom = new dxTriMesh(space, Data);
Geom->Callback = Callback;
Geom->ArrayCallback = ArrayCallback;
Geom->RayCallback = RayCallback;
return Geom;
}
void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->Callback = Callback;
}
dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->Callback;
}
void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
}
dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->ArrayCallback;
}
void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->RayCallback = Callback;
}
dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->RayCallback;
}
void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
((dxTriMesh*)g)->Data = Data;
}
dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
return ((dxTriMesh*)g)->Data;
}
void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
switch (geomClass)
{
case dSphereClass:
((dxTriMesh*)g)->doSphereTC = (1 == enable);
break;
case dBoxClass:
((dxTriMesh*)g)->doBoxTC = (1 == enable);
break;
case dCapsuleClass:
((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
break;
}
}
int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
switch (geomClass)
{
case dSphereClass:
if (((dxTriMesh*)g)->doSphereTC)
return 1;
break;
case dBoxClass:
if (((dxTriMesh*)g)->doBoxTC)
return 1;
break;
case dCapsuleClass:
if (((dxTriMesh*)g)->doCapsuleTC)
return 1;
break;
}
return 0;
}
void dGeomTriMeshClearTCCache(dGeomID g){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
Geom->ClearTCCache();
}
/*
* returns the TriMeshDataID
*/
dTriMeshDataID
dGeomTriMeshGetTriMeshDataID(dGeomID g)
{
dxTriMesh* Geom = (dxTriMesh*) g;
return Geom->Data;
}
// Getting data
void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
dVector3 v[3];
FetchTriangle(Geom, Index, Position, Rotation, v);
if (v0){
(*v0)[0] = v[0][0];
(*v0)[1] = v[0][1];
(*v0)[2] = v[0][2];
(*v0)[3] = v[0][3];
}
if (v1){
(*v1)[0] = v[1][0];
(*v1)[1] = v[1][1];
(*v1)[2] = v[1][2];
(*v1)[3] = v[1][3];
}
if (v2){
(*v2)[0] = v[2][0];
(*v2)[1] = v[2][1];
(*v2)[2] = v[2][2];
(*v2)[3] = v[2][3];
}
}
void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
dVector3 dv[3];
FetchTriangle(Geom, Index, Position, Rotation, dv);
GetPointFromBarycentric(dv, u, v, Out);
}
int dGeomTriMeshGetTriangleCount (dGeomID g)
{
#if dTRIMESH_ENABLED
dxTriMesh* Geom = (dxTriMesh*)g;
return Geom->Data->Mesh.GetNbTriangles();
#else
return 0;
#endif // dTRIMESH_ENABLED
}
void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
dUASSERT(g, "argument not trimesh data");
g->UpdateData();
}
#endif

212
ode/src/collision_trimesh_plane.cpp Normal file
View File

@ -0,0 +1,212 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// TriMesh - Plane collider by David Walters, July 2006
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#if dTRIMESH_ENABLED
#include "collision_util.h"
#include "collision_std.h"
#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"
#if dTRIMESH_OPCODE
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dTriMeshClass );
dIASSERT( o2->type == dPlaneClass );
// Alias pointers to the plane and trimesh
dxTriMesh* trimesh = (dxTriMesh*)( o1 );
dxPlane* plane = (dxPlane*)( o2 );
int contact_count = 0;
// Cache the maximum contact count.
const int contact_max = ( flags & 0x0ffff );
// Degenerate case where there are no contact slots.
if ( contact_count >= contact_max )
return contact_count; // <=== STOP HERE
// Cache trimesh position and rotation.
const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh );
const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh );
//
// For all triangles.
//
// Cache the triangle count.
const int tri_count = trimesh->Data->Mesh.GetNbTriangles();
VertexPointers VP;
dReal alpha;
dVector3 vertex;
#ifndef dSINGLE
dVector3 int_vertex; // Intermediate vertex for double precision mode.
#endif // dSINGLE
// For each triangle
for ( int t = 0; t < tri_count; ++t )
{
// Get triangle, which should also use callback.
trimesh->Data->Mesh.GetTriangle( VP, t );
// For each vertex.
for ( int v = 0; v < 3; ++v )
{
//
// Get Vertex
//
#ifdef dSINGLE
dMULTIPLY0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) );
#else // dDOUBLE
// OPCODE data is in single precision format.
int_vertex[ 0 ] = VP.Vertex[ v ]->x;
int_vertex[ 1 ] = VP.Vertex[ v ]->y;
int_vertex[ 2 ] = VP.Vertex[ v ]->z;
dMULTIPLY0_331( vertex, trimesh_R, int_vertex );
#endif // dSINGLE/dDOUBLE
vertex[ 0 ] += trimesh_pos[ 0 ];
vertex[ 1 ] += trimesh_pos[ 1 ];
vertex[ 2 ] += trimesh_pos[ 2 ];
//
// Collision?
//
// If alpha < 0 then point is if front of plane. i.e. no contact
// If alpha = 0 then the point is on the plane
alpha = plane->p[ 3 ] - dDOT( plane->p, vertex );
// If alpha > 0 the point is behind the plane. CONTACT!
if ( alpha > 0 )
{
// Alias the contact
dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip );
contact->pos[ 0 ] = vertex[ 0 ];
contact->pos[ 1 ] = vertex[ 1 ];
contact->pos[ 2 ] = vertex[ 2 ];
contact->normal[ 0 ] = plane->p[ 0 ];
contact->normal[ 1 ] = plane->p[ 1 ];
contact->normal[ 2 ] = plane->p[ 2 ];
contact->depth = alpha;
contact->g1 = plane;
contact->g2 = trimesh;
++contact_count;
// All contact slots are full?
if ( contact_count >= contact_max )
return contact_count; // <=== STOP HERE
}
}
}
// Return contact count.
return contact_count;
}
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dTriMeshClass );
dIASSERT( o2->type == dPlaneClass );
// Alias pointers to the plane and trimesh
dxTriMesh* trimesh = (dxTriMesh*)( o1 );
vec4f plane;
dGeomPlaneGetParams(o2, plane);
//Find collision
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result);
gim_trimesh_plane_collision(&trimesh->m_collision_trimesh,plane,&collision_result);
if(collision_result.m_size == 0 )
{
GIM_DYNARRAY_DESTROY(collision_result);
return 0;
}
dContactGeom* pcontact;
int contactcount = 0;
vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result);
for(unsigned int i = 0; i < collision_result.m_size; i++ )
{
if(contactcount < (flags & 0xffff))
{
pcontact = SAFECONTACT(flags, contacts, contactcount, skip);
contactcount++;
pcontact->pos[0] = (*planecontact_results)[0];
pcontact->pos[1] = (*planecontact_results)[1];
pcontact->pos[2] = (*planecontact_results)[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = plane[0];
pcontact->normal[1] = plane[1];
pcontact->normal[2] = plane[2];
pcontact->normal[3] = 0;
pcontact->depth = (*planecontact_results)[3];
pcontact->g1 = o1;
pcontact->g2 = o2;
}
planecontact_results++;
}
GIM_DYNARRAY_DESTROY(collision_result);
return contactcount;
}
#endif // dTRIMESH_GIMPACT
#endif // dTRIMESH_ENABLED

192
ode/src/collision_trimesh_ray.cpp Normal file
View File

@ -0,0 +1,192 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// TriMesh code by Erwin de Vries.
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#if dTRIMESH_ENABLED
#include "collision_util.h"
#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"
#if dTRIMESH_OPCODE
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
RayCollider& Collider = TriMesh->_RayCollider;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
Collider.SetFirstContact(FirstContact != 0);
Collider.SetClosestHit(ClosestHit != 0);
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
/* Make Ray */
Ray WorldRay;
WorldRay.mOrig.x = Origin[0];
WorldRay.mOrig.y = Origin[1];
WorldRay.mOrig.z = Origin[2];
WorldRay.mDir.x = Direction[0];
WorldRay.mDir.y = Direction[1];
WorldRay.mDir.z = Direction[2];
/* Intersect */
Matrix4x4 amatrix;
int TriCount = 0;
if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
TriCount = TriMesh->Faces.GetNbFaces();
}
if (TriCount == 0) {
return 0;
}
const CollisionFace* Faces = TriMesh->Faces.GetFaces();
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++) {
if (OutTriCount == (Flags & 0xffff)) {
break;
}
if (TriMesh->RayCallback == null ||
TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
Faces[i].mU, Faces[i].mV)) {
const int& TriIndex = Faces[i].mFaceID;
if (!Callback(TriMesh, RayGeom, TriIndex)) {
continue;
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
float T = Faces[i].mDistance;
Contact->pos[0] = Origin[0] + (Direction[0] * T);
Contact->pos[1] = Origin[1] + (Direction[1] * T);
Contact->pos[2] = Origin[2] + (Direction[2] * T);
Contact->pos[3] = REAL(0.0);
dVector3 vu;
vu[0] = dv[1][0] - dv[0][0];
vu[1] = dv[1][1] - dv[0][1];
vu[2] = dv[1][2] - dv[0][2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = dv[2][0] - dv[0][0];
vv[1] = dv[2][1] - dv[0][1];
vv[2] = dv[2][2] - dv[0][2];
vv[3] = REAL(0.0);
dCROSS(Contact->normal, =, vv, vu); // Reversed
dNormalize3(Contact->normal);
Contact->depth = T;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
OutTriCount++;
}
}
return OutTriCount;
}
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
{
dxTriMesh* TriMesh = (dxTriMesh*)g1;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
char intersect=0;
GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;
if(ClosestHit)
{
intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
}
else
{
intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
}
if(intersect == 0)
{
return 0;
}
int OutTriCount = 0;
if(TriMesh->RayCallback)
{
if(TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id,
contact_data.u , contact_data.v))
{
OutTriCount = 1;
}
}
else
{
OutTriCount = 1;
}
if(OutTriCount>0)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride);
VEC_COPY(Contact->pos,contact_data.m_point);
VEC_COPY(Contact->normal,contact_data.m_normal);
Contact->depth = contact_data.tparam;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
}
return OutTriCount;
}
#endif // dTRIMESH_GIMPACT
#endif // dTRIMESH_ENABLED

560
ode/src/collision_trimesh_sphere.cpp Normal file
View File

@ -0,0 +1,560 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
// TriMesh code by Erwin de Vries.
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"
#if dTRIMESH_ENABLED
#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"
#if dTRIMESH_OPCODE
#define MERGECONTACTS
// Ripped from Opcode 1.1.
static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, float& u, float& v){
// now onto the bulk of the collision...
dVector3 Diff;
Diff[0] = Origin[0] - Center[0];
Diff[1] = Origin[1] - Center[1];
Diff[2] = Origin[2] - Center[2];
Diff[3] = Origin[3] - Center[3];
float A00 = (float)dDOT(Edge0, Edge0);
float A01 = (float)dDOT(Edge0, Edge1);
float A11 = (float)dDOT(Edge1, Edge1);
float B0 = (float)dDOT(Diff, Edge0);
float B1 = (float)dDOT(Diff, Edge1);
float C = (float)dDOT(Diff, Diff);
float Det = (float)dFabs(A00 * A11 - A01 * A01);
u = A01 * B1 - A11 * B0;
v = A01 * B0 - A00 * B1;
dReal DistSq;
if (u + v <= Det){
if(u < REAL(0.0)){
if(v < REAL(0.0)){ // region 4
if(B0 < REAL(0.0)){
v = REAL(0.0);
if (-B0 >= A00){
u = REAL(1.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = -B0 / A00;
DistSq = B0 * u + C;
}
}
else{
u = REAL(0.0);
if(B1 >= REAL(0.0)){
v = REAL(0.0);
DistSq = C;
}
else if(-B1 >= A11){
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
v = -B1 / A11;
DistSq = B1 * v + C;
}
}
}
else{ // region 3
u = REAL(0.0);
if(B1 >= REAL(0.0)){
v = REAL(0.0);
DistSq = C;
}
else if(-B1 >= A11){
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
v = -B1 / A11;
DistSq = B1 * v + C;
}
}
}
else if(v < REAL(0.0)){ // region 5
v = REAL(0.0);
if (B0 >= REAL(0.0)){
u = REAL(0.0);
DistSq = C;
}
else if (-B0 >= A00){
u = REAL(1.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = -B0 / A00;
DistSq = B0 * u + C;
}
}
else{ // region 0
// minimum at interior point
if (Det == REAL(0.0)){
u = REAL(0.0);
v = REAL(0.0);
DistSq = FLT_MAX;
}
else{
dReal InvDet = REAL(1.0) / Det;
u *= InvDet;
v *= InvDet;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
}
else{
dReal Tmp0, Tmp1, Numer, Denom;
if(u < REAL(0.0)){ // region 2
Tmp0 = A01 + B0;
Tmp1 = A11 + B1;
if (Tmp1 > Tmp0){
Numer = Tmp1 - Tmp0;
Denom = A00 - REAL(2.0) * A01 + A11;
if (Numer >= Denom){
u = REAL(1.0);
v = REAL(0.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = Numer / Denom;
v = REAL(1.0) - u;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
else{
u = REAL(0.0);
if(Tmp1 <= REAL(0.0)){
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else if(B1 >= REAL(0.0)){
v = REAL(0.0);
DistSq = C;
}
else{
v = -B1 / A11;
DistSq = B1 * v + C;
}
}
}
else if(v < REAL(0.0)){ // region 6
Tmp0 = A01 + B1;
Tmp1 = A00 + B0;
if (Tmp1 > Tmp0){
Numer = Tmp1 - Tmp0;
Denom = A00 - REAL(2.0) * A01 + A11;
if (Numer >= Denom){
v = REAL(1.0);
u = REAL(0.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
v = Numer / Denom;
u = REAL(1.0) - v;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
else{
v = REAL(0.0);
if (Tmp1 <= REAL(0.0)){
u = REAL(1.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else if(B0 >= REAL(0.0)){
u = REAL(0.0);
DistSq = C;
}
else{
u = -B0 / A00;
DistSq = B0 * u + C;
}
}
}
else{ // region 1
Numer = A11 + B1 - A01 - B0;
if (Numer <= REAL(0.0)){
u = REAL(0.0);
v = REAL(1.0);
DistSq = A11 + REAL(2.0) * B1 + C;
}
else{
Denom = A00 - REAL(2.0) * A01 + A11;
if (Numer >= Denom){
u = REAL(1.0);
v = REAL(0.0);
DistSq = A00 + REAL(2.0) * B0 + C;
}
else{
u = Numer / Denom;
v = REAL(1.0) - u;
DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
}
}
}
}
Dist = dSqrt(dFabs(DistSq));
if (Dist <= Radius){
Dist = Radius - Dist;
return true;
}
else return false;
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
SphereCollider& Collider = TriMesh->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & 0xffff)){
break;
}
const int& TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCROSS(Plane, =, vu, vv);
dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
Plane[0] /= Area;
Plane[1] /= Area;
Plane[2] /= Area;
Plane[3] = dDOT(Plane, v0);
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
float side = dDOT(Plane,Position) - Plane[3];
if(side < 0.0f) {
continue;
}
dReal Depth;
float u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesnt hit triangle
}
if (Depth < REAL(0.0)){
Depth = REAL(0.0);
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dReal w = REAL(1.0) - u - v;
Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
Contact->pos[3] = REAL(0.0);
// Using normal as plane (reversed)
Contact->normal[0] = -Plane[0];
Contact->normal[1] = -Plane[1];
Contact->normal[2] = -Plane[2];
Contact->normal[3] = REAL(0.0);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dir[0] = Position[0]-Contact->pos[0];
dir[1] = Position[1]-Contact->pos[1];
dir[2] = Position[2]-Contact->pos[2];
dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
Contact->side1 = TriIndex;
//Contact->g1 = TriMesh;
//Contact->g2 = SphereGeom;
OutTriCount++;
}
#ifdef MERGECONTACTS // Merge all contacts into 1
if (OutTriCount != 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
if (OutTriCount != 1){
Contact->normal[0] *= Contact->depth;
Contact->normal[1] *= Contact->depth;
Contact->normal[2] *= Contact->depth;
Contact->normal[3] *= Contact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->pos[0] += TempContact->pos[0];
Contact->pos[1] += TempContact->pos[1];
Contact->pos[2] += TempContact->pos[2];
Contact->pos[3] += TempContact->pos[3];
Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
}
Contact->pos[0] /= OutTriCount;
Contact->pos[1] /= OutTriCount;
Contact->pos[2] /= OutTriCount;
Contact->pos[3] /= OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
dNormalize3(Contact->normal);
}
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
// TODO:
// Side1 now contains index of triangle that gave first hit
// Probably we should find index of triangle with deepest penetration
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1){
dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
}
else{
SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
}
return OutTriCount;
}
else return 0;
#else //MERGECONTACTNORMALS // Just gather penetration depths and return
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
/*Contact->normal[0] /= Contact->depth;
Contact->normal[1] /= Contact->depth;
Contact->normal[2] /= Contact->depth;
Contact->normal[3] /= Contact->depth;*/
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
}
#endif // dTRIMESH_OPCODE
#if dTRIMESH_GIMPACT
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride)
{
dxTriMesh* TriMesh = (dxTriMesh*)g1;
dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
//Collide trimeshes
gim_trimesh_sphere_collision(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
dContactGeom* pcontact;
int contactcount = 0;
unsigned i;
for (i=0;i<trimeshcontacts.m_size;i++)
{
if(contactcount < (Flags & 0xffff))
{
pcontact = SAFECONTACT(Flags, Contacts, contactcount, Stride);
contactcount++;
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = g1;
pcontact->g2 = SphereGeom;
}
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return contactcount;
}
#endif // dTRIMESH_GIMPACT
#endif // dTRIMESH_ENABLED

2020
ode/src/collision_trimesh_trimesh.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

612
ode/src/collision_util.cpp Normal file
View File

@ -0,0 +1,612 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
some useful collision utility stuff. this includes some API utility
functions that are defined in the public header files.
*/
#include <ode/common.h>
#include <ode/collision.h>
#include <ode/odemath.h>
#include "collision_util.h"
//****************************************************************************
int dCollideSpheres (dVector3 p1, dReal r1,
dVector3 p2, dReal r2, dContactGeom *c)
{
// printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n",
// d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
dReal d = dDISTANCE (p1,p2);
if (d > (r1 + r2)) return 0;
if (d <= 0) {
c->pos[0] = p1[0];
c->pos[1] = p1[1];
c->pos[2] = p1[2];
c->normal[0] = 1;
c->normal[1] = 0;
c->normal[2] = 0;
c->depth = r1 + r2;
}
else {
dReal d1 = dRecip (d);
c->normal[0] = (p1[0]-p2[0])*d1;
c->normal[1] = (p1[1]-p2[1])*d1;
c->normal[2] = (p1[2]-p2[2])*d1;
dReal k = REAL(0.5) * (r2 - r1 - d);
c->pos[0] = p1[0] + c->normal[0]*k;
c->pos[1] = p1[1] + c->normal[1]*k;
c->pos[2] = p1[2] + c->normal[2]*k;
c->depth = r1 + r2 - d;
}
return 1;
}
void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
const dVector3 pb, const dVector3 ub,
dReal *alpha, dReal *beta)
{
dVector3 p;
p[0] = pb[0] - pa[0];
p[1] = pb[1] - pa[1];
p[2] = pb[2] - pa[2];
dReal uaub = dDOT(ua,ub);
dReal q1 = dDOT(ua,p);
dReal q2 = -dDOT(ub,p);
dReal d = 1-uaub*uaub;
if (d <= REAL(0.0001)) {
// @@@ this needs to be made more robust
*alpha = 0;
*beta = 0;
}
else {
d = dRecip(d);
*alpha = (q1 + uaub*q2)*d;
*beta = (uaub*q1 + q2)*d;
}
}
// given two line segments A and B with endpoints a1-a2 and b1-b2, return the
// points on A and B that are closest to each other (in cp1 and cp2).
// in the case of parallel lines where there are multiple solutions, a
// solution involving the endpoint of at least one line will be returned.
// this will work correctly for zero length lines, e.g. if a1==a2 and/or
// b1==b2.
//
// the algorithm works by applying the voronoi clipping rule to the features
// of the line segments. the three features of each line segment are the two
// endpoints and the line between them. the voronoi clipping rule states that,
// for feature X on line A and feature Y on line B, the closest points PA and
// PB between X and Y are globally the closest points if PA is in V(Y) and
// PB is in V(X), where V(X) is the voronoi region of X.
void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
const dVector3 b1, const dVector3 b2,
dVector3 cp1, dVector3 cp2)
{
dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n;
dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det;
#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2];
#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
// check vertex-vertex features
SET3 (a1a2,a2,-,a1);
SET3 (b1b2,b2,-,b1);
SET3 (a1b1,b1,-,a1);
da1 = dDOT(a1a2,a1b1);
db1 = dDOT(b1b2,a1b1);
if (da1 <= 0 && db1 >= 0) {
SET2 (cp1,a1);
SET2 (cp2,b1);
return;
}
SET3 (a1b2,b2,-,a1);
da2 = dDOT(a1a2,a1b2);
db2 = dDOT(b1b2,a1b2);
if (da2 <= 0 && db2 <= 0) {
SET2 (cp1,a1);
SET2 (cp2,b2);
return;
}
SET3 (a2b1,b1,-,a2);
da3 = dDOT(a1a2,a2b1);
db3 = dDOT(b1b2,a2b1);
if (da3 >= 0 && db3 >= 0) {
SET2 (cp1,a2);
SET2 (cp2,b1);
return;
}
SET3 (a2b2,b2,-,a2);
da4 = dDOT(a1a2,a2b2);
db4 = dDOT(b1b2,a2b2);
if (da4 >= 0 && db4 <= 0) {
SET2 (cp1,a2);
SET2 (cp2,b2);
return;
}
// check edge-vertex features.
// if one or both of the lines has zero length, we will never get to here,
// so we do not have to worry about the following divisions by zero.
la = dDOT(a1a2,a1a2);
if (da1 >= 0 && da3 <= 0) {
k = da1 / la;
SET3 (n,a1b1,-,k*a1a2);
if (dDOT(b1b2,n) >= 0) {
SET3 (cp1,a1,+,k*a1a2);
SET2 (cp2,b1);
return;
}
}
if (da2 >= 0 && da4 <= 0) {
k = da2 / la;
SET3 (n,a1b2,-,k*a1a2);
if (dDOT(b1b2,n) <= 0) {
SET3 (cp1,a1,+,k*a1a2);
SET2 (cp2,b2);
return;
}
}
lb = dDOT(b1b2,b1b2);
if (db1 <= 0 && db2 >= 0) {
k = -db1 / lb;
SET3 (n,-a1b1,-,k*b1b2);
if (dDOT(a1a2,n) >= 0) {
SET2 (cp1,a1);
SET3 (cp2,b1,+,k*b1b2);
return;
}
}
if (db3 <= 0 && db4 >= 0) {
k = -db3 / lb;
SET3 (n,-a2b1,-,k*b1b2);
if (dDOT(a1a2,n) <= 0) {
SET2 (cp1,a2);
SET3 (cp2,b1,+,k*b1b2);
return;
}
}
// it must be edge-edge
k = dDOT(a1a2,b1b2);
det = la*lb - k*k;
if (det <= 0) {
// this should never happen, but just in case...
SET2(cp1,a1);
SET2(cp2,b1);
return;
}
det = dRecip (det);
dReal alpha = (lb*da1 - k*db1) * det;
dReal beta = ( k*da1 - la*db1) * det;
SET3 (cp1,a1,+,alpha*a1a2);
SET3 (cp2,b1,+,beta*b1b2);
# undef SET2
# undef SET3
}
// a simple root finding algorithm is used to find the value of 't' that
// satisfies:
// d|D(t)|^2/dt = 0
// where:
// |D(t)| = |p(t)-b(t)|
// where p(t) is a point on the line parameterized by t:
// p(t) = p1 + t*(p2-p1)
// and b(t) is that same point clipped to the boundary of the box. in box-
// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
// each of which looks like this:
//
// t_lo /
// ______/ -->t
// / t_hi
// /
//
// t_lo and t_hi are the t values where the line passes through the planes
// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
// in a piecewise fashion from t=0 to t=1, stopping at the point where
// d|D(t)|^2/dt crosses from negative to positive.
void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
const dVector3 c, const dMatrix3 R,
const dVector3 side,
dVector3 lret, dVector3 bret)
{
int i;
// compute the start and delta of the line p1-p2 relative to the box.
// we will do all subsequent computations in this box-relative coordinate
// system. we have to do a translation and rotation for each point.
dVector3 tmp,s,v;
tmp[0] = p1[0] - c[0];
tmp[1] = p1[1] - c[1];
tmp[2] = p1[2] - c[2];
dMULTIPLY1_331 (s,R,tmp);
tmp[0] = p2[0] - p1[0];
tmp[1] = p2[1] - p1[1];
tmp[2] = p2[2] - p1[2];
dMULTIPLY1_331 (v,R,tmp);
// mirror the line so that v has all components >= 0
dVector3 sign;
for (i=0; i<3; i++) {
if (v[i] < 0) {
s[i] = -s[i];
v[i] = -v[i];
sign[i] = -1;
}
else sign[i] = 1;
}
// compute v^2
dVector3 v2;
v2[0] = v[0]*v[0];
v2[1] = v[1]*v[1];
v2[2] = v[2]*v[2];
// compute the half-sides of the box
dReal h[3];
h[0] = REAL(0.5) * side[0];
h[1] = REAL(0.5) * side[1];
h[2] = REAL(0.5) * side[2];
// region is -1,0,+1 depending on which side of the box planes each
// coordinate is on. tanchor is the next t value at which there is a
// transition, or the last one if there are no more.
int region[3];
dReal tanchor[3];
// Denormals are a problem, because we divide by v[i], and then
// multiply that by 0. Alas, infinity times 0 is infinity (!)
// We also use v2[i], which is v[i] squared. Here's how the epsilons
// are chosen:
// float epsilon = 1.175494e-038 (smallest non-denormal number)
// double epsilon = 2.225074e-308 (smallest non-denormal number)
// For single precision, choose an epsilon such that v[i] squared is
// not a denormal; this is for performance.
// For double precision, choose an epsilon such that v[i] is not a
// denormal; this is for correctness. (Jon Watte on mailinglist)
#if defined( dSINGLE )
const dReal tanchor_eps = 1e-19;
#else
const dReal tanchor_eps = 1e-307;
#endif
// find the region and tanchor values for p1
for (i=0; i<3; i++) {
if (v[i] > tanchor_eps) {
if (s[i] < -h[i]) {
region[i] = -1;
tanchor[i] = (-h[i]-s[i])/v[i];
}
else {
region[i] = (s[i] > h[i]);
tanchor[i] = (h[i]-s[i])/v[i];
}
}
else {
region[i] = 0;
tanchor[i] = 2; // this will never be a valid tanchor
}
}
// compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
dReal t=0;
dReal dd2dt = 0;
for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
if (dd2dt >= 0) goto got_answer;
do {
// find the point on the line that is at the next clip plane boundary
dReal next_t = 1;
for (i=0; i<3; i++) {
if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
next_t = tanchor[i];
}
// compute d|d|^2/dt for the next t
dReal next_dd2dt = 0;
for (i=0; i<3; i++) {
next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
}
// if the sign of d|d|^2/dt has changed, solution = the crossover point
if (next_dd2dt >= 0) {
dReal m = (next_dd2dt-dd2dt)/(next_t - t);
t -= dd2dt/m;
goto got_answer;
}
// advance to the next anchor point / region
for (i=0; i<3; i++) {
if (tanchor[i] == next_t) {
tanchor[i] = (h[i]-s[i])/v[i];
region[i]++;
}
}
t = next_t;
dd2dt = next_dd2dt;
}
while (t < 1);
t = 1;
got_answer:
// compute closest point on the line
for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1
// compute closest point on the box
for (i=0; i<3; i++) {
tmp[i] = sign[i] * (s[i] + t*v[i]);
if (tmp[i] < -h[i]) tmp[i] = -h[i];
else if (tmp[i] > h[i]) tmp[i] = h[i];
}
dMULTIPLY0_331 (s,R,tmp);
for (i=0; i<3; i++) bret[i] = s[i] + c[i];
}
// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect
// or 0 if not.
int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2)
{
// two boxes are disjoint if (and only if) there is a separating axis
// perpendicular to a face from one box or perpendicular to an edge from
// either box. the following tests are derived from:
// "OBB Tree: A Hierarchical Structure for Rapid Interference Detection",
// S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
// Qij is abs(Rij)
dVector3 p,pp;
dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
// for the following tests, excluding computation of Rij, in the worst case,
// 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
// notation: R1=[u1 u2 u3], R2=[v1 v2 v3]
// separating axis = u1,u2,u3
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
// separating axis = v1,v2,v3
if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
// separating axis = u1 x (v1,v2,v3)
if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0;
if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0;
if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0;
// separating axis = u2 x (v1,v2,v3)
if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0;
if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0;
if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0;
// separating axis = u3 x (v1,v2,v3)
if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0;
if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0;
if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0;
return 1;
}
//****************************************************************************
// other utility functions
void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
{
aabb[0] = -dInfinity;
aabb[1] = dInfinity;
aabb[2] = -dInfinity;
aabb[3] = dInfinity;
aabb[4] = -dInfinity;
aabb[5] = dInfinity;
}
//****************************************************************************
// Helpers for Croteam's collider - by Nguyen Binh
int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
{
// calculate distance of edge points to plane
dReal fDistance0 = dPointPlaneDistance( vEpnt0 ,plPlane );
dReal fDistance1 = dPointPlaneDistance( vEpnt1 ,plPlane );
// if both points are behind the plane
if ( fDistance0 < 0 && fDistance1 < 0 )
{
// do nothing
return 0;
// if both points in front of the plane
}
else if ( fDistance0 > 0 && fDistance1 > 0 )
{
// accept them
return 1;
// if we have edge/plane intersection
} else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0))
{
// find intersection point of edge and plane
dVector3 vIntersectionPoint;
vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
// clamp correct edge to intersection point
if ( fDistance0 < 0 )
{
dVector3Copy(vIntersectionPoint,vEpnt0);
} else
{
dVector3Copy(vIntersectionPoint,vEpnt1);
}
return 1;
}
return 1;
}
// clip polygon with plane and generate new polygon points
void dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn,
dVector3 avArrayOut[], int &ctOut,
const dVector4 &plPlane )
{
// start with no output points
ctOut = 0;
int i0 = ctIn-1;
// for each edge in input polygon
for (int i1=0; i1<ctIn; i0=i1, i1++) {
// calculate distance of edge points to plane
dReal fDistance0 = dPointPlaneDistance( avArrayIn[i0],plPlane );
dReal fDistance1 = dPointPlaneDistance( avArrayIn[i1],plPlane );
// if first point is in front of plane
if( fDistance0 >= 0 ) {
// emit point
avArrayOut[ctOut][0] = avArrayIn[i0][0];
avArrayOut[ctOut][1] = avArrayIn[i0][1];
avArrayOut[ctOut][2] = avArrayIn[i0][2];
ctOut++;
}
// if points are on different sides
if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
// find intersection point of edge and plane
dVector3 vIntersectionPoint;
vIntersectionPoint[0]= avArrayIn[i0][0] -
(avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[1]= avArrayIn[i0][1] -
(avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[2]= avArrayIn[i0][2] -
(avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
// emit intersection point
avArrayOut[ctOut][0] = vIntersectionPoint[0];
avArrayOut[ctOut][1] = vIntersectionPoint[1];
avArrayOut[ctOut][2] = vIntersectionPoint[2];
ctOut++;
}
}
}
void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn,
dVector3 avArrayOut[], int &ctOut,
const dVector4 &plPlane ,dReal fRadius)
{
// start with no output points
ctOut = 0;
int i0 = ctIn-1;
// for each edge in input polygon
for (int i1=0; i1<ctIn; i0=i1, i1++)
{
// calculate distance of edge points to plane
dReal fDistance0 = dPointPlaneDistance( avArrayIn[i0],plPlane );
dReal fDistance1 = dPointPlaneDistance( avArrayIn[i1],plPlane );
// if first point is in front of plane
if( fDistance0 >= 0 )
{
// emit point
if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
{
avArrayOut[ctOut][0] = avArrayIn[i0][0];
avArrayOut[ctOut][1] = avArrayIn[i0][1];
avArrayOut[ctOut][2] = avArrayIn[i0][2];
ctOut++;
}
}
// if points are on different sides
if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) )
{
// find intersection point of edge and plane
dVector3 vIntersectionPoint;
vIntersectionPoint[0]= avArrayIn[i0][0] -
(avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[1]= avArrayIn[i0][1] -
(avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
vIntersectionPoint[2]= avArrayIn[i0][2] -
(avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
// emit intersection point
if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
{
avArrayOut[ctOut][0] = vIntersectionPoint[0];
avArrayOut[ctOut][1] = vIntersectionPoint[1];
avArrayOut[ctOut][2] = vIntersectionPoint[2];
ctOut++;
}
}
}
}

339
ode/src/collision_util.h Normal file
View File

@ -0,0 +1,339 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
some useful collision utility stuff.
*/
#ifndef _ODE_COLLISION_UTIL_H_
#define _ODE_COLLISION_UTIL_H_
#include <ode/common.h>
#include <ode/contact.h>
#include <ode/odemath.h>
#include <ode/rotation.h>
// given a pointer `p' to a dContactGeom, return the dContactGeom at
// p + skip bytes.
#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
// return 1, else return 0.
int dCollideSpheres (dVector3 p1, dReal r1,
dVector3 p2, dReal r2, dContactGeom *c);
// given two lines
// qa = pa + alpha* ua
// qb = pb + beta * ub
// where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
// beta go from [-inf,inf], return alpha and beta such that qa and qb are
// as close as possible
void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
const dVector3 pb, const dVector3 ub,
dReal *alpha, dReal *beta);
// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
// vector 'side'), compute the points of closest approach between the box
// and the line. return these points in 'lret' (the point on the line) and
// 'bret' (the point on the box). if the line actually penetrates the box
// then the solution is not unique, but only one solution will be returned.
// in this case the solution points will coincide.
void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
const dVector3 c, const dMatrix3 R,
const dVector3 side,
dVector3 lret, dVector3 bret);
// 20 Apr 2004
// Start code by Nguyen Binh
int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
// clip polygon with plane and generate new polygon points
void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );
void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
// Some vector math
inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
{
c[0] = a[0] - b[0];
c[1] = a[1] - b[1];
c[2] = a[2] - b[2];
}
// Some vector math
inline void dVector3Scale(dVector3& a,dReal nScale)
{
a[0] *= nScale ;
a[1] *= nScale ;
a[2] *= nScale ;
}
inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
{
c[0] = a[0] + b[0];
c[1] = a[1] + b[1];
c[2] = a[2] + b[2];
}
inline void dVector3Copy(const dVector3& a,dVector3& c)
{
c[0] = a[0];
c[1] = a[1];
c[2] = a[2];
}
inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
{
dCROSS(c,=,a,b);
}
inline dReal dVector3Length(const dVector3& a)
{
return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
}
inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
{
return dDOT(a,b);
}
inline void dVector3Inv(dVector3& a)
{
a[0] = -a[0];
a[1] = -a[1];
a[2] = -a[2];
}
inline dReal dVector3Length2(const dVector3& a)
{
return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
}
inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
{
v[0] = m[col + 0];
v[1] = m[col + 4];
v[2] = m[col + 8];
}
inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
{
r[0] = v[1] * m[2*4 + col] - v[2] * m[1*4 + col];
r[1] = v[2] * m[0*4 + col] - v[0] * m[2*4 + col];
r[2] = v[0] * m[1*4 + col] - v[1] * m[0*4 + col];
}
inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
{
r[0] = v[2] * m[1*4 + col] - v[1] * m[2*4 + col];
r[1] = v[0] * m[2*4 + col] - v[2] * m[0*4 + col];
r[2] = v[1] * m[0*4 + col] - v[0] * m[1*4 + col];
}
inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
{
dMULTIPLY0_331(r,m,v);
}
inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
{
return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
}
inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
{
plane[0] = normal[0];
plane[1] = normal[1];
plane[2] = normal[2];
plane[3] = distance;
}
inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
dest[4] = source[4];
dest[5] = source[5];
dest[6] = source[6];
dest[8] = source[8];
dest[9] = source[9];
dest[10]= source[10];
}
inline dReal dMatrix3Det( const dMatrix3& mat )
{
dReal det;
det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
- mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
+ mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] );
return( det );
}
inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
{
dReal det = dMatrix3Det( ma );
if ( dFabs( det ) < REAL(0.0005) )
{
dRSetIdentity( dst );
return;
}
dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det;
dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det;
dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det;
dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det;
dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det;
dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det;
dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det;
dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det;
dst[10] = ma[0]*ma[5] - ma[1]*ma[4] / det;
}
inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
{
// Nguyen Binh : this code seem to be the fastest.
dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
/*
// nVidia SDK implementation
dVector3 uv, uuv;
dVector3 qvec;
qvec[0] = quat[1];
qvec[1] = quat[2];
qvec[2] = quat[3];
dVector3Cross(qvec,source,uv);
dVector3Cross(qvec,uv,uuv);
dVector3Scale(uv,REAL(2.0)*quat[0]);
dVector3Scale(uuv,REAL(2.0));
dest[0] = source[0] + uv[0] + uuv[0];
dest[1] = source[1] + uv[1] + uuv[1];
dest[2] = source[2] + uv[2] + uuv[2];
*/
}
inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
{
dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
if (norm > REAL(0.0))
{
dQuaternion invQuat;
invQuat[0] = quat[0] / norm;
invQuat[1] = -quat[1] / norm;
invQuat[2] = -quat[2] / norm;
invQuat[3] = -quat[3] / norm;
dQuatTransform(invQuat,source,dest);
}
else
{
// Singular -> return identity
dVector3Copy(source,dest);
}
}
inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
{
rY = asin(mRot[0 * 4 + 2]);
if (rY < M_PI /2)
{
if (rY > -M_PI /2)
{
rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
}
else
{
// not unique
rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
rZ = REAL(0.0);
}
}
else
{
// not unique
rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
rZ = REAL(0.0);
}
}
inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
{
dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
if (norm > 0.0f)
{
dest[0] = source[0] / norm;
dest[1] = -source[1] / norm;
dest[2] = -source[2] / norm;
dest[3] = -source[3] / norm;
}
else
{
// Singular -> return identity
dest[0] = REAL(1.0);
dest[1] = REAL(0.0);
dest[2] = REAL(0.0);
dest[3] = REAL(0.0);
}
}
#if 1
// Fetches a contact
inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff));
return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
}
#endif
#endif

1287
ode/src/convex.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

100
ode/src/cylinder.cpp Normal file
View File

@ -0,0 +1,100 @@
/*************************************************************************
* *
* 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"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
// flat cylinder public API
dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) :
dxGeom (space,1)
{
dAASSERT (_radius > 0 && _length > 0);
type = dCylinderClass;
radius = _radius;
lz = _length;
}
void dxCylinder::computeAABB()
{
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dReal xrange = dFabs (R[0] * radius) + dFabs (R[1] * radius) + REAL(0.5)* dFabs (R[2] *
lz);
dReal yrange = dFabs (R[4] * radius) + dFabs (R[5] * radius) + REAL(0.5)* dFabs (R[6] *
lz);
dReal zrange = dFabs (R[8] * radius) + dFabs (R[9] * radius) + REAL(0.5)* dFabs (R[10] *
lz);
aabb[0] = pos[0] - xrange;
aabb[1] = pos[0] + xrange;
aabb[2] = pos[1] - yrange;
aabb[3] = pos[1] + yrange;
aabb[4] = pos[2] - zrange;
aabb[5] = pos[2] + zrange;
}
dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length)
{
return new dxCylinder (space,radius,length);
}
void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length)
{
dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
dAASSERT (radius > 0 && length > 0);
dxCylinder *c = (dxCylinder*) cylinder;
c->radius = radius;
c->lz = length;
dGeomMoved (cylinder);
}
void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length)
{
dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
dxCylinder *c = (dxCylinder*) cylinder;
*radius = c->radius;
*length = c->lz;
}

172
ode/src/error.cpp Normal file
View File

@ -0,0 +1,172 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/config.h>
#include <ode/error.h>
static dMessageFunction *error_function = 0;
static dMessageFunction *debug_function = 0;
static dMessageFunction *message_function = 0;
extern "C" void dSetErrorHandler (dMessageFunction *fn)
{
error_function = fn;
}
extern "C" void dSetDebugHandler (dMessageFunction *fn)
{
debug_function = fn;
}
extern "C" void dSetMessageHandler (dMessageFunction *fn)
{
message_function = fn;
}
extern "C" dMessageFunction *dGetErrorHandler()
{
return error_function;
}
extern "C" dMessageFunction *dGetDebugHandler()
{
return debug_function;
}
extern "C" dMessageFunction *dGetMessageHandler()
{
return message_function;
}
static void printMessage (int num, const char *msg1, const char *msg2,
va_list ap)
{
fflush (stderr);
fflush (stdout);
if (num) fprintf (stderr,"\n%s %d: ",msg1,num);
else fprintf (stderr,"\n%s: ",msg1);
vfprintf (stderr,msg2,ap);
fprintf (stderr,"\n");
fflush (stderr);
}
//****************************************************************************
// unix
#ifndef WIN32
extern "C" void dError (int num, const char *msg, ...)
{
va_list ap;
va_start (ap,msg);
if (error_function) error_function (num,msg,ap);
else printMessage (num,"ODE Error",msg,ap);
exit (1);
}
extern "C" void dDebug (int num, const char *msg, ...)
{
va_list ap;
va_start (ap,msg);
if (debug_function) debug_function (num,msg,ap);
else printMessage (num,"ODE INTERNAL ERROR",msg,ap);
// *((char *)0) = 0; ... commit SEGVicide
abort();
}
extern "C" void dMessage (int num, const char *msg, ...)
{
va_list ap;
va_start (ap,msg);
if (message_function) message_function (num,msg,ap);
else printMessage (num,"ODE Message",msg,ap);
}
#endif
//****************************************************************************
// windows
#ifdef WIN32
// isn't cygwin annoying!
#ifdef CYGWIN
#define _snprintf snprintf
#define _vsnprintf vsnprintf
#endif
#include "windows.h"
extern "C" void dError (int num, const char *msg, ...)
{
va_list ap;
va_start (ap,msg);
if (error_function) error_function (num,msg,ap);
else {
char s[1000],title[100];
_snprintf (title,sizeof(title),"ODE Error %d",num);
_vsnprintf (s,sizeof(s),msg,ap);
s[sizeof(s)-1] = 0;
MessageBox(0,s,title,MB_OK | MB_ICONWARNING);
}
exit (1);
}
extern "C" void dDebug (int num, const char *msg, ...)
{
va_list ap;
va_start (ap,msg);
if (debug_function) debug_function (num,msg,ap);
else {
char s[1000],title[100];
_snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num);
_vsnprintf (s,sizeof(s),msg,ap);
s[sizeof(s)-1] = 0;
MessageBox(0,s,title,MB_OK | MB_ICONSTOP);
}
abort();
}
extern "C" void dMessage (int num, const char *msg, ...)
{
va_list ap;
va_start (ap,msg);
if (message_function) message_function (num,msg,ap);
else printMessage (num,"ODE Message",msg,ap);
}
#endif

564
ode/src/export-dif.cpp Normal file
View File

@ -0,0 +1,564 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
* Export a DIF (Dynamics Interchange Format) file.
*/
// @@@ TODO:
// * export all spaces, and geoms in spaces, not just ones attached to bodies
// (separate export function?)
// * say the space each geom is in, so reader can construct space heirarchy
// * limot --> separate out into limits and motors?
// * make sure ODE-specific parameters divided out
#include "ode/ode.h"
#include "objects.h"
#include "joint.h"
#include "collision_kernel.h"
//***************************************************************************
// utility
struct PrintingContext {
FILE *file; // file to write to
int precision; // digits of precision to print
int indent; // number of levels of indent
void printIndent();
void printReal (dReal x);
void print (const char *name, int x);
void print (const char *name, dReal x);
void print (const char *name, const dReal *x, int n=3);
void print (const char *name, const char *x=0);
void printNonzero (const char *name, dReal x);
void printNonzero (const char *name, const dReal x[3]);
};
void PrintingContext::printIndent()
{
for (int i=0; i<indent; i++) fputc ('\t',file);
}
void PrintingContext::print (const char *name, int x)
{
printIndent();
fprintf (file,"%s = %d,\n",name,x);
}
void PrintingContext::printReal (dReal x)
{
if (x==dInfinity) {
fprintf (file,"inf");
}
else if (x==-dInfinity) {
fprintf (file,"-inf");
}
else {
fprintf (file,"%.*g",precision,x);
}
}
void PrintingContext::print (const char *name, dReal x)
{
printIndent();
fprintf (file,"%s = ",name);
printReal (x);
fprintf (file,",\n");
}
void PrintingContext::print (const char *name, const dReal *x, int n)
{
printIndent();
fprintf (file,"%s = {",name);
for (int i=0; i<n; i++) {
printReal (x[i]);
if (i < n-1) fputc (',',file);
}
fprintf (file,"},\n");
}
void PrintingContext::print (const char *name, const char *x)
{
printIndent();
if (x) {
fprintf (file,"%s = \"%s\",\n",name,x);
}
else {
fprintf (file,"%s\n",name);
}
}
void PrintingContext::printNonzero (const char *name, dReal x)
{
if (x != 0) print (name,x);
}
void PrintingContext::printNonzero (const char *name, const dReal x[3])
{
if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x);
}
//***************************************************************************
// joints
static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num)
{
if (num >= 0) {
c.printIndent();
fprintf (c.file,"limit%d = {\n",num);
}
else {
c.print ("limit = {");
}
c.indent++;
c.print ("low_stop",limot.lostop);
c.print ("high_stop",limot.histop);
c.printNonzero ("bounce",limot.bounce);
c.print ("ODE = {");
c.indent++;
c.printNonzero ("stop_erp",limot.stop_erp);
c.printNonzero ("stop_cfm",limot.stop_cfm);
c.indent--;
c.print ("},");
c.indent--;
c.print ("},");
if (num >= 0) {
c.printIndent();
fprintf (c.file,"motor%d = {\n",num);
}
else {
c.print ("motor = {");
}
c.indent++;
c.printNonzero ("vel",limot.vel);
c.printNonzero ("fmax",limot.fmax);
c.print ("ODE = {");
c.indent++;
c.printNonzero ("fudge_factor",limot.fudge_factor);
c.printNonzero ("normal_cfm",limot.normal_cfm);
c.indent--;
c.print ("},");
c.indent--;
c.print ("},");
}
static const char *getJointName (dxJoint *j)
{
switch (j->vtable->typenum) {
case dJointTypeBall: return "ball";
case dJointTypeHinge: return "hinge";
case dJointTypeSlider: return "slider";
case dJointTypeContact: return "contact";
case dJointTypeUniversal: return "universal";
case dJointTypeHinge2: return "ODE_hinge2";
case dJointTypeFixed: return "fixed";
case dJointTypeNull: return "null";
case dJointTypeAMotor: return "ODE_angular_motor";
case dJointTypeLMotor: return "ODE_linear_motor";
case dJointTypePR: return "PR";
}
return "unknown";
}
static void printBall (PrintingContext &c, dxJoint *j)
{
dxJointBall *b = (dxJointBall*) j;
c.print ("anchor1",b->anchor1);
c.print ("anchor2",b->anchor2);
}
static void printHinge (PrintingContext &c, dxJoint *j)
{
dxJointHinge *h = (dxJointHinge*) j;
c.print ("anchor1",h->anchor1);
c.print ("anchor2",h->anchor2);
c.print ("axis1",h->axis1);
c.print ("axis2",h->axis2);
c.print ("qrel",h->qrel,4);
printLimot (c,h->limot,-1);
}
static void printSlider (PrintingContext &c, dxJoint *j)
{
dxJointSlider *s = (dxJointSlider*) j;
c.print ("axis1",s->axis1);
c.print ("qrel",s->qrel,4);
c.print ("offset",s->offset);
printLimot (c,s->limot,-1);
}
static void printContact (PrintingContext &c, dxJoint *j)
{
dxJointContact *ct = (dxJointContact*) j;
int mode = ct->contact.surface.mode;
c.print ("pos",ct->contact.geom.pos);
c.print ("normal",ct->contact.geom.normal);
c.print ("depth",ct->contact.geom.depth);
//@@@ may want to write the geoms g1 and g2 that are involved, for debugging.
// to do this we must have written out all geoms in all spaces, not just
// geoms that are attached to bodies.
c.print ("mu",ct->contact.surface.mu);
if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2);
if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce);
if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel);
if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp);
if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm);
if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1);
if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2);
if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1);
if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2);
int fa = 0; // friction approximation code
if (mode & dContactApprox1_1) fa |= 1;
if (mode & dContactApprox1_2) fa |= 2;
if (fa) c.print ("friction_approximation",fa);
if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1);
}
static void printUniversal (PrintingContext &c, dxJoint *j)
{
dxJointUniversal *u = (dxJointUniversal*) j;
c.print ("anchor1",u->anchor1);
c.print ("anchor2",u->anchor2);
c.print ("axis1",u->axis1);
c.print ("axis2",u->axis2);
c.print ("qrel1",u->qrel1,4);
c.print ("qrel2",u->qrel2,4);
printLimot (c,u->limot1,1);
printLimot (c,u->limot2,2);
}
static void printHinge2 (PrintingContext &c, dxJoint *j)
{
dxJointHinge2 *h = (dxJointHinge2*) j;
c.print ("anchor1",h->anchor1);
c.print ("anchor2",h->anchor2);
c.print ("axis1",h->axis1);
c.print ("axis2",h->axis2);
c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available
c.print ("v2",h->v2);
c.print ("susp_erp",h->susp_erp);
c.print ("susp_cfm",h->susp_cfm);
printLimot (c,h->limot1,1);
printLimot (c,h->limot2,2);
}
static void printPR (PrintingContext &c, dxJoint *j)
{
dxJointPR *pr = (dxJointPR*) j;
c.print ("anchor2",pr->anchor2);
c.print ("axisR1",pr->axisR1);
c.print ("axisR2",pr->axisR2);
c.print ("axisP1",pr->axisP1);
c.print ("qrel",pr->qrel,4);
c.print ("offset",pr->offset);
printLimot (c,pr->limotP,1);
printLimot (c,pr->limotR,2);
}
static void printFixed (PrintingContext &c, dxJoint *j)
{
dxJointFixed *f = (dxJointFixed*) j;
c.print ("qrel",f->qrel);
c.print ("offset",f->offset);
}
static void printLMotor (PrintingContext &c, dxJoint *j)
{
dxJointLMotor *a = (dxJointLMotor*) j;
c.print("num", a->num);
c.printIndent();
fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
c.print ("axis1",a->axis[0]);
c.print ("axis2",a->axis[1]);
c.print ("axis3",a->axis[2]);
for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
}
static void printAMotor (PrintingContext &c, dxJoint *j)
{
dxJointAMotor *a = (dxJointAMotor*) j;
c.print ("num",a->num);
c.print ("mode",a->mode);
c.printIndent();
fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
c.print ("axis1",a->axis[0]);
c.print ("axis2",a->axis[1]);
c.print ("axis3",a->axis[2]);
for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
c.print ("angle1",a->angle[0]);
c.print ("angle2",a->angle[1]);
c.print ("angle3",a->angle[2]);
}
//***************************************************************************
// geometry
static void printGeom (PrintingContext &c, dxGeom *g);
static void printSphere (PrintingContext &c, dxGeom *g)
{
c.print ("type","sphere");
c.print ("radius",dGeomSphereGetRadius (g));
}
static void printBox (PrintingContext &c, dxGeom *g)
{
dVector3 sides;
dGeomBoxGetLengths (g,sides);
c.print ("type","box");
c.print ("sides",sides);
}
static void printCapsule (PrintingContext &c, dxGeom *g)
{
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
c.print ("type","capsule");
c.print ("radius",radius);
c.print ("length",length);
}
static void printPlane (PrintingContext &c, dxGeom *g)
{
dVector4 e;
dGeomPlaneGetParams (g,e);
c.print ("type","plane");
c.print ("normal",e);
c.print ("d",e[3]);
}
static void printRay (PrintingContext &c, dxGeom *g)
{
dReal length = dGeomRayGetLength (g);
c.print ("type","ray");
c.print ("length",length);
}
static void printGeomTransform (PrintingContext &c, dxGeom *g)
{
dxGeom *g2 = dGeomTransformGetGeom (g);
const dReal *pos = dGeomGetPosition (g2);
dQuaternion q;
dGeomGetQuaternion (g2,q);
c.print ("type","transform");
c.print ("pos",pos);
c.print ("q",q,4);
c.print ("geometry = {");
c.indent++;
printGeom (c,g2);
c.indent--;
c.print ("}");
}
static void printTriMesh (PrintingContext &c, dxGeom *g)
{
c.print ("type","trimesh");
//@@@ i don't think that the trimesh accessor functions are really
// sufficient to read out all the triangle data, and anyway we
// should have a method of not duplicating trimesh data that is
// shared.
}
static void printGeom (PrintingContext &c, dxGeom *g)
{
unsigned long category = dGeomGetCategoryBits (g);
if (category != (unsigned long)(~0)) {
c.printIndent();
fprintf (c.file,"category_bits = %lu\n",category);
}
unsigned long collide = dGeomGetCollideBits (g);
if (collide != (unsigned long)(~0)) {
c.printIndent();
fprintf (c.file,"collide_bits = %lu\n",collide);
}
if (!dGeomIsEnabled (g)) {
c.print ("disabled",1);
}
switch (g->type) {
case dSphereClass: printSphere (c,g); break;
case dBoxClass: printBox (c,g); break;
case dCapsuleClass: printCapsule (c,g); break;
case dPlaneClass: printPlane (c,g); break;
case dRayClass: printRay (c,g); break;
case dGeomTransformClass: printGeomTransform (c,g); break;
case dTriMeshClass: printTriMesh (c,g); break;
}
}
//***************************************************************************
// world
void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
{
PrintingContext c;
c.file = file;
#if defined(dSINGLE)
c.precision = 7;
#else
c.precision = 15;
#endif
c.indent = 1;
fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
c.print ("gravity",w->gravity);
c.print ("ODE = {");
c.indent++;
c.print ("ERP",w->global_erp);
c.print ("CFM",w->global_cfm);
c.print ("auto_disable = {");
c.indent++;
c.print ("linear_threshold",w->adis.linear_average_threshold);
c.print ("angular_threshold",w->adis.angular_average_threshold);
c.print ("average_samples",(int)w->adis.average_samples);
c.print ("idle_time",w->adis.idle_time);
c.print ("idle_steps",w->adis.idle_steps);
fprintf (file,"\t\t},\n\t},\n}\n");
c.indent -= 3;
// bodies
int num = 0;
fprintf (file,"%sbody = {}\n",prefix);
for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
b->tag = num;
fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
c.indent++;
c.print ("pos",b->posr.pos);
c.print ("q",b->q,4);
c.print ("lvel",b->lvel);
c.print ("avel",b->avel);
c.print ("mass",b->mass.mass);
fprintf (file,"\tI = {{");
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
c.printReal (b->mass.I[i*4+j]);
if (j < 2) fputc (',',file);
}
if (i < 2) fprintf (file,"},{");
}
fprintf (file,"}},\n");
c.printNonzero ("com",b->mass.c);
c.print ("ODE = {");
c.indent++;
if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
if (b->flags & dxBodyDisabled) c.print ("disabled",1);
if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
if (b->flags & dxBodyAutoDisable) {
c.print ("auto_disable = {");
c.indent++;
c.print ("linear_threshold",b->adis.linear_average_threshold);
c.print ("angular_threshold",b->adis.angular_average_threshold);
c.print ("average_samples",(int)b->adis.average_samples);
c.print ("idle_time",b->adis.idle_time);
c.print ("idle_steps",b->adis.idle_steps);
c.print ("time_left",b->adis_timeleft);
c.print ("steps_left",b->adis_stepsleft);
c.indent--;
c.print ("},");
}
c.printNonzero ("facc",b->facc);
c.printNonzero ("tacc",b->tacc);
if (b->flags & dxBodyFlagFiniteRotationAxis) {
c.print ("finite_rotation_axis",b->finite_rot_axis);
}
c.indent--;
c.print ("},");
if (b->geom) {
c.print ("geometry = {");
c.indent++;
for (dxGeom *g=b->geom; g; g=g->body_next) {
c.print ("{");
c.indent++;
printGeom (c,g);
c.indent--;
c.print ("},");
}
c.indent--;
c.print ("},");
}
c.indent--;
c.print ("}");
num++;
}
// joints
num = 0;
fprintf (file,"%sjoint = {}\n",prefix);
for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
c.indent++;
const char *name = getJointName (j);
fprintf (file,
"%sjoint[%d] = dynamics.%s_joint {\n"
"\tworld = %sworld,\n"
"\tbody = {%sbody[%d]"
,prefix,num,name,prefix,prefix,j->node[0].body->tag);
if (j->node[1].body) fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
fprintf (file,"},\n");
switch (j->vtable->typenum) {
case dJointTypeBall: printBall (c,j); break;
case dJointTypeHinge: printHinge (c,j); break;
case dJointTypeSlider: printSlider (c,j); break;
case dJointTypeContact: printContact (c,j); break;
case dJointTypeUniversal: printUniversal (c,j); break;
case dJointTypeHinge2: printHinge2 (c,j); break;
case dJointTypeFixed: printFixed (c,j); break;
case dJointTypeAMotor: printAMotor (c,j); break;
case dJointTypeLMotor: printLMotor (c,j); break;
case dJointTypePR: printPR (c,j); break;
}
c.indent--;
c.print ("}");
num++;
}
}

30
ode/src/fastdot.c Normal file
View File

@ -0,0 +1,30 @@
/* generated code, do not edit. */
#include "ode/matrix.h"
dReal dDot (const dReal *a, const dReal *b, int n)
{
dReal p0,q0,m0,p1,q1,m1,sum;
sum = 0;
n -= 2;
while (n >= 0) {
p0 = a[0]; q0 = b[0];
m0 = p0 * q0;
p1 = a[1]; q1 = b[1];
m1 = p1 * q1;
sum += m0;
sum += m1;
a += 2;
b += 2;
n -= 2;
}
n += 2;
while (n > 0) {
sum += (*a) * (*b);
a++;
b++;
n--;
}
return sum;
}

381
ode/src/fastldlt.c Normal file
View File

@ -0,0 +1,381 @@
/* generated code, do not edit. */
#include "ode/matrix.h"
/* solve L*X=B, with B containing 1 right hand sides.
* L is an n*n lower triangular matrix with ones on the diagonal.
* L is stored by rows and its leading dimension is lskip.
* B is an n*1 matrix that contains the right hand sides.
* B is stored by columns and its leading dimension is also lskip.
* B is overwritten with X.
* this processes blocks of 2*2.
* if this is in the factorizer source file, n must be a multiple of 2.
*/
static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1)
{
/* declare variables - Z matrix, p and q vectors, etc */
dReal Z11,m11,Z21,m21,p1,q1,p2,*ex;
const dReal *ell;
int i,j;
/* compute all 2 x 1 blocks of X */
for (i=0; i < n; i+=2) {
/* compute all 2 x 1 block of X, from rows i..i+2-1 */
/* set the Z matrix to 0 */
Z11=0;
Z21=0;
ell = L + i*lskip1;
ex = B;
/* the inner loop that computes outer products and adds them to Z */
for (j=i-2; j >= 0; j -= 2) {
/* compute outer product and add it to the Z matrix */
p1=ell[0];
q1=ex[0];
m11 = p1 * q1;
p2=ell[lskip1];
m21 = p2 * q1;
Z11 += m11;
Z21 += m21;
/* compute outer product and add it to the Z matrix */
p1=ell[1];
q1=ex[1];
m11 = p1 * q1;
p2=ell[1+lskip1];
m21 = p2 * q1;
/* advance pointers */
ell += 2;
ex += 2;
Z11 += m11;
Z21 += m21;
/* end of inner loop */
}
/* compute left-over iterations */
j += 2;
for (; j > 0; j--) {
/* compute outer product and add it to the Z matrix */
p1=ell[0];
q1=ex[0];
m11 = p1 * q1;
p2=ell[lskip1];
m21 = p2 * q1;
/* advance pointers */
ell += 1;
ex += 1;
Z11 += m11;
Z21 += m21;
}
/* finish computing the X(i) block */
Z11 = ex[0] - Z11;
ex[0] = Z11;
p1 = ell[lskip1];
Z21 = ex[1] - Z21 - p1*Z11;
ex[1] = Z21;
/* end of outer loop */
}
}
/* solve L*X=B, with B containing 2 right hand sides.
* L is an n*n lower triangular matrix with ones on the diagonal.
* L is stored by rows and its leading dimension is lskip.
* B is an n*2 matrix that contains the right hand sides.
* B is stored by columns and its leading dimension is also lskip.
* B is overwritten with X.
* this processes blocks of 2*2.
* if this is in the factorizer source file, n must be a multiple of 2.
*/
static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1)
{
/* declare variables - Z matrix, p and q vectors, etc */
dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
const dReal *ell;
int i,j;
/* compute all 2 x 2 blocks of X */
for (i=0; i < n; i+=2) {
/* compute all 2 x 2 block of X, from rows i..i+2-1 */
/* set the Z matrix to 0 */
Z11=0;
Z12=0;
Z21=0;
Z22=0;
ell = L + i*lskip1;
ex = B;
/* the inner loop that computes outer products and adds them to Z */
for (j=i-2; j >= 0; j -= 2) {
/* compute outer product and add it to the Z matrix */
p1=ell[0];
q1=ex[0];
m11 = p1 * q1;
q2=ex[lskip1];
m12 = p1 * q2;
p2=ell[lskip1];
m21 = p2 * q1;
m22 = p2 * q2;
Z11 += m11;
Z12 += m12;
Z21 += m21;
Z22 += m22;
/* compute outer product and add it to the Z matrix */
p1=ell[1];
q1=ex[1];
m11 = p1 * q1;
q2=ex[1+lskip1];
m12 = p1 * q2;
p2=ell[1+lskip1];
m21 = p2 * q1;
m22 = p2 * q2;
/* advance pointers */
ell += 2;
ex += 2;
Z11 += m11;
Z12 += m12;
Z21 += m21;
Z22 += m22;
/* end of inner loop */
}
/* compute left-over iterations */
j += 2;
for (; j > 0; j--) {
/* compute outer product and add it to the Z matrix */
p1=ell[0];
q1=ex[0];
m11 = p1 * q1;
q2=ex[lskip1];
m12 = p1 * q2;
p2=ell[lskip1];
m21 = p2 * q1;
m22 = p2 * q2;
/* advance pointers */
ell += 1;
ex += 1;
Z11 += m11;
Z12 += m12;
Z21 += m21;
Z22 += m22;
}
/* finish computing the X(i) block */
Z11 = ex[0] - Z11;
ex[0] = Z11;
Z12 = ex[lskip1] - Z12;
ex[lskip1] = Z12;
p1 = ell[lskip1];
Z21 = ex[1] - Z21 - p1*Z11;
ex[1] = Z21;
Z22 = ex[1+lskip1] - Z22 - p1*Z12;
ex[1+lskip1] = Z22;
/* end of outer loop */
}
}
void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
{
int i,j;
dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
if (n < 1) return;
for (i=0; i<=n-2; i += 2) {
/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
dSolveL1_2 (A,A+i*nskip1,i,nskip1);
/* scale the elements in a 2 x i block at A(i,0), and also */
/* compute Z = the outer product matrix that we'll need. */
Z11 = 0;
Z21 = 0;
Z22 = 0;
ell = A+i*nskip1;
dee = d;
for (j=i-6; j >= 0; j -= 6) {
p1 = ell[0];
p2 = ell[nskip1];
dd = dee[0];
q1 = p1*dd;
q2 = p2*dd;
ell[0] = q1;
ell[nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[1];
p2 = ell[1+nskip1];
dd = dee[1];
q1 = p1*dd;
q2 = p2*dd;
ell[1] = q1;
ell[1+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[2];
p2 = ell[2+nskip1];
dd = dee[2];
q1 = p1*dd;
q2 = p2*dd;
ell[2] = q1;
ell[2+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[3];
p2 = ell[3+nskip1];
dd = dee[3];
q1 = p1*dd;
q2 = p2*dd;
ell[3] = q1;
ell[3+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[4];
p2 = ell[4+nskip1];
dd = dee[4];
q1 = p1*dd;
q2 = p2*dd;
ell[4] = q1;
ell[4+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[5];
p2 = ell[5+nskip1];
dd = dee[5];
q1 = p1*dd;
q2 = p2*dd;
ell[5] = q1;
ell[5+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
ell += 6;
dee += 6;
}
/* compute left-over iterations */
j += 6;
for (; j > 0; j--) {
p1 = ell[0];
p2 = ell[nskip1];
dd = dee[0];
q1 = p1*dd;
q2 = p2*dd;
ell[0] = q1;
ell[nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
ell++;
dee++;
}
/* solve for diagonal 2 x 2 block at A(i,i) */
Z11 = ell[0] - Z11;
Z21 = ell[nskip1] - Z21;
Z22 = ell[1+nskip1] - Z22;
dee = d + i;
/* factorize 2 x 2 block Z,dee */
/* factorize row 1 */
dee[0] = dRecip(Z11);
/* factorize row 2 */
sum = 0;
q1 = Z21;
q2 = q1 * dee[0];
Z21 = q2;
sum += q1*q2;
dee[1] = dRecip(Z22 - sum);
/* done factorizing 2 x 2 block */
ell[nskip1] = Z21;
}
/* compute the (less than 2) rows at the bottom */
switch (n-i) {
case 0:
break;
case 1:
dSolveL1_1 (A,A+i*nskip1,i,nskip1);
/* scale the elements in a 1 x i block at A(i,0), and also */
/* compute Z = the outer product matrix that we'll need. */
Z11 = 0;
ell = A+i*nskip1;
dee = d;
for (j=i-6; j >= 0; j -= 6) {
p1 = ell[0];
dd = dee[0];
q1 = p1*dd;
ell[0] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[1];
dd = dee[1];
q1 = p1*dd;
ell[1] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[2];
dd = dee[2];
q1 = p1*dd;
ell[2] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[3];
dd = dee[3];
q1 = p1*dd;
ell[3] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[4];
dd = dee[4];
q1 = p1*dd;
ell[4] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[5];
dd = dee[5];
q1 = p1*dd;
ell[5] = q1;
m11 = p1*q1;
Z11 += m11;
ell += 6;
dee += 6;
}
/* compute left-over iterations */
j += 6;
for (; j > 0; j--) {
p1 = ell[0];
dd = dee[0];
q1 = p1*dd;
ell[0] = q1;
m11 = p1*q1;
Z11 += m11;
ell++;
dee++;
}
/* solve for diagonal 1 x 1 block at A(i,i) */
Z11 = ell[0] - Z11;
dee = d + i;
/* factorize 1 x 1 block Z,dee */
/* factorize row 1 */
dee[0] = dRecip(Z11);
/* done factorizing 1 x 1 block */
break;
default: *((char*)0)=0; /* this should never happen! */
}
}

298
ode/src/fastlsolve.c Normal file
View File

@ -0,0 +1,298 @@
/* generated code, do not edit. */
#include "ode/matrix.h"
/* solve L*X=B, with B containing 1 right hand sides.
* L is an n*n lower triangular matrix with ones on the diagonal.
* L is stored by rows and its leading dimension is lskip.
* B is an n*1 matrix that contains the right hand sides.
* B is stored by columns and its leading dimension is also lskip.
* B is overwritten with X.
* this processes blocks of 4*4.
* if this is in the factorizer source file, n must be a multiple of 4.
*/
void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1)
{
/* declare variables - Z matrix, p and q vectors, etc */
dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
const dReal *ell;
int lskip2,lskip3,i,j;
/* compute lskip values */
lskip2 = 2*lskip1;
lskip3 = 3*lskip1;
/* compute all 4 x 1 blocks of X */
for (i=0; i <= n-4; i+=4) {
/* compute all 4 x 1 block of X, from rows i..i+4-1 */
/* set the Z matrix to 0 */
Z11=0;
Z21=0;
Z31=0;
Z41=0;
ell = L + i*lskip1;
ex = B;
/* the inner loop that computes outer products and adds them to Z */
for (j=i-12; j >= 0; j -= 12) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
p2=ell[lskip1];
p3=ell[lskip2];
p4=ell[lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[1];
q1=ex[1];
p2=ell[1+lskip1];
p3=ell[1+lskip2];
p4=ell[1+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[2];
q1=ex[2];
p2=ell[2+lskip1];
p3=ell[2+lskip2];
p4=ell[2+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[3];
q1=ex[3];
p2=ell[3+lskip1];
p3=ell[3+lskip2];
p4=ell[3+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[4];
q1=ex[4];
p2=ell[4+lskip1];
p3=ell[4+lskip2];
p4=ell[4+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[5];
q1=ex[5];
p2=ell[5+lskip1];
p3=ell[5+lskip2];
p4=ell[5+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[6];
q1=ex[6];
p2=ell[6+lskip1];
p3=ell[6+lskip2];
p4=ell[6+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[7];
q1=ex[7];
p2=ell[7+lskip1];
p3=ell[7+lskip2];
p4=ell[7+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[8];
q1=ex[8];
p2=ell[8+lskip1];
p3=ell[8+lskip2];
p4=ell[8+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[9];
q1=ex[9];
p2=ell[9+lskip1];
p3=ell[9+lskip2];
p4=ell[9+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[10];
q1=ex[10];
p2=ell[10+lskip1];
p3=ell[10+lskip2];
p4=ell[10+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* load p and q values */
p1=ell[11];
q1=ex[11];
p2=ell[11+lskip1];
p3=ell[11+lskip2];
p4=ell[11+lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* advance pointers */
ell += 12;
ex += 12;
/* end of inner loop */
}
/* compute left-over iterations */
j += 12;
for (; j > 0; j--) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
p2=ell[lskip1];
p3=ell[lskip2];
p4=ell[lskip3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
Z21 += p2 * q1;
Z31 += p3 * q1;
Z41 += p4 * q1;
/* advance pointers */
ell += 1;
ex += 1;
}
/* finish computing the X(i) block */
Z11 = ex[0] - Z11;
ex[0] = Z11;
p1 = ell[lskip1];
Z21 = ex[1] - Z21 - p1*Z11;
ex[1] = Z21;
p1 = ell[lskip2];
p2 = ell[1+lskip2];
Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
ex[2] = Z31;
p1 = ell[lskip3];
p2 = ell[1+lskip3];
p3 = ell[2+lskip3];
Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
ex[3] = Z41;
/* end of outer loop */
}
/* compute rows at end that are not a multiple of block size */
for (; i < n; i++) {
/* compute all 1 x 1 block of X, from rows i..i+1-1 */
/* set the Z matrix to 0 */
Z11=0;
ell = L + i*lskip1;
ex = B;
/* the inner loop that computes outer products and adds them to Z */
for (j=i-12; j >= 0; j -= 12) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[1];
q1=ex[1];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[2];
q1=ex[2];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[3];
q1=ex[3];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[4];
q1=ex[4];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[5];
q1=ex[5];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[6];
q1=ex[6];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[7];
q1=ex[7];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[8];
q1=ex[8];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[9];
q1=ex[9];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[10];
q1=ex[10];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* load p and q values */
p1=ell[11];
q1=ex[11];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* advance pointers */
ell += 12;
ex += 12;
/* end of inner loop */
}
/* compute left-over iterations */
j += 12;
for (; j > 0; j--) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
/* compute outer product and add it to the Z matrix */
Z11 += p1 * q1;
/* advance pointers */
ell += 1;
ex += 1;
}
/* finish computing the X(i) block */
Z11 = ex[0] - Z11;
ex[0] = Z11;
}
}

199
ode/src/fastltsolve.c Normal file
View File

@ -0,0 +1,199 @@
/* generated code, do not edit. */
#include "ode/matrix.h"
/* solve L^T * x=b, with b containing 1 right hand side.
* L is an n*n lower triangular matrix with ones on the diagonal.
* L is stored by rows and its leading dimension is lskip.
* b is an n*1 matrix that contains the right hand side.
* b is overwritten with x.
* this processes blocks of 4.
*/
void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1)
{
/* declare variables - Z matrix, p and q vectors, etc */
dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
const dReal *ell;
int lskip2,lskip3,i,j;
/* special handling for L and B because we're solving L1 *transpose* */
L = L + (n-1)*(lskip1+1);
B = B + n-1;
lskip1 = -lskip1;
/* compute lskip values */
lskip2 = 2*lskip1;
lskip3 = 3*lskip1;
/* compute all 4 x 1 blocks of X */
for (i=0; i <= n-4; i+=4) {
/* compute all 4 x 1 block of X, from rows i..i+4-1 */
/* set the Z matrix to 0 */
Z11=0;
Z21=0;
Z31=0;
Z41=0;
ell = L - i;
ex = B;
/* the inner loop that computes outer products and adds them to Z */
for (j=i-4; j >= 0; j -= 4) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
p2=ell[-1];
p3=ell[-2];
p4=ell[-3];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
m21 = p2 * q1;
m31 = p3 * q1;
m41 = p4 * q1;
ell += lskip1;
Z11 += m11;
Z21 += m21;
Z31 += m31;
Z41 += m41;
/* load p and q values */
p1=ell[0];
q1=ex[-1];
p2=ell[-1];
p3=ell[-2];
p4=ell[-3];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
m21 = p2 * q1;
m31 = p3 * q1;
m41 = p4 * q1;
ell += lskip1;
Z11 += m11;
Z21 += m21;
Z31 += m31;
Z41 += m41;
/* load p and q values */
p1=ell[0];
q1=ex[-2];
p2=ell[-1];
p3=ell[-2];
p4=ell[-3];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
m21 = p2 * q1;
m31 = p3 * q1;
m41 = p4 * q1;
ell += lskip1;
Z11 += m11;
Z21 += m21;
Z31 += m31;
Z41 += m41;
/* load p and q values */
p1=ell[0];
q1=ex[-3];
p2=ell[-1];
p3=ell[-2];
p4=ell[-3];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
m21 = p2 * q1;
m31 = p3 * q1;
m41 = p4 * q1;
ell += lskip1;
ex -= 4;
Z11 += m11;
Z21 += m21;
Z31 += m31;
Z41 += m41;
/* end of inner loop */
}
/* compute left-over iterations */
j += 4;
for (; j > 0; j--) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
p2=ell[-1];
p3=ell[-2];
p4=ell[-3];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
m21 = p2 * q1;
m31 = p3 * q1;
m41 = p4 * q1;
ell += lskip1;
ex -= 1;
Z11 += m11;
Z21 += m21;
Z31 += m31;
Z41 += m41;
}
/* finish computing the X(i) block */
Z11 = ex[0] - Z11;
ex[0] = Z11;
p1 = ell[-1];
Z21 = ex[-1] - Z21 - p1*Z11;
ex[-1] = Z21;
p1 = ell[-2];
p2 = ell[-2+lskip1];
Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
ex[-2] = Z31;
p1 = ell[-3];
p2 = ell[-3+lskip1];
p3 = ell[-3+lskip2];
Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
ex[-3] = Z41;
/* end of outer loop */
}
/* compute rows at end that are not a multiple of block size */
for (; i < n; i++) {
/* compute all 1 x 1 block of X, from rows i..i+1-1 */
/* set the Z matrix to 0 */
Z11=0;
ell = L - i;
ex = B;
/* the inner loop that computes outer products and adds them to Z */
for (j=i-4; j >= 0; j -= 4) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
ell += lskip1;
Z11 += m11;
/* load p and q values */
p1=ell[0];
q1=ex[-1];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
ell += lskip1;
Z11 += m11;
/* load p and q values */
p1=ell[0];
q1=ex[-2];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
ell += lskip1;
Z11 += m11;
/* load p and q values */
p1=ell[0];
q1=ex[-3];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
ell += lskip1;
ex -= 4;
Z11 += m11;
/* end of inner loop */
}
/* compute left-over iterations */
j += 4;
for (; j > 0; j--) {
/* load p and q values */
p1=ell[0];
q1=ex[0];
/* compute outer product and add it to the Z matrix */
m11 = p1 * q1;
ell += lskip1;
ex -= 1;
Z11 += m11;
}
/* finish computing the X(i) block */
Z11 = ex[0] - Z11;
ex[0] = Z11;
}
}

1734
ode/src/heightfield.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

207
ode/src/heightfield.h Normal file
View File

@ -0,0 +1,207 @@
// dHeightfield Collider
// Martijn Buijs 2006 http://home.planet.nl/~buijs512/
// Based on Terrain & Cone contrib by:
// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
#ifndef _DHEIGHTFIELD_H_
#define _DHEIGHTFIELD_H_
//------------------------------------------------------------------------------
#include <ode/common.h>
#include "collision_kernel.h"
#define HEIGHTFIELDMAXCONTACTPERCELL 10
//
// dxHeightfieldData
//
// Heightfield Data structure
//
struct dxHeightfieldData
{
dReal m_fWidth; // World space heightfield dimension on X axis
dReal m_fDepth; // World space heightfield dimension on Z axis
dReal m_fSampleWidth; // Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1))
dReal m_fSampleDepth; // Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
dReal m_fInvSampleWidth; // Cache of inverse Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1))
dReal m_fInvSampleDepth; // Cache of inverse Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
dReal m_fHalfWidth; // Cache of half of m_fWidth
dReal m_fHalfDepth; // Cache of half of m_fDepth
dReal m_fMinHeight; // Min sample height value (scaled and offset)
dReal m_fMaxHeight; // Max sample height value (scaled and offset)
dReal m_fThickness; // Surface thickness (added to bottom AABB)
dReal m_fScale; // Sample value multiplier
dReal m_fOffset; // Vertical sample offset
int m_nWidthSamples; // Vertex count on X axis edge (number of samples)
int m_nDepthSamples; // Vertex count on Z axis edge (number of samples)
int m_bCopyHeightData; // Do we own the sample data?
int m_bWrapMode; // Heightfield wrapping mode (0=finite, 1=infinite)
int m_nGetHeightMode; // GetHeight mode ( 0=callback, 1=byte, 2=short, 3=float )
const void* m_pHeightData; // Sample data array
void* m_pUserData; // Callback user data
dContactGeom m_contacts[HEIGHTFIELDMAXCONTACTPERCELL];
dHeightfieldGetHeight* m_pGetHeightCallback; // Callback pointer.
dxHeightfieldData();
~dxHeightfieldData();
void SetData( int nWidthSamples, int nDepthSamples,
dReal fWidth, dReal fDepth,
dReal fScale, dReal fOffset,
dReal fThickness, int bWrapMode );
void ComputeHeightBounds();
bool IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const;
bool IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const;
dReal GetHeight(int x, int z);
dReal GetHeight(dReal x, dReal z);
};
class HeightFieldVertex;
class HeightFieldEdge;
class HeightFieldTriangle;
class HeightFieldVertex
{
public:
HeightFieldVertex(){};
dVector3 vertex;
bool state;
};
class HeightFieldEdge
{
public:
HeightFieldEdge(){};
HeightFieldVertex *vertices[2];
};
//
// HeightFieldTriangle
//
// HeightFieldTriangle composing heightfield mesh
//
class HeightFieldTriangle
{
public:
HeightFieldTriangle(){};
inline void setMinMax()
{
maxAAAB = vertices[0]->vertex[1] > vertices[1]->vertex[1] ? vertices[0]->vertex[1] : vertices[1]->vertex[1];
maxAAAB = vertices[2]->vertex[1] > maxAAAB ? vertices[2]->vertex[1] : maxAAAB;
};
HeightFieldVertex *vertices[3];
dReal planeDef[4];
dReal maxAAAB;
bool isUp;
bool state;
};
//
// HeightFieldTriangle
//
// HeightFieldPlane composing heightfield mesh
//
class HeightFieldPlane
{
public:
HeightFieldPlane():
trianglelist(0),
trianglelistReservedSize(0),
trianglelistCurrentSize(0)
{
};
~HeightFieldPlane()
{
delete [] trianglelist;
};
inline void setMinMax()
{
const size_t asize = trianglelistCurrentSize;
if (asize > 0)
{
maxAAAB = trianglelist[0]->maxAAAB;
for (size_t k = 1; asize > k; k++)
{
if (trianglelist[k]->maxAAAB > maxAAAB)
maxAAAB = trianglelist[k]->maxAAAB;
}
}
};
void resetTriangleListSize(const size_t newSize)
{
if (trianglelistReservedSize < newSize)
{
delete [] trianglelist;
trianglelistReservedSize = newSize;
trianglelist = new HeightFieldTriangle *[newSize];
}
trianglelistCurrentSize = 0;
}
void addTriangle(HeightFieldTriangle *tri)
{
trianglelist[trianglelistCurrentSize++] = tri;
}
HeightFieldTriangle **trianglelist;
size_t trianglelistReservedSize;
size_t trianglelistCurrentSize;
dReal maxAAAB;
dReal planeDef[4];
};
//
// dxHeightfield
//
// Heightfield geom structure
//
struct dxHeightfield : public dxGeom
{
dxHeightfieldData* m_p_data;
dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable );
~dxHeightfield();
void computeAABB();
int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
dxGeom *o2, const int numMaxContacts,
int flags, dContactGeom *contact, int skip );
void resetHeightBuffer();
void sortPlanes(const size_t numPlanes);
HeightFieldPlane **tempPlaneBuffer;
size_t tempPlaneBufferSize;
HeightFieldTriangle *tempTriangleBuffer;
size_t tempTriangleBufferSize;
HeightFieldVertex **tempHeightBuffer;
size_t tempHeightBufferSizeX;
size_t tempHeightBufferSizeZ;
};
//------------------------------------------------------------------------------
#endif //_DHEIGHTFIELD_H_

3986
ode/src/joint.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

339
ode/src/joint.h Normal file
View File

@ -0,0 +1,339 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#ifndef _ODE_JOINT_H_
#define _ODE_JOINT_H_
#include "objects.h"
#include <ode/contact.h>
#include "obstack.h"
// joint flags
enum {
// if this flag is set, the joint was allocated in a joint group
dJOINT_INGROUP = 1,
// if this flag is set, the joint was attached with arguments (0,body).
// our convention is to treat all attaches as (body,0), i.e. so node[0].body
// is always nonzero, so this flag records the fact that the arguments were
// swapped.
dJOINT_REVERSE = 2,
// if this flag is set, the joint can not have just one body attached to it,
// it must have either zero or two bodies attached.
dJOINT_TWOBODIES = 4
};
// there are two of these nodes in the joint, one for each connection to a
// body. these are node of a linked list kept by each body of it's connecting
// joints. but note that the body pointer in each node points to the body that
// makes use of the *other* node, not this node. this trick makes it a bit
// easier to traverse the body/joint graph.
struct dxJointNode {
dxJoint *joint; // pointer to enclosing dxJoint object
dxBody *body; // *other* body this joint is connected to
dxJointNode *next; // next node in body's list of connected joints
};
struct dxJoint : public dObject {
// naming convention: the "first" body this is connected to is node[0].body,
// and the "second" body is node[1].body. if this joint is only connected
// to one body then the second body is 0.
// info returned by getInfo1 function. the constraint dimension is m (<=6).
// i.e. that is the total number of rows in the jacobian. `nub' is the
// number of unbounded variables (which have lo,hi = -/+ infinity).
struct Info1 {
int m,nub;
};
// info returned by getInfo2 function
struct Info2 {
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
dReal fps,erp;
// for the first and second body, pointers to two (linear and angular)
// n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0.
dReal *J1l,*J1a,*J2l,*J2a;
// elements to jump from one row to the next in J's
int rowskip;
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry.
dReal *c,*cfm;
// lo and hi limits for variables (set to -/+ infinity on entry).
dReal *lo,*hi;
// findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry.
// note that the returned indexes are relative to the first index of
// the constraint.
int *findex;
};
// virtual function table: size of the joint structure, function pointers.
// we do it this way instead of using C++ virtual functions because
// sometimes we need to allocate joints ourself within a memory pool.
typedef void init_fn (dxJoint *joint);
typedef void getInfo1_fn (dxJoint *joint, Info1 *info);
typedef void getInfo2_fn (dxJoint *joint, Info2 *info);
struct Vtable {
int size;
init_fn *init;
getInfo1_fn *getInfo1;
getInfo2_fn *getInfo2;
int typenum; // a dJointTypeXXX type number
};
Vtable *vtable; // virtual function table
int flags; // dJOINT_xxx flags
dxJointNode node[2]; // connections to bodies. node[1].body can be 0
dJointFeedback *feedback; // optional feedback structure
dReal lambda[6]; // lambda generated by last step
};
// joint group. NOTE: any joints in the group that have their world destroyed
// will have their world pointer set to 0.
struct dxJointGroup : public dBase {
int num; // number of joints on the stack
dObStack stack; // a stack of (possibly differently sized) dxJoint
}; // objects.
// common limit and motor information for a single joint axis of movement
struct dxJointLimitMotor {
dReal vel,fmax; // powered joint: velocity, max force
dReal lostop,histop; // joint limits, relative to initial position
dReal fudge_factor; // when powering away from joint limits
dReal normal_cfm; // cfm to use when not at a stop
dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit
dReal bounce; // restitution factor
// variables used between getInfo1() and getInfo2()
int limit; // 0=free, 1=at lo limit, 2=at hi limit
dReal limit_err; // if at limit, amount over limit
void init (dxWorld *);
void set (int num, dReal value);
dReal get (int num);
int testRotationalLimit (dReal angle);
int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row,
dVector3 ax1, int rotational);
};
// ball and socket
struct dxJointBall : public dxJoint {
dVector3 anchor1; // anchor w.r.t first body
dVector3 anchor2; // anchor w.r.t second body
};
extern struct dxJoint::Vtable __dball_vtable;
// hinge
struct dxJointHinge : public dxJoint {
dVector3 anchor1; // anchor w.r.t first body
dVector3 anchor2; // anchor w.r.t second body
dVector3 axis1; // axis w.r.t first body
dVector3 axis2; // axis w.r.t second body
dQuaternion qrel; // initial relative rotation body1 -> body2
dxJointLimitMotor limot; // limit and motor information
};
extern struct dxJoint::Vtable __dhinge_vtable;
// universal
struct dxJointUniversal : public dxJoint {
dVector3 anchor1; // anchor w.r.t first body
dVector3 anchor2; // anchor w.r.t second body
dVector3 axis1; // axis w.r.t first body
dVector3 axis2; // axis w.r.t second body
dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece
dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2
dxJointLimitMotor limot1; // limit and motor information for axis1
dxJointLimitMotor limot2; // limit and motor information for axis2
};
extern struct dxJoint::Vtable __duniversal_vtable;
/**
* The axisP must be perpendicular to axis2
* <PRE>
* +-------------+
* | x |
* +------------\+
* Prismatic articulation .. ..
* | .. ..
* \/ .. ..
* +--------------+ --| __.. .. anchor2
* | x | .....|.......(__) ..
* +--------------+ --| ^ <
* |----------------------->|
* Offset |--- Rotoide articulation
* </PRE>
*/
struct dxJointPR : public dxJoint {
dVector3 anchor2; ///< @brief Position of the rotoide articulation
///< w.r.t second body.
///< @note Position of body 2 in world frame +
///< anchor2 in world frame give the position
///< of the rotoide articulation
dVector3 axisR1; ///< axis of the rotoide articulation w.r.t first body.
///< @note This is considered as axis1 from the parameter
///< view.
dVector3 axisR2; ///< axis of the rotoide articulation w.r.t second body.
///< @note This is considered also as axis1 from the
///< parameter view
dVector3 axisP1; ///< axis for the prismatic articulation w.r.t first body.
///< @note This is considered as axis2 in from the parameter
///< view
dQuaternion qrel; ///< initial relative rotation body1 -> body2.
dVector3 offset; ///< @brief vector between the body1 and the rotoide
///< articulation.
///<
///< Going from the first to the second in the frame
///< of body1.
///< That should be aligned with body1 center along axisP
///< This is calculated whe the axis are set.
dVector3 prev; ///< Previous position in world frame of cm of body to w.r.t anchor2.
dxJointLimitMotor limotR; ///< limit and motor information for the rotoide articulation.
dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation.
};
extern struct dxJoint::Vtable __dPR_vtable;
// slider. if body2 is 0 then qrel is the absolute rotation of body1 and
// offset is the position of body1 center along axis1.
struct dxJointSlider : public dxJoint {
dVector3 axis1; // axis w.r.t first body
dQuaternion qrel; // initial relative rotation body1 -> body2
dVector3 offset; // point relative to body2 that should be
// aligned with body1 center along axis1
dxJointLimitMotor limot; // limit and motor information
};
extern struct dxJoint::Vtable __dslider_vtable;
// contact
struct dxJointContact : public dxJoint {
int the_m; // number of rows computed by getInfo1
dContact contact;
};
extern struct dxJoint::Vtable __dcontact_vtable;
// hinge 2
struct dxJointHinge2 : public dxJoint {
dVector3 anchor1; // anchor w.r.t first body
dVector3 anchor2; // anchor w.r.t second body
dVector3 axis1; // axis 1 w.r.t first body
dVector3 axis2; // axis 2 w.r.t second body
dReal c0,s0; // cos,sin of desired angle between axis 1,2
dVector3 v1,v2; // angle ref vectors embedded in first body
dxJointLimitMotor limot1; // limit+motor info for axis 1
dxJointLimitMotor limot2; // limit+motor info for axis 2
dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm)
};
extern struct dxJoint::Vtable __dhinge2_vtable;
// angular motor
struct dxJointAMotor : public dxJoint {
int num; // number of axes (0..3)
int mode; // a dAMotorXXX constant
int rel[3]; // what the axes are relative to (global,b1,b2)
dVector3 axis[3]; // three axes
dxJointLimitMotor limot[3]; // limit+motor info for axes
dReal angle[3]; // user-supplied angles for axes
// these vectors are used for calculating euler angles
dVector3 reference1; // original axis[2], relative to body 1
dVector3 reference2; // original axis[0], relative to body 2
};
extern struct dxJoint::Vtable __damotor_vtable;
struct dxJointLMotor : public dxJoint {
int num;
int rel[3];
dVector3 axis[3];
dxJointLimitMotor limot[3];
};
extern struct dxJoint::Vtable __dlmotor_vtable;
// 2d joint, constrains to z == 0
struct dxJointPlane2D : public dxJoint
{
int row_motor_x;
int row_motor_y;
int row_motor_angle;
dxJointLimitMotor motor_x;
dxJointLimitMotor motor_y;
dxJointLimitMotor motor_angle;
};
extern struct dxJoint::Vtable __dplane2d_vtable;
// fixed
struct dxJointFixed : public dxJoint {
dQuaternion qrel; // initial relative rotation body1 -> body2
dVector3 offset; // relative offset between the bodies
};
extern struct dxJoint::Vtable __dfixed_vtable;
// null joint, for testing only
struct dxJointNull : public dxJoint {
};
extern struct dxJoint::Vtable __dnull_vtable;
#endif

2007
ode/src/lcp.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

58
ode/src/lcp.h Normal file
View File

@ -0,0 +1,58 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
satisfies one of
(1) x = lo, w >= 0
(2) x = hi, w <= 0
(3) lo < x < hi, w = 0
A is a matrix of dimension n*n, everything else is a vector of size n*1.
lo and hi can be +/- dInfinity as needed. the first `nub' variables are
unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
we restrict lo(i) <= 0 and hi(i) >= 0.
the original data (A,b) may be modified by this function.
if the `findex' (friction index) parameter is nonzero, it points to an array
of index values. in this case constraints that have findex[i] >= 0 are
special. all non-special constraints are solved for, then the lo and hi values
for the special constraints are set:
hi[i] = abs( hi[i] * x[findex[i]] )
lo[i] = -hi[i]
and the solution continues. this mechanism allows a friction approximation
to be implemented. the first `nub' variables are assumed to have findex < 0.
*/
#ifndef _ODE_LCP_H_
#define _ODE_LCP_H_
void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w,
int nub, dReal *lo, dReal *hi, int *findex);
#endif

517
ode/src/mass.cpp Normal file
View File

@ -0,0 +1,517 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/config.h>
#include <ode/mass.h>
#include <ode/odemath.h>
#include <ode/matrix.h>
// Local dependencies
#include "collision_kernel.h"
#define SQR(x) ((x)*(x)) //!< Returns x square
#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
#define _I(i,j) I[(i)*4+(j)]
// return 1 if ok, 0 if bad
int dMassCheck (const dMass *m)
{
int i;
if (m->mass <= 0) {
dDEBUGMSG ("mass must be > 0");
return 0;
}
if (!dIsPositiveDefinite (m->I,3)) {
dDEBUGMSG ("inertia must be positive definite");
return 0;
}
// verify that the center of mass position is consistent with the mass
// and inertia matrix. this is done by checking that the inertia around
// the center of mass is also positive definite. from the comment in
// dMassTranslate(), if the body is translated so that its center of mass
// is at the point of reference, then the new inertia is:
// I + mass*crossmat(c)^2
// note that requiring this to be positive definite is exactly equivalent
// to requiring that the spatial inertia matrix
// [ mass*eye(3,3) M*crossmat(c)^T ]
// [ M*crossmat(c) I ]
// is positive definite, given that I is PD and mass>0. see the theorem
// about partitioned PD matrices for proof.
dMatrix3 I2,chat;
dSetZero (chat,12);
dCROSSMAT (chat,m->c,4,+,-);
dMULTIPLY0_333 (I2,chat,chat);
for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i];
for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i];
for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i];
if (!dIsPositiveDefinite (I2,3)) {
dDEBUGMSG ("center of mass inconsistent with mass parameters");
return 0;
}
return 1;
}
void dMassSetZero (dMass *m)
{
dAASSERT (m);
m->mass = REAL(0.0);
dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
}
void dMassSetParameters (dMass *m, dReal themass,
dReal cgx, dReal cgy, dReal cgz,
dReal I11, dReal I22, dReal I33,
dReal I12, dReal I13, dReal I23)
{
dAASSERT (m);
dMassSetZero (m);
m->mass = themass;
m->c[0] = cgx;
m->c[1] = cgy;
m->c[2] = cgz;
m->_I(0,0) = I11;
m->_I(1,1) = I22;
m->_I(2,2) = I33;
m->_I(0,1) = I12;
m->_I(0,2) = I13;
m->_I(1,2) = I23;
m->_I(1,0) = I12;
m->_I(2,0) = I13;
m->_I(2,1) = I23;
dMassCheck (m);
}
void dMassSetSphere (dMass *m, dReal density, dReal radius)
{
dMassSetSphereTotal (m, (REAL(4.0)/REAL(3.0)) * M_PI *
radius*radius*radius * density, radius);
}
void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius)
{
dAASSERT (m);
dMassSetZero (m);
m->mass = total_mass;
dReal II = REAL(0.4) * total_mass * radius*radius;
m->_I(0,0) = II;
m->_I(1,1) = II;
m->_I(2,2) = II;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassSetCapsule (dMass *m, dReal density, int direction,
dReal radius, dReal length)
{
dReal M1,M2,Ia,Ib;
dAASSERT (m);
dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
dMassSetZero (m);
M1 = M_PI*radius*radius*length*density; // cylinder mass
M2 = (REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density; // total cap mass
m->mass = M1+M2;
Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) +
M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length);
Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius;
m->_I(0,0) = Ia;
m->_I(1,1) = Ia;
m->_I(2,2) = Ia;
m->_I(direction-1,direction-1) = Ib;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction,
dReal a, dReal b)
{
dMassSetCapsule (m, 1.0, direction, a, b);
dMassAdjust (m, total_mass);
}
void dMassSetCylinder (dMass *m, dReal density, int direction,
dReal radius, dReal length)
{
dMassSetCylinderTotal (m, M_PI*radius*radius*length*density,
direction, radius, length);
}
void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction,
dReal radius, dReal length)
{
dReal r2,I;
dAASSERT (m);
dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
dMassSetZero (m);
r2 = radius*radius;
m->mass = total_mass;
I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length);
m->_I(0,0) = I;
m->_I(1,1) = I;
m->_I(2,2) = I;
m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassSetBox (dMass *m, dReal density,
dReal lx, dReal ly, dReal lz)
{
dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz);
}
void dMassSetBoxTotal (dMass *m, dReal total_mass,
dReal lx, dReal ly, dReal lz)
{
dAASSERT (m);
dMassSetZero (m);
m->mass = total_mass;
m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz);
m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz);
m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly);
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
#if dTRIMESH_ENABLED
/*
* dMassSetTrimesh, implementation by Gero Mueller.
* Based on Brian Mirtich, "Fast and Accurate Computation of
* Polyhedral Mass Properties," journal of graphics tools, volume 1,
* number 2, 1996.
*/
void dMassSetTrimesh( dMass *m, dReal density, dGeomID g )
{
dAASSERT (m);
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dMassSetZero (m);
unsigned int triangles = dGeomTriMeshGetTriangleCount( g );
dReal nx, ny, nz;
unsigned int i, A, B, C;
// face integrals
dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
// projection integrals
dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
dReal T0 = 0;
dReal T1[3] = {0., 0., 0.};
dReal T2[3] = {0., 0., 0.};
dReal TP[3] = {0., 0., 0.};
for( i = 0; i < triangles; i++ )
{
dVector3 v0, v1, v2;
dGeomTriMeshGetTriangle( g, i, &v0, &v1, &v2);
dVector3 n, a, b;
dOP( a, -, v1, v0 );
dOP( b, -, v2, v0 );
dCROSS( n, =, b, a );
nx = fabs(n[0]);
ny = fabs(n[1]);
nz = fabs(n[2]);
if( nx > ny && nx > nz )
C = 0;
else
C = (ny > nz) ? 1 : 2;
A = (C + 1) % 3;
B = (A + 1) % 3;
// calculate face integrals
{
dReal w;
dReal k1, k2, k3, k4;
//compProjectionIntegrals(f);
{
dReal a0, a1, da;
dReal b0, b1, db;
dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
dReal a1_2, a1_3, b1_2, b1_3;
dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
dReal Cab, Kab, Caab, Kaab, Cabb, Kabb;
P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
for( int j = 0; j < 3; j++)
{
switch(j)
{
case 0:
a0 = v0[A];
b0 = v0[B];
a1 = v1[A];
b1 = v1[B];
break;
case 1:
a0 = v1[A];
b0 = v1[B];
a1 = v2[A];
b1 = v2[B];
break;
case 2:
a0 = v2[A];
b0 = v2[B];
a1 = v0[A];
b1 = v0[B];
break;
}
da = a1 - a0;
db = b1 - b0;
a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
a1_2 = a1 * a1; a1_3 = a1_2 * a1;
b1_2 = b1 * b1; b1_3 = b1_2 * b1;
C1 = a1 + a0;
Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
P1 += db*C1;
Pa += db*Ca;
Paa += db*Caa;
Paaa += db*Caaa;
Pb += da*Cb;
Pbb += da*Cbb;
Pbbb += da*Cbbb;
Pab += db*(b1*Cab + b0*Kab);
Paab += db*(b1*Caab + b0*Kaab);
Pabb += da*(a1*Cabb + a0*Kabb);
}
P1 /= 2.0;
Pa /= 6.0;
Paa /= 12.0;
Paaa /= 20.0;
Pb /= -6.0;
Pbb /= -12.0;
Pbbb /= -20.0;
Pab /= 24.0;
Paab /= 60.0;
Pabb /= -60.0;
}
w = - dDOT(n, v0);
k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
Fa = k1 * Pa;
Fb = k1 * Pb;
Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
Faa = k1 * Paa;
Fbb = k1 * Pbb;
Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb +
w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
Faaa = k1 * Paaa;
Fbbb = k1 * Pbbb;
Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
+ 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
+ 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
+ w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
Faab = k1 * Paab;
Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
+ w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
}
T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));
T1[A] += n[A] * Faa;
T1[B] += n[B] * Fbb;
T1[C] += n[C] * Fcc;
T2[A] += n[A] * Faaa;
T2[B] += n[B] * Fbbb;
T2[C] += n[C] * Fccc;
TP[A] += n[A] * Faab;
TP[B] += n[B] * Fbbc;
TP[C] += n[C] * Fcca;
}
T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
m->mass = density * T0;
m->_I(0,0) = density * (T2[1] + T2[2]);
m->_I(1,1) = density * (T2[2] + T2[0]);
m->_I(2,2) = density * (T2[0] + T2[1]);
m->_I(0,1) = - density * TP[0];
m->_I(1,0) = - density * TP[0];
m->_I(2,1) = - density * TP[1];
m->_I(1,2) = - density * TP[1];
m->_I(2,0) = - density * TP[2];
m->_I(0,2) = - density * TP[2];
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
#endif // dTRIMESH_ENABLED
void dMassAdjust (dMass *m, dReal newmass)
{
dAASSERT (m);
dReal scale = newmass / m->mass;
m->mass = newmass;
for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
{
// if the body is translated by `a' relative to its point of reference,
// the new inertia about the point of reference is:
//
// I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
//
// where c is the existing center of mass and I is the old inertia.
int i,j;
dMatrix3 ahat,chat,t1,t2;
dReal a[3];
dAASSERT (m);
// adjust inertia matrix
dSetZero (chat,12);
dCROSSMAT (chat,m->c,4,+,-);
a[0] = x + m->c[0];
a[1] = y + m->c[1];
a[2] = z + m->c[2];
dSetZero (ahat,12);
dCROSSMAT (ahat,a,4,+,-);
dMULTIPLY0_333 (t1,ahat,ahat);
dMULTIPLY0_333 (t2,chat,chat);
for (i=0; i<3; i++) for (j=0; j<3; j++)
m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
// ensure perfect symmetry
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
// adjust center of mass
m->c[0] += x;
m->c[1] += y;
m->c[2] += z;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassRotate (dMass *m, const dMatrix3 R)
{
// if the body is rotated by `R' relative to its point of reference,
// the new inertia about the point of reference is:
//
// R * I * R'
//
// where I is the old inertia.
dMatrix3 t1;
dReal t2[3];
dAASSERT (m);
// rotate inertia matrix
dMULTIPLY2_333 (t1,m->I,R);
dMULTIPLY0_333 (m->I,R,t1);
// ensure perfect symmetry
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
// rotate center of mass
dMULTIPLY0_331 (t2,R,m->c);
m->c[0] = t2[0];
m->c[1] = t2[1];
m->c[2] = t2[2];
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassAdd (dMass *a, const dMass *b)
{
int i;
dAASSERT (a && b);
dReal denom = dRecip (a->mass + b->mass);
for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
a->mass += b->mass;
for (i=0; i<12; i++) a->I[i] += b->I[i];
}

230
ode/src/mat.cpp Normal file
View File

@ -0,0 +1,230 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/config.h>
#include <ode/misc.h>
#include <ode/matrix.h>
#include <ode/error.h>
#include <ode/memory.h>
#include "mat.h"
dMatrix::dMatrix()
{
n = 0;
m = 0;
data = 0;
}
dMatrix::dMatrix (int rows, int cols)
{
if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
n = rows;
m = cols;
data = (dReal*) dAlloc (n*m*sizeof(dReal));
dSetZero (data,n*m);
}
dMatrix::dMatrix (const dMatrix &a)
{
n = a.n;
m = a.m;
data = (dReal*) dAlloc (n*m*sizeof(dReal));
memcpy (data,a.data,n*m*sizeof(dReal));
}
dMatrix::dMatrix (int rows, int cols,
dReal *_data, int rowskip, int colskip)
{
if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
n = rows;
m = cols;
data = (dReal*) dAlloc (n*m*sizeof(dReal));
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip];
}
}
dMatrix::~dMatrix()
{
if (data) dFree (data,n*m*sizeof(dReal));
}
dReal & dMatrix::operator () (int i, int j)
{
if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)");
return data [i*m+j];
}
void dMatrix::operator= (const dMatrix &a)
{
if (data) dFree (data,n*m*sizeof(dReal));
n = a.n;
m = a.m;
if (n > 0 && m > 0) {
data = (dReal*) dAlloc (n*m*sizeof(dReal));
memcpy (data,a.data,n*m*sizeof(dReal));
}
else data = 0;
}
void dMatrix::operator= (dReal a)
{
for (int i=0; i<n*m; i++) data[i] = a;
}
dMatrix dMatrix::transpose()
{
dMatrix r (m,n);
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j];
}
return r;
}
dMatrix dMatrix::select (int np, int *p, int nq, int *q)
{
if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes");
dMatrix r (np,nq);
for (int i=0; i<np; i++) {
for (int j=0; j<nq; j++) {
if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m)
dDebug (0,"Matrix select, bad index arrays");
r.data[i*nq+j] = data[p[i]*m+q[j]];
}
}
return r;
}
dMatrix dMatrix::operator + (const dMatrix &a)
{
if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes");
dMatrix r (n,m);
for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i];
return r;
}
dMatrix dMatrix::operator - (const dMatrix &a)
{
if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes");
dMatrix r (n,m);
for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i];
return r;
}
dMatrix dMatrix::operator - ()
{
dMatrix r (n,m);
for (int i=0; i<n*m; i++) r.data[i] = -data[i];
return r;
}
dMatrix dMatrix::operator * (const dMatrix &a)
{
if (m != a.n) dDebug (0,"matrix *, mismatched sizes");
dMatrix r (n,a.m);
for (int i=0; i<n; i++) {
for (int j=0; j<a.m; j++) {
dReal sum = 0;
for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j];
r.data [i*a.m+j] = sum;
}
}
return r;
}
void dMatrix::operator += (const dMatrix &a)
{
if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes");
for (int i=0; i<n*m; i++) data[i] += a.data[i];
}
void dMatrix::operator -= (const dMatrix &a)
{
if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes");
for (int i=0; i<n*m; i++) data[i] -= a.data[i];
}
void dMatrix::clearUpperTriangle()
{
if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices");
for (int i=0; i<n; i++) {
for (int j=i+1; j<m; j++) data[i*m+j] = 0;
}
}
void dMatrix::clearLowerTriangle()
{
if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices");
for (int i=0; i<n; i++) {
for (int j=0; j<i; j++) data[i*m+j] = 0;
}
}
void dMatrix::makeRandom (dReal range)
{
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++)
data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
}
}
void dMatrix::print (char *fmt, FILE *f)
{
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]);
fprintf (f,"\n");
}
}
dReal dMatrix::maxDifference (const dMatrix &a)
{
if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes");
dReal max = 0;
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++) {
dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]);
if (diff > max) max = diff;
}
}
return max;
}

71
ode/src/mat.h Normal file
View File

@ -0,0 +1,71 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
// matrix class. this is mostly for convenience in the testing code, it is
// not optimized at all. correctness is much more importance here.
#ifndef _ODE_MAT_H_
#define _ODE_MAT_H_
#include <ode/common.h>
class dMatrix {
int n,m; // matrix dimension, n,m >= 0
dReal *data; // if nonzero, n*m elements allocated on the heap
public:
// constructors, destructors
dMatrix(); // make default 0x0 matrix
dMatrix (int rows, int cols); // construct zero matrix of given size
dMatrix (const dMatrix &); // construct copy of given matrix
// create copy of given data - element (i,j) is data[i*rowskip+j*colskip]
dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip);
~dMatrix(); // destructor
// data movement
dReal & operator () (int i, int j); // reference an element
void operator= (const dMatrix &); // matrix = matrix
void operator= (dReal); // matrix = scalar
dMatrix transpose(); // return transposed matrix
// return a permuted submatrix of this matrix, made up of the rows in p
// and the columns in q. p has np elements, q has nq elements.
dMatrix select (int np, int *p, int nq, int *q);
// operators
dMatrix operator + (const dMatrix &);
dMatrix operator - (const dMatrix &);
dMatrix operator - ();
dMatrix operator * (const dMatrix &);
void operator += (const dMatrix &);
void operator -= (const dMatrix &);
// utility
void clearUpperTriangle();
void clearLowerTriangle();
void makeRandom (dReal range);
void print (char *fmt = "%10.4f ", FILE *f=stdout);
dReal maxDifference (const dMatrix &);
};
#endif

358
ode/src/matrix.cpp Normal file
View File

@ -0,0 +1,358 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/common.h>
#include <ode/matrix.h>
// misc defines
#define ALLOCA dALLOCA16
void dSetZero (dReal *a, int n)
{
dAASSERT (a && n >= 0);
while (n > 0) {
*(a++) = 0;
n--;
}
}
void dSetValue (dReal *a, int n, dReal value)
{
dAASSERT (a && n >= 0);
while (n > 0) {
*(a++) = value;
n--;
}
}
void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
{
int i,j,k,qskip,rskip,rpad;
dAASSERT (A && B && C && p>0 && q>0 && r>0);
qskip = dPAD(q);
rskip = dPAD(r);
rpad = rskip - r;
dReal sum;
const dReal *b,*c,*bb;
bb = B;
for (i=p; i; i--) {
for (j=0 ; j<r; j++) {
c = C + j;
b = bb;
sum = 0;
for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c);
*(A++) = sum;
}
A += rpad;
bb += qskip;
}
}
void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
{
int i,j,k,pskip,rskip;
dReal sum;
dAASSERT (A && B && C && p>0 && q>0 && r>0);
pskip = dPAD(p);
rskip = dPAD(r);
for (i=0; i<p; i++) {
for (j=0; j<r; j++) {
sum = 0;
for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip];
A[i*rskip+j] = sum;
}
}
}
void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
{
int i,j,k,z,rpad,qskip;
dReal sum;
const dReal *bb,*cc;
dAASSERT (A && B && C && p>0 && q>0 && r>0);
rpad = dPAD(r) - r;
qskip = dPAD(q);
bb = B;
for (i=p; i; i--) {
cc = C;
for (j=r; j; j--) {
z = 0;
sum = 0;
for (k=q; k; k--,z++) sum += bb[z] * cc[z];
*(A++) = sum;
cc += qskip;
}
A += rpad;
bb += qskip;
}
}
int dFactorCholesky (dReal *A, int n)
{
int i,j,k,nskip;
dReal sum,*a,*b,*aa,*bb,*cc,*recip;
dAASSERT (n > 0 && A);
nskip = dPAD (n);
recip = (dReal*) ALLOCA (n * sizeof(dReal));
aa = A;
for (i=0; i<n; i++) {
bb = A;
cc = A + i*nskip;
for (j=0; j<i; j++) {
sum = *cc;
a = aa;
b = bb;
for (k=j; k; k--) sum -= (*(a++))*(*(b++));
*cc = sum * recip[j];
bb += nskip;
cc++;
}
sum = *cc;
a = aa;
for (k=i; k; k--, a++) sum -= (*a)*(*a);
if (sum <= REAL(0.0)) return 0;
*cc = dSqrt(sum);
recip[i] = dRecip (*cc);
aa += nskip;
}
return 1;
}
void dSolveCholesky (const dReal *L, dReal *b, int n)
{
int i,k,nskip;
dReal sum,*y;
dAASSERT (n > 0 && L && b);
nskip = dPAD (n);
y = (dReal*) ALLOCA (n*sizeof(dReal));
for (i=0; i<n; i++) {
sum = 0;
for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k];
y[i] = (b[i]-sum)/L[i*nskip+i];
}
for (i=n-1; i >= 0; i--) {
sum = 0;
for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k];
b[i] = (y[i]-sum)/L[i*nskip+i];
}
}
int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n)
{
int i,j,nskip;
dReal *L,*x;
dAASSERT (n > 0 && A && Ainv);
nskip = dPAD (n);
L = (dReal*) ALLOCA (nskip*n*sizeof(dReal));
memcpy (L,A,nskip*n*sizeof(dReal));
x = (dReal*) ALLOCA (n*sizeof(dReal));
if (dFactorCholesky (L,n)==0) return 0;
dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0
for (i=0; i<n; i++) {
for (j=0; j<n; j++) x[j] = 0;
x[i] = 1;
dSolveCholesky (L,x,n);
for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j];
}
return 1;
}
int dIsPositiveDefinite (const dReal *A, int n)
{
dReal *Acopy;
dAASSERT (n > 0 && A);
int nskip = dPAD (n);
Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal));
memcpy (Acopy,A,nskip*n * sizeof(dReal));
return dFactorCholesky (Acopy,n);
}
/***** this has been replaced by a faster version
void dSolveL1T (const dReal *L, dReal *b, int n, int nskip)
{
int i,j;
dAASSERT (L && b && n >= 0 && nskip >= n);
dReal sum;
for (i=n-2; i>=0; i--) {
sum = 0;
for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j];
b[i] -= sum;
}
}
*/
void dVectorScale (dReal *a, const dReal *d, int n)
{
dAASSERT (a && d && n >= 0);
for (int i=0; i<n; i++) a[i] *= d[i];
}
void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip)
{
dAASSERT (L && d && b && n > 0 && nskip >= n);
dSolveL1 (L,b,n,nskip);
dVectorScale (b,d,n);
dSolveL1T (L,b,n,nskip);
}
void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip)
{
int j,p;
dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee;
dAASSERT (L && d && a && n > 0 && nskip >= n);
if (n < 2) return;
W1 = (dReal*) ALLOCA (n*sizeof(dReal));
W2 = (dReal*) ALLOCA (n*sizeof(dReal));
W1[0] = 0;
W2[0] = 0;
for (j=1; j<n; j++) W1[j] = W2[j] = a[j] * M_SQRT1_2;
W11 = (REAL(0.5)*a[0]+1)*M_SQRT1_2;
W21 = (REAL(0.5)*a[0]-1)*M_SQRT1_2;
alpha1=1;
alpha2=1;
dee = d[0];
alphanew = alpha1 + (W11*W11)*dee;
dee /= alphanew;
gamma1 = W11 * dee;
dee *= alpha1;
alpha1 = alphanew;
alphanew = alpha2 - (W21*W21)*dee;
dee /= alphanew;
gamma2 = W21 * dee;
alpha2 = alphanew;
k1 = REAL(1.0) - W21*gamma1;
k2 = W21*gamma1*W11 - W21;
for (p=1; p<n; p++) {
Wp = W1[p];
ell = L[p*nskip];
W1[p] = Wp - W11*ell;
W2[p] = k1*Wp + k2*ell;
}
for (j=1; j<n; j++) {
dee = d[j];
alphanew = alpha1 + (W1[j]*W1[j])*dee;
dee /= alphanew;
gamma1 = W1[j] * dee;
dee *= alpha1;
alpha1 = alphanew;
alphanew = alpha2 - (W2[j]*W2[j])*dee;
dee /= alphanew;
gamma2 = W2[j] * dee;
dee *= alpha2;
d[j] = dee;
alpha2 = alphanew;
k1 = W1[j];
k2 = W2[j];
for (p=j+1; p<n; p++) {
ell = L[p*nskip+j];
Wp = W1[p] - k1 * ell;
ell += gamma1 * Wp;
W1[p] = Wp;
Wp = W2[p] - k2 * ell;
ell -= gamma2 * Wp;
W2[p] = Wp;
L[p*nskip+j] = ell;
}
}
}
// macros for dLDLTRemove() for accessing A - either access the matrix
// directly or access it via row pointers. we are only supposed to reference
// the lower triangle of A (it is symmetric), but indexes i and j come from
// permutation vectors so they are not predictable. so do a test on the
// indexes - this should not slow things down too much, as we don't do this
// in an inner loop.
#define _GETA(i,j) (A[i][j])
//#define _GETA(i,j) (A[(i)*nskip+(j)])
#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i))
void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
int n1, int n2, int r, int nskip)
{
int i;
dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
n1 >= n2 && nskip >= n1);
#ifndef dNODEBUG
for (i=0; i<n2; i++) dIASSERT(p[i] >= 0 && p[i] < n1);
#endif
if (r==n2-1) {
return; // deleting last row/col is easy
}
else if (r==0) {
dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal));
for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]);
a[0] += REAL(1.0);
dLDLTAddTL (L,d,a,n2,nskip);
}
else {
dReal *t = (dReal*) ALLOCA (r * sizeof(dReal));
dReal *a = (dReal*) ALLOCA ((n2-r) * sizeof(dReal));
for (i=0; i<r; i++) t[i] = L[r*nskip+i] / d[i];
for (i=0; i<(n2-r); i++)
a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]);
a[0] += REAL(1.0);
dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip);
}
// snip out row/column r from L and d
dRemoveRowCol (L,n2,nskip,r);
if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal));
}
void dRemoveRowCol (dReal *A, int n, int nskip, int r)
{
int i;
dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n);
if (r >= n-1) return;
if (r > 0) {
for (i=0; i<r; i++)
memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal));
for (i=r; i<(n-1); i++)
memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal));
}
for (i=r; i<(n-1); i++)
memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal));
}

87
ode/src/memory.cpp Normal file
View File

@ -0,0 +1,87 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/config.h>
#include <ode/memory.h>
#include <ode/error.h>
static dAllocFunction *allocfn = 0;
static dReallocFunction *reallocfn = 0;
static dFreeFunction *freefn = 0;
void dSetAllocHandler (dAllocFunction *fn)
{
allocfn = fn;
}
void dSetReallocHandler (dReallocFunction *fn)
{
reallocfn = fn;
}
void dSetFreeHandler (dFreeFunction *fn)
{
freefn = fn;
}
dAllocFunction *dGetAllocHandler()
{
return allocfn;
}
dReallocFunction *dGetReallocHandler()
{
return reallocfn;
}
dFreeFunction *dGetFreeHandler()
{
return freefn;
}
void * dAlloc (size_t size)
{
if (allocfn) return allocfn (size); else return malloc (size);
}
void * dRealloc (void *ptr, size_t oldsize, size_t newsize)
{
if (reallocfn) return reallocfn (ptr,oldsize,newsize);
else return realloc (ptr,newsize);
}
void dFree (void *ptr, size_t size)
{
if (!ptr) return;
if (freefn) freefn (ptr,size); else free (ptr);
}

169
ode/src/misc.cpp Normal file
View File

@ -0,0 +1,169 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/config.h>
#include <ode/misc.h>
#include <ode/matrix.h>
//****************************************************************************
// random numbers
static unsigned long seed = 0;
unsigned long dRand()
{
seed = (1664525L*seed + 1013904223L) & 0xffffffff;
return seed;
}
unsigned long dRandGetSeed()
{
return seed;
}
void dRandSetSeed (unsigned long s)
{
seed = s;
}
int dTestRand()
{
unsigned long oldseed = seed;
int ret = 1;
seed = 0;
if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
dRand() != 0x6252e503) ret = 0;
seed = oldseed;
return ret;
}
// adam's all-int straightforward(?) dRandInt (0..n-1)
int dRandInt (int n)
{
// seems good; xor-fold and modulus
const unsigned long un = n;
unsigned long r = dRand();
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
if (un <= 0x00010000UL) {
r ^= (r >> 16);
if (un <= 0x00000100UL) {
r ^= (r >> 8);
if (un <= 0x00000010UL) {
r ^= (r >> 4);
if (un <= 0x00000004UL) {
r ^= (r >> 2);
if (un <= 0x00000002UL) {
r ^= (r >> 1);
}
}
}
}
}
return (int) (r % un);
}
dReal dRandReal()
{
return ((dReal) dRand()) / ((dReal) 0xffffffff);
}
//****************************************************************************
// matrix utility stuff
void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f)
{
int i,j;
int skip = dPAD(m);
for (i=0; i<n; i++) {
for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);
fprintf (f,"\n");
}
}
void dMakeRandomVector (dReal *A, int n, dReal range)
{
int i;
for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
}
void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
{
int i,j;
int skip = dPAD(m);
dSetZero (A,n*skip);
for (i=0; i<n; i++) {
for (j=0; j<m; j++) A[i*skip+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
}
}
void dClearUpperTriangle (dReal *A, int n)
{
int i,j;
int skip = dPAD(n);
for (i=0; i<n; i++) {
for (j=i+1; j<n; j++) A[i*skip+j] = 0;
}
}
dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
{
int i,j;
int skip = dPAD(m);
dReal diff,max;
max = 0;
for (i=0; i<n; i++) {
for (j=0; j<m; j++) {
diff = dFabs(A[i*skip+j] - B[i*skip+j]);
if (diff > max) max = diff;
}
}
return max;
}
dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
{
int i,j;
int skip = dPAD(n);
dReal diff,max;
max = 0;
for (i=0; i<n; i++) {
for (j=0; j<=i; j++) {
diff = dFabs(A[i*skip+j] - B[i*skip+j]);
if (diff > max) max = diff;
}
}
return max;
}

138
ode/src/objects.h Normal file
View File

@ -0,0 +1,138 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
// object, body, and world structs.
#ifndef _ODE_OBJECT_H_
#define _ODE_OBJECT_H_
#include <ode/common.h>
#include <ode/memory.h>
#include <ode/mass.h>
#include "array.h"
// some body flags
enum {
dxBodyFlagFiniteRotation = 1, // use finite rotations
dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis
dxBodyDisabled = 4, // body is disabled
dxBodyNoGravity = 8, // body is not influenced by gravity
dxBodyAutoDisable = 16 // enable auto-disable on body
};
// base class that does correct object allocation / deallocation
struct dBase {
void *operator new (size_t size) { return dAlloc (size); }
void operator delete (void *ptr, size_t size) { dFree (ptr,size); }
void *operator new[] (size_t size) { return dAlloc (size); }
void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); }
};
// base class for bodies and joints
struct dObject : public dBase {
dxWorld *world; // world this object is in
dObject *next; // next object of this type in list
dObject **tome; // pointer to previous object's next ptr
void *userdata; // user settable data
int tag; // used by dynamics algorithms
};
// auto disable parameters
struct dxAutoDisable {
dReal idle_time; // time the body needs to be idle to auto-disable it
int idle_steps; // steps the body needs to be idle to auto-disable it
dReal linear_average_threshold; // linear (squared) average velocity threshold
dReal angular_average_threshold; // angular (squared) average velocity threshold
unsigned int average_samples; // size of the average_lvel and average_avel buffers
};
// quick-step parameters
struct dxQuickStepParameters {
int num_iterations; // number of SOR iterations to perform
dReal w; // the SOR over-relaxation parameter
};
// contact generation parameters
struct dxContactParameters {
dReal max_vel; // maximum correcting velocity
dReal min_depth; // thickness of 'surface layer'
};
// position vector and rotation matrix for geometry objects that are not
// connected to bodies.
struct dxPosR {
dVector3 pos;
dMatrix3 R;
};
struct dxBody : public dObject {
dxJointNode *firstjoint; // list of attached joints
int flags; // some dxBodyFlagXXX flags
dGeomID geom; // first collision geom associated with body
dMass mass; // mass parameters about POR
dMatrix3 invI; // inverse of mass.I
dReal invMass; // 1 / mass.mass
dxPosR posr; // position and orientation of point of reference
dQuaternion q; // orientation quaternion
dVector3 lvel,avel; // linear and angular velocity of POR
dVector3 facc,tacc; // force and torque accumulators
dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none
// auto-disable information
dxAutoDisable adis; // auto-disable parameters
dReal adis_timeleft; // time left to be idle
int adis_stepsleft; // steps left to be idle
dVector3* average_lvel_buffer; // buffer for the linear average velocity calculation
dVector3* average_avel_buffer; // buffer for the angular average velocity calculation
unsigned int average_counter; // counter/index to fill the average-buffers
int average_ready; // indicates ( with = 1 ), if the Body's buffers are ready for average-calculations
};
struct dxWorld : public dBase {
dxBody *firstbody; // body linked list
dxJoint *firstjoint; // joint linked list
int nb,nj; // number of bodies and joints in lists
dVector3 gravity; // gravity vector (m/s/s)
dReal global_erp; // global error reduction parameter
dReal global_cfm; // global costraint force mixing parameter
dxAutoDisable adis; // auto-disable parameters
int adis_flag; // auto-disable flag for new bodies
dxQuickStepParameters qs;
dxContactParameters contactp;
};
#endif

130
ode/src/obstack.cpp Normal file
View File

@ -0,0 +1,130 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/common.h>
#include <ode/error.h>
#include <ode/memory.h>
#include "obstack.h"
//****************************************************************************
// macros and constants
#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \
ofs = (size_t) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) );
#define MAX_ALLOC_SIZE \
((size_t)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1))
//****************************************************************************
// dObStack
dObStack::dObStack()
{
first = 0;
last = 0;
current_arena = 0;
current_ofs = 0;
}
dObStack::~dObStack()
{
// free all arenas
Arena *a,*nexta;
a = first;
while (a) {
nexta = a->next;
dFree (a,dOBSTACK_ARENA_SIZE);
a = nexta;
}
}
void *dObStack::alloc (int num_bytes)
{
if ((size_t)num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large");
// allocate or move to a new arena if necessary
if (!first) {
// allocate the first arena if necessary
first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
first->next = 0;
first->used = sizeof (Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
}
else {
// we already have one or more arenas, see if a new arena must be used
if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) {
if (!last->next) {
last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
last->next->next = 0;
}
last = last->next;
last->used = sizeof (Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
}
}
// allocate an area in the arena
char *c = ((char*) last) + last->used;
last->used += num_bytes;
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
return c;
}
void dObStack::freeAll()
{
last = first;
if (first) {
first->used = sizeof(Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
}
}
void *dObStack::rewind()
{
current_arena = first;
current_ofs = sizeof (Arena);
if (current_arena) {
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs)
return ((char*) current_arena) + current_ofs;
}
else return 0;
}
void *dObStack::next (int num_bytes)
{
// this functions like alloc, except that no new storage is ever allocated
if (!current_arena) return 0;
current_ofs += num_bytes;
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
if (current_ofs >= current_arena->used) {
current_arena = current_arena->next;
if (!current_arena) return 0;
current_ofs = sizeof (Arena);
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
}
return ((char*) current_arena) + current_ofs;
}

68
ode/src/obstack.h Normal file
View File

@ -0,0 +1,68 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#ifndef _ODE_OBSTACK_H_
#define _ODE_OBSTACK_H_
#include "objects.h"
// each obstack Arena pointer points to a block of this many bytes
#define dOBSTACK_ARENA_SIZE 16384
struct dObStack : public dBase {
struct Arena {
Arena *next; // next arena in linked list
size_t used; // total number of bytes used in this arena, counting
}; // this header
Arena *first; // head of the arena linked list. 0 if no arenas yet
Arena *last; // arena where blocks are currently being allocated
// used for iterator
Arena *current_arena;
size_t current_ofs;
dObStack();
~dObStack();
void *alloc (int num_bytes);
// allocate a block in the last arena, allocating a new arena if necessary.
// it is a runtime error if num_bytes is larger than the arena size.
void freeAll();
// free all blocks in all arenas. this does not deallocate the arenas
// themselves, so future alloc()s will reuse them.
void *rewind();
// rewind the obstack iterator, and return the address of the first
// allocated block. return 0 if there are no allocated blocks.
void *next (int num_bytes);
// return the address of the next allocated block. 'num_bytes' is the size
// of the previous block. this returns null if there are no more arenas.
// the sequence of 'num_bytes' parameters passed to next() during a
// traversal of the list must exactly match the parameters passed to alloc().
};
#endif

1732
ode/src/ode.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

165
ode/src/odemath.cpp Normal file
View File

@ -0,0 +1,165 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include <ode/common.h>
#include <ode/odemath.h>
// get some math functions under windows
#ifdef WIN32
#include <float.h>
#ifndef CYGWIN // added by andy for cygwin
#undef copysign
#define copysign(a,b) ((dReal)_copysign(a,b))
#endif // added by andy for cygwin
#endif
// this may be called for vectors `a' with extremely small magnitude, for
// example the result of a cross product on two nearly perpendicular vectors.
// we must be robust to these small vectors. to prevent numerical error,
// first find the component a[i] with the largest magnitude and then scale
// all the components by 1/a[i]. then we can compute the length of `a' and
// scale the components by 1/l. this has been verified to work with vectors
// containing the smallest representable numbers.
void dNormalize3 (dVector3 a)
{
dReal a0,a1,a2,aa0,aa1,aa2,l;
dAASSERT (a);
a0 = a[0];
a1 = a[1];
a2 = a[2];
aa0 = dFabs(a0);
aa1 = dFabs(a1);
aa2 = dFabs(a2);
if (aa1 > aa0) {
if (aa2 > aa1) {
goto aa2_largest;
}
else { // aa1 is largest
a0 /= aa1;
a2 /= aa1;
l = dRecipSqrt (a0*a0 + a2*a2 + 1);
a[0] = a0*l;
a[1] = dCopySign(l,a1);
a[2] = a2*l;
}
}
else {
if (aa2 > aa0) {
aa2_largest: // aa2 is largest
a0 /= aa2;
a1 /= aa2;
l = dRecipSqrt (a0*a0 + a1*a1 + 1);
a[0] = a0*l;
a[1] = a1*l;
a[2] = dCopySign(l,a2);
}
else { // aa0 is largest
if (aa0 <= 0) {
// dDEBUGMSG ("vector has zero size"); ... this messace is annoying
a[0] = 1; // if all a's are zero, this is where we'll end up.
a[1] = 0; // return a default unit length vector.
a[2] = 0;
return;
}
a1 /= aa0;
a2 /= aa0;
l = dRecipSqrt (a1*a1 + a2*a2 + 1);
a[0] = dCopySign(l,a0);
a[1] = a1*l;
a[2] = a2*l;
}
}
}
/* OLD VERSION */
/*
void dNormalize3 (dVector3 a)
{
dASSERT (a);
dReal l = dDOT(a,a);
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
}
else {
a[0] = 1;
a[1] = 0;
a[2] = 0;
}
}
*/
void dNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dDOT(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
}
else {
dDEBUGMSG ("vector has zero size");
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
}
}
void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
{
dAASSERT (n && p && q);
if (dFabs(n[2]) > M_SQRT1_2) {
// choose p in y-z plane
dReal a = n[1]*n[1] + n[2]*n[2];
dReal k = dRecipSqrt (a);
p[0] = 0;
p[1] = -n[2]*k;
p[2] = n[1]*k;
// set q = n x p
q[0] = a*k;
q[1] = -n[0]*p[2];
q[2] = n[0]*p[1];
}
else {
// choose p in x-y plane
dReal a = n[0]*n[0] + n[1]*n[1];
dReal k = dRecipSqrt (a);
p[0] = -n[1]*k;
p[1] = n[0]*k;
p[2] = 0;
// set q = n x p
q[0] = -n[2]*p[1];
q[1] = n[2]*p[0];
q[2] = a*k;
}
}

145
ode/src/plane.cpp Normal file
View File

@ -0,0 +1,145 @@
/*************************************************************************
* *
* 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"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// plane public API
static void make_sure_plane_normal_has_unit_length (dxPlane *g)
{
dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2];
if (l > 0) {
l = dRecipSqrt(l);
g->p[0] *= l;
g->p[1] *= l;
g->p[2] *= l;
g->p[3] *= l;
}
else {
g->p[0] = 1;
g->p[1] = 0;
g->p[2] = 0;
g->p[3] = 0;
}
}
dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) :
dxGeom (space,0)
{
type = dPlaneClass;
p[0] = a;
p[1] = b;
p[2] = c;
p[3] = d;
make_sure_plane_normal_has_unit_length (this);
}
void dxPlane::computeAABB()
{
aabb[0] = -dInfinity;
aabb[1] = dInfinity;
aabb[2] = -dInfinity;
aabb[3] = dInfinity;
aabb[4] = -dInfinity;
aabb[5] = dInfinity;
// Planes that have normal vectors aligned along an axis can use a
// less comprehensive (half space) bounding box.
if ( p[1] == 0.0f && p[2] == 0.0f ) {
// normal aligned with x-axis
aabb[0] = (p[0] > 0) ? -dInfinity : -p[3];
aabb[1] = (p[0] > 0) ? p[3] : dInfinity;
} else
if ( p[0] == 0.0f && p[2] == 0.0f ) {
// normal aligned with y-axis
aabb[2] = (p[1] > 0) ? -dInfinity : -p[3];
aabb[3] = (p[1] > 0) ? p[3] : dInfinity;
} else
if ( p[0] == 0.0f && p[1] == 0.0f ) {
// normal aligned with z-axis
aabb[4] = (p[2] > 0) ? -dInfinity : -p[3];
aabb[5] = (p[2] > 0) ? p[3] : dInfinity;
}
}
dGeomID dCreatePlane (dSpaceID space,
dReal a, dReal b, dReal c, dReal d)
{
return new dxPlane (space,a,b,c,d);
}
void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d)
{
dUASSERT (g && g->type == dPlaneClass,"argument not a plane");
dxPlane *p = (dxPlane*) g;
p->p[0] = a;
p->p[1] = b;
p->p[2] = c;
p->p[3] = d;
make_sure_plane_normal_has_unit_length (p);
dGeomMoved (g);
}
void dGeomPlaneGetParams (dGeomID g, dVector4 result)
{
dUASSERT (g && g->type == dPlaneClass,"argument not a plane");
dxPlane *p = (dxPlane*) g;
result[0] = p->p[0];
result[1] = p->p[1];
result[2] = p->p[2];
result[3] = p->p[3];
}
dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dPlaneClass,"argument not a plane");
dxPlane *p = (dxPlane*) g;
return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z;
}

880
ode/src/quickstep.cpp Normal file
View File

@ -0,0 +1,880 @@
/*************************************************************************
* *
* 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. *
* *
*************************************************************************/
#include "objects.h"
#include "joint.h"
#include <ode/config.h>
#include <ode/odemath.h>
#include <ode/rotation.h>
#include <ode/timer.h>
#include <ode/error.h>
#include <ode/matrix.h>
#include <ode/misc.h>
#include "lcp.h"
#include "util.h"
#define ALLOCA dALLOCA16
typedef const dReal *dRealPtr;
typedef dReal *dRealMutablePtr;
#define dRealArray(name,n) dReal name[n];
#define dRealAllocaArray(name,n) dReal *name = (dReal*) ALLOCA ((n)*sizeof(dReal));
//***************************************************************************
// configuration
// for the SOR and CG methods:
// uncomment the following line to use warm starting. this definitely
// help for motor-driven joints. unfortunately it appears to hurt
// with high-friction contacts using the SOR method. use with care
//#define WARM_STARTING 1
// for the SOR method:
// uncomment the following line to determine a new constraint-solving
// order for each iteration. however, the qsort per iteration is expensive,
// and the optimal order is somewhat problem dependent.
// @@@ try the leaf->root ordering.
//#define REORDER_CONSTRAINTS 1
// for the SOR method:
// uncomment the following line to randomly reorder constraint rows
// during the solution. depending on the situation, this can help a lot
// or hardly at all, but it doesn't seem to hurt.
#define RANDOMLY_REORDER_CONSTRAINTS 1
//****************************************************************************
// special matrix multipliers
// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q)
static void Multiply1_12q1 (dReal *A, dReal *B, dReal *C, int q)
{
int k;
dReal sum;
dIASSERT (q>0 && A && B && C);
sum = 0;
for (k=0; k<q; k++) sum += B[k*12] * C[k];
A[0] = sum;
sum = 0;
for (k=0; k<q; k++) sum += B[1+k*12] * C[k];
A[1] = sum;
sum = 0;
for (k=0; k<q; k++) sum += B[2+k*12] * C[k];
A[2] = sum;
sum = 0;
for (k=0; k<q; k++) sum += B[3+k*12] * C[k];
A[3] = sum;
sum = 0;
for (k=0; k<q; k++) sum += B[4+k*12] * C[k];
A[4] = sum;
sum = 0;
for (k=0; k<q; k++) sum += B[5+k*12] * C[k];
A[5] = sum;
}
//***************************************************************************
// testing stuff
#ifdef TIMING
#define IFTIMING(x) x
#else
#define IFTIMING(x) /* */
#endif
//***************************************************************************
// various common computations involving the matrix J
// compute iMJ = inv(M)*J'
static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
dxBody * const *body, dRealPtr invI)
{
int i,j;
dRealMutablePtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dReal k = body[b1]->invMass;
for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
if (b2 >= 0) {
k = body[b2]->invMass;
for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
}
J_ptr += 12;
iMJ_ptr += 12;
}
}
// compute out = inv(M)*J'*in.
static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
dRealMutablePtr in, dRealMutablePtr out)
{
int i,j;
dSetZero (out,6*nb);
dRealPtr iMJ_ptr = iMJ;
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dRealMutablePtr out_ptr = out + b1*6;
for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
iMJ_ptr += 6;
if (b2 >= 0) {
out_ptr = out + b2*6;
for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
}
iMJ_ptr += 6;
}
}
// compute out = J*in.
static void multiply_J (int m, dRealMutablePtr J, int *jb,
dRealMutablePtr in, dRealMutablePtr out)
{
int i,j;
dRealPtr J_ptr = J;
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dReal sum = 0;
dRealMutablePtr in_ptr = in + b1*6;
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
J_ptr += 6;
if (b2 >= 0) {
in_ptr = in + b2*6;
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
}
J_ptr += 6;
out[i] = sum;
}
}
// compute out = (J*inv(M)*J' + cfm)*in.
// use z as an nb*6 temporary.
static void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out)
{
multiply_invM_JT (m,nb,iMJ,jb,in,z);
multiply_J (m,J,jb,z,out);
// add cfm
for (int i=0; i<m; i++) out[i] += cfm[i] * in[i];
}
//***************************************************************************
// conjugate gradient method with jacobi preconditioner
// THIS IS EXPERIMENTAL CODE that doesn't work too well, so it is ifdefed out.
//
// adding CFM seems to be critically important to this method.
#if 0
static inline dReal dot (int n, dRealPtr x, dRealPtr y)
{
dReal sum=0;
for (int i=0; i<n; i++) sum += x[i]*y[i];
return sum;
}
// x = y + z*alpha
static inline void add (int n, dRealMutablePtr x, dRealPtr y, dRealPtr z, dReal alpha)
{
for (int i=0; i<n; i++) x[i] = y[i] + z[i]*alpha;
}
static void CG_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
dxQuickStepParameters *qs)
{
int i,j;
const int num_iterations = qs->num_iterations;
// precompute iMJ = inv(M)*J'
dRealAllocaArray (iMJ,m*12);
compute_invM_JT (m,J,iMJ,jb,body,invI);
dReal last_rho = 0;
dRealAllocaArray (r,m);
dRealAllocaArray (z,m);
dRealAllocaArray (p,m);
dRealAllocaArray (q,m);
// precompute 1 / diagonals of A
dRealAllocaArray (Ad,m);
dRealPtr iMJ_ptr = iMJ;
dRealPtr J_ptr = J;
for (i=0; i<m; i++) {
dReal sum = 0;
for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
if (jb[i*2+1] >= 0) {
for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
}
iMJ_ptr += 12;
J_ptr += 12;
Ad[i] = REAL(1.0) / (sum + cfm[i]);
}
#ifdef WARM_STARTING
// compute residual r = b - A*lambda
multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
for (i=0; i<m; i++) r[i] = b[i] - r[i];
#else
dSetZero (lambda,m);
memcpy (r,b,m*sizeof(dReal)); // residual r = b - A*lambda
#endif
for (int iteration=0; iteration < num_iterations; iteration++) {
for (i=0; i<m; i++) z[i] = r[i]*Ad[i]; // z = inv(M)*r
dReal rho = dot (m,r,z); // rho = r'*z
// @@@
// we must check for convergence, otherwise rho will go to 0 if
// we get an exact solution, which will introduce NaNs into the equations.
if (rho < 1e-10) {
printf ("CG returned at iteration %d\n",iteration);
break;
}
if (iteration==0) {
memcpy (p,z,m*sizeof(dReal)); // p = z
}
else {
add (m,p,z,p,rho/last_rho); // p = z + (rho/last_rho)*p
}
// compute q = (J*inv(M)*J')*p
multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,p,q);
dReal alpha = rho/dot (m,p,q); // alpha = rho/(p'*q)
add (m,lambda,lambda,p,alpha); // lambda = lambda + alpha*p
add (m,r,r,q,-alpha); // r = r - alpha*q
last_rho = rho;
}
// compute fc = inv(M)*J'*lambda
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
#if 0
// measure solution error
multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
dReal error = 0;
for (i=0; i<m; i++) error += dFabs(r[i] - b[i]);
printf ("lambda error = %10.6e\n",error);
#endif
}
#endif
//***************************************************************************
// SOR-LCP method
// nb is the number of bodies in the body array.
// J is an m*12 matrix of constraint rows
// jb is an array of first and second body numbers for each constraint row
// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
//
// this returns lambda and fc (the constraint force).
// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
//
// b, lo and hi are modified on exit
struct IndexError {
dReal error; // error to sort on
int findex;
int index; // row index
};
#ifdef REORDER_CONSTRAINTS
static int compare_index_error (const void *a, const void *b)
{
const IndexError *i1 = (IndexError*) a;
const IndexError *i2 = (IndexError*) b;
if (i1->findex < 0 && i2->findex >= 0) return -1;
if (i1->findex >= 0 && i2->findex < 0) return 1;
if (i1->error < i2->error) return -1;
if (i1->error > i2->error) return 1;
return 0;
}
#endif
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
dxQuickStepParameters *qs)
{
const int num_iterations = qs->num_iterations;
const dReal sor_w = qs->w; // SOR over-relaxation parameter
int i,j;
#ifdef WARM_STARTING
// for warm starting, this seems to be necessary to prevent
// jerkiness in motor-driven joints. i have no idea why this works.
for (i=0; i<m; i++) lambda[i] *= 0.9;
#else
dSetZero (lambda,m);
#endif
#ifdef REORDER_CONSTRAINTS
// the lambda computed at the previous iteration.
// this is used to measure error for when we are reordering the indexes.
dRealAllocaArray (last_lambda,m);
#endif
// a copy of the 'hi' vector in case findex[] is being used
dRealAllocaArray (hicopy,m);
memcpy (hicopy,hi,m*sizeof(dReal));
// precompute iMJ = inv(M)*J'
dRealAllocaArray (iMJ,m*12);
compute_invM_JT (m,J,iMJ,jb,body,invI);
// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
// as we change lambda.
#ifdef WARM_STARTING
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
#else
dSetZero (fc,nb*6);
#endif
// precompute 1 / diagonals of A
dRealAllocaArray (Ad,m);
dRealPtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
dReal sum = 0;
for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
if (jb[i*2+1] >= 0) {
for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
}
iMJ_ptr += 12;
J_ptr += 12;
Ad[i] = sor_w / (sum + cfm[i]);
}
// scale J and b by Ad
J_ptr = J;
for (i=0; i<m; i++) {
for (j=0; j<12; j++) {
J_ptr[0] *= Ad[i];
J_ptr++;
}
b[i] *= Ad[i];
// scale Ad by CFM. N.B. this should be done last since it is used above
Ad[i] *= cfm[i];
}
// order to solve constraint rows in
IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
j=0;
int k=1;
// Fill the array from both ends
for (i=0; i<m; i++)
if (findex[i] < 0)
order[j++].index = i; // Place them at the front
else
order[m-k++].index = i; // Place them at the end
dIASSERT ((j+k-1)==m); // -1 since k was started at 1 and not 0
#endif
for (int iteration=0; iteration < num_iterations; iteration++) {
#ifdef REORDER_CONSTRAINTS
// constraints with findex < 0 always come first.
if (iteration < 2) {
// for the first two iterations, solve the constraints in
// the given order
for (i=0; i<m; i++) {
order[i].error = i;
order[i].findex = findex[i];
order[i].index = i;
}
}
else {
// sort the constraints so that the ones converging slowest
// get solved last. use the absolute (not relative) error.
for (i=0; i<m; i++) {
dReal v1 = dFabs (lambda[i]);
dReal v2 = dFabs (last_lambda[i]);
dReal max = (v1 > v2) ? v1 : v2;
if (max > 0) {
//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
order[i].error = dFabs(lambda[i]-last_lambda[i]);
}
else {
order[i].error = dInfinity;
}
order[i].findex = findex[i];
order[i].index = i;
}
}
qsort (order,m,sizeof(IndexError),&compare_index_error);
//@@@ potential optimization: swap lambda and last_lambda pointers rather
// than copying the data. we must make sure lambda is properly
// returned to the caller
memcpy (last_lambda,lambda,m*sizeof(dReal));
#endif
#ifdef RANDOMLY_REORDER_CONSTRAINTS
if ((iteration & 7) == 0) {
for (i=1; i<m; ++i) {
IndexError tmp = order[i];
int swapi = dRandInt(i+1);
order[i] = order[swapi];
order[swapi] = tmp;
}
}
#endif
for (int i=0; i<m; i++) {
// @@@ potential optimization: we could pre-sort J and iMJ, thereby
// linearizing access to those arrays. hmmm, this does not seem
// like a win, but we should think carefully about our memory
// access pattern.
int index = order[i].index;
J_ptr = J + index*12;
iMJ_ptr = iMJ + index*12;
// set the limits for this constraint. note that 'hicopy' is used.
// this is the place where the QuickStep method differs from the
// direct LCP solving method, since that method only performs this
// limit adjustment once per time step, whereas this method performs
// once per iteration per constraint row.
// the constraints are ordered so that all lambda[] values needed have
// already been computed.
if (findex[index] >= 0) {
hi[index] = dFabs (hicopy[index] * lambda[findex[index]]);
lo[index] = -hi[index];
}
int b1 = jb[index*2];
int b2 = jb[index*2+1];
dReal delta = b[index] - lambda[index]*Ad[index];
dRealMutablePtr fc_ptr = fc + 6*b1;
// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = fc + 6*b2;
delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
}
// compute lambda and clamp it to [lo,hi].
// @@@ potential optimization: does SSE have clamping instructions
// to save test+jump penalties here?
dReal new_lambda = lambda[index] + delta;
if (new_lambda < lo[index]) {
delta = lo[index]-lambda[index];
lambda[index] = lo[index];
}
else if (new_lambda > hi[index]) {
delta = hi[index]-lambda[index];
lambda[index] = hi[index];
}
else {
lambda[index] = new_lambda;
}
//@@@ a trick that may or may not help
//dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations));
//delta *= ramp;
// update fc.
// @@@ potential optimization: SIMD for this and the b2 >= 0 case
fc_ptr = fc + 6*b1;
fc_ptr[0] += delta * iMJ_ptr[0];
fc_ptr[1] += delta * iMJ_ptr[1];
fc_ptr[2] += delta * iMJ_ptr[2];
fc_ptr[3] += delta * iMJ_ptr[3];
fc_ptr[4] += delta * iMJ_ptr[4];
fc_ptr[5] += delta * iMJ_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = fc + 6*b2;
fc_ptr[0] += delta * iMJ_ptr[6];
fc_ptr[1] += delta * iMJ_ptr[7];
fc_ptr[2] += delta * iMJ_ptr[8];
fc_ptr[3] += delta * iMJ_ptr[9];
fc_ptr[4] += delta * iMJ_ptr[10];
fc_ptr[5] += delta * iMJ_ptr[11];
}
}
}
}
void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *_joint, int nj, dReal stepsize)
{
int i,j;
IFTIMING(dTimerStart("preprocessing");)
dReal stepsize1 = dRecip(stepsize);
// number all bodies in the body list - set their tag values
for (i=0; i<nb; i++) body[i]->tag = i;
// make a local copy of the joint array, because we might want to modify it.
// (the "dxJoint *const*" declaration says we're allowed to modify the joints
// but not the joint array, because the caller might need it unchanged).
//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
dxJoint **joint = (dxJoint**) alloca (nj * sizeof(dxJoint*));
memcpy (joint,_joint,nj * sizeof(dxJoint*));
// for all bodies, compute the inertia tensor and its inverse in the global
// frame, and compute the rotational force and add it to the torque
// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
//dRealAllocaArray (I,3*4*nb); // need to remember all I's for feedback purposes only
dRealAllocaArray (invI,3*4*nb);
for (i=0; i<nb; i++) {
dMatrix3 tmp;
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);
dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);
#ifdef dGYROSCOPIC
dMatrix3 I;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
//dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);
dMULTIPLY0_333 (I,body[i]->posr.R,tmp);
// compute rotational force
//dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
dMULTIPLY0_331 (tmp,I,body[i]->avel);
dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
#endif
}
// add the gravity force to all bodies
for (i=0; i<nb; i++) {
if ((body[i]->flags & dxBodyNoGravity)==0) {
body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
}
}
// get joint information (m = total constraint dimension, nub = number of unbounded variables).
// joints with m=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them.
//@@@ do we really need to save all the info1's
dxJoint::Info1 *info = (dxJoint::Info1*) alloca (nj*sizeof(dxJoint::Info1));
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
joint[j]->vtable->getInfo1 (joint[j],info+i);
dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
if (info[i].m > 0) {
joint[i] = joint[j];
i++;
}
}
nj = i;
// create the row offset array
int m = 0;
int *ofs = (int*) alloca (nj*sizeof(int));
for (i=0; i<nj; i++) {
ofs[i] = m;
m += info[i].m;
}
// if there are constraints, compute the constraint force
dRealAllocaArray (J,m*12);
int *jb = (int*) alloca (m*2*sizeof(int));
if (m > 0) {
// create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
dRealAllocaArray (c,m);
dRealAllocaArray (cfm,m);
dRealAllocaArray (lo,m);
dRealAllocaArray (hi,m);
int *findex = (int*) alloca (m*sizeof(int));
dSetZero (c,m);
dSetValue (cfm,m,world->global_cfm);
dSetValue (lo,m,-dInfinity);
dSetValue (hi,m, dInfinity);
for (i=0; i<m; i++) findex[i] = -1;
// get jacobian data from constraints. an m*12 matrix will be created
// to store the two jacobian blocks from each constraint. it has this
// format:
//
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ .
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows)
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
// etc...
//
// (lll) = linear jacobian data
// (aaa) = angular jacobian data
//
IFTIMING (dTimerNow ("create J");)
dSetZero (J,m*12);
dxJoint::Info2 Jinfo;
Jinfo.rowskip = 12;
Jinfo.fps = stepsize1;
Jinfo.erp = world->global_erp;
int mfb = 0; // number of rows of Jacobian we will have to save for joint feedback
for (i=0; i<nj; i++) {
Jinfo.J1l = J + ofs[i]*12;
Jinfo.J1a = Jinfo.J1l + 3;
Jinfo.J2l = Jinfo.J1l + 6;
Jinfo.J2a = Jinfo.J1l + 9;
Jinfo.c = c + ofs[i];
Jinfo.cfm = cfm + ofs[i];
Jinfo.lo = lo + ofs[i];
Jinfo.hi = hi + ofs[i];
Jinfo.findex = findex + ofs[i];
joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
// adjust returned findex values for global index numbering
for (j=0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
}
if (joint[i]->feedback)
mfb += info[i].m;
}
// we need a copy of Jacobian for joint feedbacks
// because it gets destroyed by SOR solver
// instead of saving all Jacobian, we can save just rows
// for joints, that requested feedback (which is normaly much less)
dRealAllocaArray (Jcopy,mfb*12);
if (mfb > 0) {
mfb = 0;
for (i=0; i<nj; i++)
if (joint[i]->feedback) {
memcpy(Jcopy+mfb*12, J+ofs[i]*12, info[i].m*12*sizeof(dReal));
mfb += info[i].m;
}
}
// create an array of body numbers for each joint row
int *jb_ptr = jb;
for (i=0; i<nj; i++) {
int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->tag) : -1;
int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->tag) : -1;
for (j=0; j<info[i].m; j++) {
jb_ptr[0] = b1;
jb_ptr[1] = b2;
jb_ptr += 2;
}
}
dIASSERT (jb_ptr == jb+2*m);
// compute the right hand side `rhs'
IFTIMING (dTimerNow ("compute rhs");)
dRealAllocaArray (tmp1,nb*6);
// put v/h + invM*fe into tmp1
for (i=0; i<nb; i++) {
dReal body_invMass = body[i]->invMass;
for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1;
dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1;
}
// put J*tmp1 into rhs
dRealAllocaArray (rhs,m);
multiply_J (m,J,jb,tmp1,rhs);
// complete rhs
for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
// scale CFM
for (i=0; i<m; i++) cfm[i] *= stepsize1;
// load lambda from the value saved on the previous iteration
dRealAllocaArray (lambda,m);
#ifdef WARM_STARTING
dSetZero (lambda,m); //@@@ shouldn't be necessary
for (i=0; i<nj; i++) {
memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(dReal));
}
#endif
// solve the LCP problem and get lambda and invM*constraint_force
IFTIMING (dTimerNow ("solving LCP problem");)
dRealAllocaArray (cforce,nb*6);
SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,&world->qs);
#ifdef WARM_STARTING
// save lambda for the next iteration
//@@@ note that this doesn't work for contact joints yet, as they are
// recreated every iteration
for (i=0; i<nj; i++) {
memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(dReal));
}
#endif
// note that the SOR method overwrites rhs and J at this point, so
// they should not be used again.
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
}
// if joint feedback is requested, compute the constraint force.
// BUT: cforce is inv(M)*J'*lambda, whereas we want just J'*lambda,
// so we must compute M*cforce.
// @@@ if any joint has a feedback request we compute the entire
// adjusted cforce, which is not the most efficient way to do it.
/*for (j=0; j<nj; j++) {
if (joint[j]->feedback) {
// compute adjusted cforce
for (i=0; i<nb; i++) {
dReal k = body[i]->mass.mass;
cforce [i*6+0] *= k;
cforce [i*6+1] *= k;
cforce [i*6+2] *= k;
dVector3 tmp;
dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3);
cforce [i*6+3] = tmp[0];
cforce [i*6+4] = tmp[1];
cforce [i*6+5] = tmp[2];
}
// compute feedback for this and all remaining joints
for (; j<nj; j++) {
dJointFeedback *fb = joint[j]->feedback;
if (fb) {
int b1 = joint[j]->node[0].body->tag;
memcpy (fb->f1,cforce+b1*6,3*sizeof(dReal));
memcpy (fb->t1,cforce+b1*6+3,3*sizeof(dReal));
if (joint[j]->node[1].body) {
int b2 = joint[j]->node[1].body->tag;
memcpy (fb->f2,cforce+b2*6,3*sizeof(dReal));
memcpy (fb->t2,cforce+b2*6+3,3*sizeof(dReal));
}
}
}
}
}*/
if (mfb > 0) {
// straightforward computation of joint constraint forces:
// multiply related lambdas with respective J' block for joints
// where feedback was requested
mfb = 0;
for (i=0; i<nj; i++) {
if (joint[i]->feedback) {
dJointFeedback *fb = joint[i]->feedback;
dReal data[6];
Multiply1_12q1 (data, Jcopy+mfb*12, lambda+ofs[i], info[i].m);
fb->f1[0] = data[0];
fb->f1[1] = data[1];
fb->f1[2] = data[2];
fb->t1[0] = data[3];
fb->t1[1] = data[4];
fb->t1[2] = data[5];
if (joint[i]->node[1].body)
{
Multiply1_12q1 (data, Jcopy+mfb*12+6, lambda+ofs[i], info[i].m);
fb->f2[0] = data[0];
fb->f2[1] = data[1];
fb->f2[2] = data[2];
fb->t2[0] = data[3];
fb->t2[1] = data[4];
fb->t2[2] = data[5];
}
mfb += info[i].m;
}
}
}
}
// compute the velocity update:
// add stepsize * invM * fe to the body velocity
IFTIMING (dTimerNow ("compute velocity update");)
for (i=0; i<nb; i++) {
dReal body_invMass = body[i]->invMass;
for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j];
for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;
dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc);
}
#if 0
// check that the updated velocity obeys the constraint (this check needs unmodified J)
dRealAllocaArray (vel,nb*6);
for (i=0; i<nb; i++) {
for (j=0; j<3; j++) vel[i*6+j] = body[i]->lvel[j];
for (j=0; j<3; j++) vel[i*6+3+j] = body[i]->avel[j];
}
dRealAllocaArray (tmp,m);
multiply_J (m,J,jb,vel,tmp);
dReal error = 0;
for (i=0; i<m; i++) error += dFabs(tmp[i]);
printf ("velocity error = %10.6e\n",error);
#endif
// update the position and orientation from the new linear/angular velocity
// (over the given timestep)
IFTIMING (dTimerNow ("update position");)
for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
IFTIMING (dTimerNow ("tidy up");)
// zero all force accumulators
for (i=0; i<nb; i++) {
dSetZero (body[i]->facc,3);
dSetZero (body[i]->tacc,3);
}
IFTIMING (dTimerEnd();)
IFTIMING (if (m > 0) dTimerReport (stdout,1);)
}

33
ode/src/quickstep.h Normal file
View File

@ -0,0 +1,33 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#ifndef _ODE_QUICK_STEP_H_
#define _ODE_QUICK_STEP_H_
#include <ode/common.h>
void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *_joint, int nj, dReal stepsize);
#endif

677
ode/src/ray.cpp Normal file
View File

@ -0,0 +1,677 @@
/*************************************************************************
* *
* 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"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// ray public API
dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1)
{
type = dRayClass;
length = _length;
}
void dxRay::computeAABB()
{
dVector3 e;
e[0] = final_posr->pos[0] + final_posr->R[0*4+2]*length;
e[1] = final_posr->pos[1] + final_posr->R[1*4+2]*length;
e[2] = final_posr->pos[2] + final_posr->R[2*4+2]*length;
if (final_posr->pos[0] < e[0]){
aabb[0] = final_posr->pos[0];
aabb[1] = e[0];
}
else{
aabb[0] = e[0];
aabb[1] = final_posr->pos[0];
}
if (final_posr->pos[1] < e[1]){
aabb[2] = final_posr->pos[1];
aabb[3] = e[1];
}
else{
aabb[2] = e[1];
aabb[3] = final_posr->pos[1];
}
if (final_posr->pos[2] < e[2]){
aabb[4] = final_posr->pos[2];
aabb[5] = e[2];
}
else{
aabb[4] = e[2];
aabb[5] = final_posr->pos[2];
}
}
dGeomID dCreateRay (dSpaceID space, dReal length)
{
return new dxRay (space,length);
}
void dGeomRaySetLength (dGeomID g, dReal length)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
dxRay *r = (dxRay*) g;
r->length = length;
dGeomMoved (g);
}
dReal dGeomRayGetLength (dGeomID g)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
dxRay *r = (dxRay*) g;
return r->length;
}
void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz,
dReal dx, dReal dy, dReal dz)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
g->recomputePosr();
dReal* rot = g->final_posr->R;
dReal* pos = g->final_posr->pos;
dVector3 n;
pos[0] = px;
pos[1] = py;
pos[2] = pz;
n[0] = dx;
n[1] = dy;
n[2] = dz;
dNormalize3(n);
rot[0*4+2] = n[0];
rot[1*4+2] = n[1];
rot[2*4+2] = n[2];
dGeomMoved (g);
}
void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
g->recomputePosr();
start[0] = g->final_posr->pos[0];
start[1] = g->final_posr->pos[1];
start[2] = g->final_posr->pos[2];
dir[0] = g->final_posr->R[0*4+2];
dir[1] = g->final_posr->R[1*4+2];
dir[2] = g->final_posr->R[2*4+2];
}
void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
if (FirstContact){
g->gflags |= RAY_FIRSTCONTACT;
}
else g->gflags &= ~RAY_FIRSTCONTACT;
if (BackfaceCull){
g->gflags |= RAY_BACKFACECULL;
}
else g->gflags &= ~RAY_BACKFACECULL;
}
void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
(*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0);
(*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0);
}
void dGeomRaySetClosestHit (dxGeom *g, int closestHit)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
if (closestHit){
g->gflags |= RAY_CLOSEST_HIT;
}
else g->gflags &= ~RAY_CLOSEST_HIT;
}
int dGeomRayGetClosestHit (dxGeom *g)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
return ((g->gflags & RAY_CLOSEST_HIT) != 0);
}
// if mode==1 then use the sphere exit contact, not the entry contact
static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius,
dContactGeom *contact, int mode)
{
dVector3 q;
q[0] = ray->final_posr->pos[0] - sphere_pos[0];
q[1] = ray->final_posr->pos[1] - sphere_pos[1];
q[2] = ray->final_posr->pos[2] - sphere_pos[2];
dReal B = dDOT14(q,ray->final_posr->R+2);
dReal C = dDOT(q,q) - radius*radius;
// note: if C <= 0 then the start of the ray is inside the sphere
dReal k = B*B - C;
if (k < 0) return 0;
k = dSqrt(k);
dReal alpha;
if (mode && C >= 0) {
alpha = -B + k;
if (alpha < 0) return 0;
}
else {
alpha = -B - k;
if (alpha < 0) {
alpha = -B + k;
if (alpha < 0) return 0;
}
}
if (alpha > ray->length) return 0;
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0);
contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]);
contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]);
contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]);
dNormalize3 (contact->normal);
contact->depth = alpha;
return 1;
}
int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dSphereClass);
dxRay *ray = (dxRay*) o1;
dxSphere *sphere = (dxSphere*) o2;
contact->g1 = ray;
contact->g2 = sphere;
return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0);
}
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dBoxClass);
dxRay *ray = (dxRay*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = ray;
contact->g2 = box;
int i;
// compute the start and delta of the ray relative to the box.
// we will do all subsequent computations in this box-relative coordinate
// system. we have to do a translation and rotation for each point.
dVector3 tmp,s,v;
tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
dMULTIPLY1_331 (s,box->final_posr->R,tmp);
tmp[0] = ray->final_posr->R[0*4+2];
tmp[1] = ray->final_posr->R[1*4+2];
tmp[2] = ray->final_posr->R[2*4+2];
dMULTIPLY1_331 (v,box->final_posr->R,tmp);
// mirror the line so that v has all components >= 0
dVector3 sign;
for (i=0; i<3; i++) {
if (v[i] < 0) {
s[i] = -s[i];
v[i] = -v[i];
sign[i] = 1;
}
else sign[i] = -1;
}
// compute the half-sides of the box
dReal h[3];
h[0] = REAL(0.5) * box->side[0];
h[1] = REAL(0.5) * box->side[1];
h[2] = REAL(0.5) * box->side[2];
// do a few early exit tests
if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] ||
(s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] ||
(s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] ||
(v[0] == 0 && v[1] == 0 && v[2] == 0)) {
return 0;
}
// compute the t=[lo..hi] range for where s+v*t intersects the box
dReal lo = -dInfinity;
dReal hi = dInfinity;
int nlo = 0, nhi = 0;
for (i=0; i<3; i++) {
if (v[i] != 0) {
dReal k = (-h[i] - s[i])/v[i];
if (k > lo) {
lo = k;
nlo = i;
}
k = (h[i] - s[i])/v[i];
if (k < hi) {
hi = k;
nhi = i;
}
}
}
// check if the ray intersects
if (lo > hi) return 0;
dReal alpha;
int n;
if (lo >= 0) {
alpha = lo;
n = nlo;
}
else {
alpha = hi;
n = nhi;
}
if (alpha < 0 || alpha > ray->length) return 0;
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
contact->normal[0] = box->final_posr->R[0*4+n] * sign[n];
contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
contact->depth = alpha;
return 1;
}
int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dCapsuleClass);
dxRay *ray = (dxRay*) o1;
dxCapsule *ccyl = (dxCapsule*) o2;
contact->g1 = ray;
contact->g2 = ccyl;
dReal lz2 = ccyl->lz * REAL(0.5);
// compute some useful info
dVector3 cs,q,r;
dReal C,k;
cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0];
cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1];
cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2];
k = dDOT41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis
q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0];
q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1];
q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2];
C = dDOT(q,q) - ccyl->radius*ccyl->radius;
// if C < 0 then ray start position within infinite extension of cylinder
// see if ray start position is inside the capped cylinder
int inside_ccyl = 0;
if (C < 0) {
if (k < -lz2) k = -lz2;
else if (k > lz2) k = lz2;
r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) +
(ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) +
(ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) {
inside_ccyl = 1;
}
}
// compute ray collision with infinite cylinder, except for the case where
// the ray is outside the capped cylinder but within the infinite cylinder
// (it that case the ray can only hit endcaps)
if (!inside_ccyl && C < 0) {
// set k to cap position to check
if (k < 0) k = -lz2; else k = lz2;
}
else {
dReal uv = dDOT44(ccyl->final_posr->R+2,ray->final_posr->R+2);
r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2];
r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2];
r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2];
dReal A = dDOT(r,r);
dReal B = 2*dDOT(q,r);
k = B*B-4*A*C;
if (k < 0) {
// the ray does not intersect the infinite cylinder, but if the ray is
// inside and parallel to the cylinder axis it may intersect the end
// caps. set k to cap position to check.
if (!inside_ccyl) return 0;
if (uv < 0) k = -lz2; else k = lz2;
}
else {
k = dSqrt(k);
A = dRecip (2*A);
dReal alpha = (-B-k)*A;
if (alpha < 0) {
alpha = (-B+k)*A;
if (alpha < 0) return 0;
}
if (alpha > ray->length) return 0;
// the ray intersects the infinite cylinder. check to see if the
// intersection point is between the caps
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
q[0] = contact->pos[0] - ccyl->final_posr->pos[0];
q[1] = contact->pos[1] - ccyl->final_posr->pos[1];
q[2] = contact->pos[2] - ccyl->final_posr->pos[2];
k = dDOT14(q,ccyl->final_posr->R+2);
dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0);
if (k >= -lz2 && k <= lz2) {
contact->normal[0] = nsign * (contact->pos[0] -
(ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]));
contact->normal[1] = nsign * (contact->pos[1] -
(ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]));
contact->normal[2] = nsign * (contact->pos[2] -
(ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]));
dNormalize3 (contact->normal);
contact->depth = alpha;
return 1;
}
// the infinite cylinder intersection point is not between the caps.
// set k to cap position to check.
if (k < 0) k = -lz2; else k = lz2;
}
}
// check for ray intersection with the caps. k must indicate the cap
// position to check
q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl);
}
int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dPlaneClass);
dxRay *ray = (dxRay*) o1;
dxPlane *plane = (dxPlane*) o2;
dReal alpha = plane->p[3] - dDOT (plane->p,ray->final_posr->pos);
// note: if alpha > 0 the starting point is below the plane
dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
dReal k = dDOT14(plane->p,ray->final_posr->R+2);
if (k==0) return 0; // ray parallel to plane
alpha /= k;
if (alpha < 0 || alpha > ray->length) return 0;
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
contact->normal[0] = nsign*plane->p[0];
contact->normal[1] = nsign*plane->p[1];
contact->normal[2] = nsign*plane->p[2];
contact->depth = alpha;
contact->g1 = ray;
contact->g2 = plane;
return 1;
}
// Ray - Cylinder collider by David Walters (June 2006)
int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dRayClass );
dIASSERT( o2->type == dCylinderClass );
dxRay* ray = (dxRay*)( o1 );
dxCylinder* cyl = (dxCylinder*)( o2 );
// Fill in contact information.
contact->g1 = ray;
contact->g2 = cyl;
const dReal half_length = cyl->lz * REAL( 0.5 );
//
// Compute some useful info
//
dVector3 q, r;
dReal d, C, k;
// Vector 'r', line segment from C to R (ray start) ( r = R - C )
r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];
// Distance that ray start is along cyl axis ( Z-axis direction )
d = dDOT41( cyl->final_posr->R + 2, r );
//
// Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
//
// Point on axis ( in world space ): cp = ( d * Cz ) + C
//
// Line 'q' from R to cp: q = cp - R
// q = ( d * Cz ) + C - R
// q = ( d * Cz ) - ( R - C )
q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];
// Compute square length of 'q'. Subtract from radius squared to
// get square distance 'C' between the line q and the radius.
// if C < 0 then ray start position is within infinite extension of cylinder
C = dDOT( q, q ) - ( cyl->radius * cyl->radius );
// Compute the projection of ray direction normal onto cylinder direction normal.
dReal uv = dDOT44( cyl->final_posr->R+2, ray->final_posr->R+2 );
//
// Find ray collision with infinite cylinder
//
// Compute vector from end of ray direction normal to projection on cylinder direction normal.
r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];
// Quadratic Formula Magic
// Compute discriminant 'k':
// k < 0 : No intersection
// k = 0 : Tangent
// k > 0 : Intersection
dReal A = dDOT( r, r );
dReal B = 2 * dDOT( q, r );
k = B*B - 4*A*C;
//
// Collision with Flat Caps ?
//
// No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
if ( k < dEpsilon && C <= 0 )
{
// The ray does not intersect the edge of the infinite cylinder,
// but the ray start is inside and so must run parallel to the axis.
// It may yet intersect an end cap. The following cases are valid:
// -ve-cap , -half centre +half , +ve-cap
// <<================|-------------------|------------->>>---|================>>
// | |
// | d-------------------> 1.
// 2. d------------------> |
// 3. <------------------d |
// | <-------------------d 4.
// | |
// <<================|-------------------|------------->>>---|===============>>
// Negative if the ray and cylinder axes point in opposite directions.
const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
// Negative if the ray start is inside the cylinder
const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );
// Ray and Cylinder axes run in the same direction ( cases 1, 2 )
// Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
{
return 0; // No intersection with caps or curved surface.
}
// Compute depth (distance from ray to cylinder)
contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );
// Compute contact point.
contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );
// Compute reflected contact normal.
contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );
// Contact!
return 1;
}
//
// Collision with Curved Edge ?
//
if ( k > 0 )
{
// Finish off quadratic formula to get intersection co-efficient
k = dSqrt( k );
A = dRecip( 2 * A );
// Compute distance along line to contact point.
dReal alpha = ( -B - k ) * A;
if ( alpha < 0 )
{
// Flip in the other direction.
alpha = ( -B + k ) * A;
}
// Intersection point is within ray length?
if ( alpha >= 0 && alpha <= ray->length )
{
// The ray intersects the infinite cylinder!
// Compute contact point.
contact->pos[0] = ray->final_posr->pos[0] + ( alpha * ray->final_posr->R[0*4+2] );
contact->pos[1] = ray->final_posr->pos[1] + ( alpha * ray->final_posr->R[1*4+2] );
contact->pos[2] = ray->final_posr->pos[2] + ( alpha * ray->final_posr->R[2*4+2] );
// q is the vector from the cylinder centre to the contact point.
q[0] = contact->pos[0] - cyl->final_posr->pos[0];
q[1] = contact->pos[1] - cyl->final_posr->pos[1];
q[2] = contact->pos[2] - cyl->final_posr->pos[2];
// Compute the distance along the cylinder axis of this contact point.
d = dDOT14( q, cyl->final_posr->R+2 );
// Check to see if the intersection point is between the flat end caps
if ( d >= -half_length && d <= +half_length )
{
// Flip the normal if the start point is inside the cylinder.
const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
// Compute contact normal.
contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
dNormalize3( contact->normal );
// Store depth.
contact->depth = alpha;
// Contact!
return 1;
}
}
}
// No contact with anything.
return 0;
}

316
ode/src/rotation.cpp Normal file
View File

@ -0,0 +1,316 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the
"rotation axis" and s is the "rotation angle".
*/
#include <ode/rotation.h>
#include <ode/odemath.h>
#define _R(i,j) R[(i)*4+(j)]
#define SET_3x3_IDENTITY \
_R(0,0) = REAL(1.0); \
_R(0,1) = REAL(0.0); \
_R(0,2) = REAL(0.0); \
_R(0,3) = REAL(0.0); \
_R(1,0) = REAL(0.0); \
_R(1,1) = REAL(1.0); \
_R(1,2) = REAL(0.0); \
_R(1,3) = REAL(0.0); \
_R(2,0) = REAL(0.0); \
_R(2,1) = REAL(0.0); \
_R(2,2) = REAL(1.0); \
_R(2,3) = REAL(0.0);
void dRSetIdentity (dMatrix3 R)
{
dAASSERT (R);
SET_3x3_IDENTITY;
}
void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal angle)
{
dAASSERT (R);
dQuaternion q;
dQFromAxisAndAngle (q,ax,ay,az,angle);
dQtoR (q,R);
}
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
dAASSERT (R);
sphi = dSin(phi);
cphi = dCos(phi);
stheta = dSin(theta);
ctheta = dCos(theta);
spsi = dSin(psi);
cpsi = dCos(psi);
_R(0,0) = cpsi*ctheta;
_R(0,1) = spsi*ctheta;
_R(0,2) =-stheta;
_R(0,3) = REAL(0.0);
_R(1,0) = cpsi*stheta*sphi - spsi*cphi;
_R(1,1) = spsi*stheta*sphi + cpsi*cphi;
_R(1,2) = ctheta*sphi;
_R(1,3) = REAL(0.0);
_R(2,0) = cpsi*stheta*cphi + spsi*sphi;
_R(2,1) = spsi*stheta*cphi - cpsi*sphi;
_R(2,2) = ctheta*cphi;
_R(2,3) = REAL(0.0);
}
void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal bx, dReal by, dReal bz)
{
dReal l,k;
dAASSERT (R);
l = dSqrt (ax*ax + ay*ay + az*az);
if (l <= REAL(0.0)) {
dDEBUGMSG ("zero length vector");
return;
}
l = dRecip(l);
ax *= l;
ay *= l;
az *= l;
k = ax*bx + ay*by + az*bz;
bx -= k*ax;
by -= k*ay;
bz -= k*az;
l = dSqrt (bx*bx + by*by + bz*bz);
if (l <= REAL(0.0)) {
dDEBUGMSG ("zero length vector");
return;
}
l = dRecip(l);
bx *= l;
by *= l;
bz *= l;
_R(0,0) = ax;
_R(1,0) = ay;
_R(2,0) = az;
_R(0,1) = bx;
_R(1,1) = by;
_R(2,1) = bz;
_R(0,2) = - by*az + ay*bz;
_R(1,2) = - bz*ax + az*bx;
_R(2,2) = - bx*ay + ax*by;
_R(0,3) = REAL(0.0);
_R(1,3) = REAL(0.0);
_R(2,3) = REAL(0.0);
}
void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az)
{
dVector3 n,p,q;
n[0] = ax;
n[1] = ay;
n[2] = az;
dNormalize3 (n);
dPlaneSpace (n,p,q);
_R(0,0) = p[0];
_R(1,0) = p[1];
_R(2,0) = p[2];
_R(0,1) = q[0];
_R(1,1) = q[1];
_R(2,1) = q[2];
_R(0,2) = n[0];
_R(1,2) = n[1];
_R(2,2) = n[2];
_R(0,3) = REAL(0.0);
_R(1,3) = REAL(0.0);
_R(2,3) = REAL(0.0);
}
void dQSetIdentity (dQuaternion q)
{
dAASSERT (q);
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dAASSERT (q);
dReal l = ax*ax + ay*ay + az*az;
if (l > REAL(0.0)) {
angle *= REAL(0.5);
q[0] = dCos (angle);
l = dSin(angle) * dRecipSqrt(l);
q[1] = ax*l;
q[2] = ay*l;
q[3] = az*l;
}
else {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
{
dAASSERT (qa && qb && qc);
qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
}
void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
{
dAASSERT (qa && qb && qc);
qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
}
void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
{
dAASSERT (qa && qb && qc);
qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
}
void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
{
dAASSERT (qa && qb && qc);
qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
}
// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction
// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
// University, 1997.
void dRfromQ (dMatrix3 R, const dQuaternion q)
{
dAASSERT (q && R);
// q = (s,vx,vy,vz)
dReal qq1 = 2*q[1]*q[1];
dReal qq2 = 2*q[2]*q[2];
dReal qq3 = 2*q[3]*q[3];
_R(0,0) = 1 - qq2 - qq3;
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
_R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
_R(0,3) = REAL(0.0);
_R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
_R(1,1) = 1 - qq1 - qq3;
_R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
_R(1,3) = REAL(0.0);
_R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
_R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
_R(2,2) = 1 - qq1 - qq2;
_R(2,3) = REAL(0.0);
}
void dQfromR (dQuaternion q, const dMatrix3 R)
{
dAASSERT (q && R);
dReal tr,s;
tr = _R(0,0) + _R(1,1) + _R(2,2);
if (tr >= 0) {
s = dSqrt (tr + 1);
q[0] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[1] = (_R(2,1) - _R(1,2)) * s;
q[2] = (_R(0,2) - _R(2,0)) * s;
q[3] = (_R(1,0) - _R(0,1)) * s;
}
else {
// find the largest diagonal element and jump to the appropriate case
if (_R(1,1) > _R(0,0)) {
if (_R(2,2) > _R(1,1)) goto case_2;
goto case_1;
}
if (_R(2,2) > _R(0,0)) goto case_2;
goto case_0;
case_0:
s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
q[1] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[2] = (_R(0,1) + _R(1,0)) * s;
q[3] = (_R(2,0) + _R(0,2)) * s;
q[0] = (_R(2,1) - _R(1,2)) * s;
return;
case_1:
s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
q[2] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[3] = (_R(1,2) + _R(2,1)) * s;
q[1] = (_R(0,1) + _R(1,0)) * s;
q[0] = (_R(0,2) - _R(2,0)) * s;
return;
case_2:
s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
q[3] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[1] = (_R(2,0) + _R(0,2)) * s;
q[2] = (_R(1,2) + _R(2,1)) * s;
q[0] = (_R(1,0) - _R(0,1)) * s;
return;
}
}
void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q)
{
dAASSERT (w && q && dq);
dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
}

235
ode/src/sphere.cpp Normal file
View File

@ -0,0 +1,235 @@
/*************************************************************************
* *
* 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"
#ifdef _MSC_VER
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
//****************************************************************************
// sphere public API
dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1)
{
dAASSERT (_radius > 0);
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;
}
dGeomID dCreateSphere (dSpaceID space, dReal radius)
{
return new dxSphere (space,radius);
}
void dGeomSphereSetRadius (dGeomID g, dReal radius)
{
dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
dAASSERT (radius > 0);
dxSphere *s = (dxSphere*) g;
s->radius = radius;
dGeomMoved (g);
}
dReal dGeomSphereGetRadius (dGeomID g)
{
dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
dxSphere *s = (dxSphere*) g;
return s->radius;
}
dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
g->recomputePosr();
dxSphere *s = (dxSphere*) g;
dReal * pos = s->final_posr->pos;
return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) +
(y-pos[1])*(y-pos[1]) +
(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)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dSphereClass);
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;
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dBoxClass);
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] = 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] = 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] = 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)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dPlaneClass);
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] - plane->p[0] * sphere->radius;
contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
contact->depth = depth;
return 1;
}
else return 0;
}

1795
ode/src/step.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

36
ode/src/step.h Normal file
View File

@ -0,0 +1,36 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#ifndef _ODE_STEP_H_
#define _ODE_STEP_H_
#include <ode/common.h>
void dInternalStepIsland (dxWorld *world,
dxBody * const *body, int nb,
dxJoint * const *joint, int nj,
dReal stepsize);
#endif

1139
ode/src/stepfast.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

421
ode/src/timer.cpp Normal file
View File

@ -0,0 +1,421 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
/*
TODO
----
* gettimeofday() and the pentium time stamp counter return the real time,
not the process time. fix this somehow!
*/
#include <ode/common.h>
#include <ode/timer.h>
// misc defines
#define ALLOCA dALLOCA16
//****************************************************************************
// implementation for windows based on the multimedia performance counter.
#ifdef WIN32
#include "windows.h"
static inline void getClockCount (unsigned long cc[2])
{
LARGE_INTEGER a;
QueryPerformanceCounter (&a);
cc[0] = a.LowPart;
cc[1] = a.HighPart;
}
static inline void serialize()
{
}
static inline double loadClockCount (unsigned long cc[2])
{
LARGE_INTEGER a;
a.LowPart = cc[0];
a.HighPart = cc[1];
return double(a.QuadPart);
}
double dTimerResolution()
{
return 1.0/dTimerTicksPerSecond();
}
double dTimerTicksPerSecond()
{
static int query=0;
static double hz=0.0;
if (!query) {
LARGE_INTEGER a;
QueryPerformanceFrequency (&a);
hz = double(a.QuadPart);
query = 1;
}
return hz;
}
#endif
//****************************************************************************
// implementation based on the pentium time stamp counter. the timer functions
// can be serializing or non-serializing. serializing will ensure that all
// instructions have executed and data has been written back before the cpu
// time stamp counter is read. the CPUID instruction is used to serialize.
#if defined(PENTIUM) && !defined(WIN32)
// we need to know the clock rate so that the timing function can report
// accurate times. this number only needs to be set accurately if we're
// doing performance tests and care about real-world time numbers - otherwise,
// just ignore this. i have not worked out how to determine this number
// automatically yet.
#define PENTIUM_HZ (500e6)
static inline void getClockCount (unsigned long cc[2])
{
#ifndef X86_64_SYSTEM
asm volatile (
"rdtsc\n"
"movl %%eax,(%%esi)\n"
"movl %%edx,4(%%esi)\n"
: : "S" (cc) : "%eax","%edx","cc","memory");
#else
asm volatile (
"rdtsc\n"
"movl %%eax,(%%rsi)\n"
"movl %%edx,4(%%rsi)\n"
: : "S" (cc) : "%eax","%edx","cc","memory");
#endif
}
static inline void serialize()
{
#ifndef X86_64_SYSTEM
asm volatile (
"mov $0,%%eax\n"
"push %%ebx\n"
"cpuid\n"
"pop %%ebx\n"
: : : "%eax","%ecx","%edx","cc","memory");
#else
asm volatile (
"mov $0,%%rax\n"
"push %%rbx\n"
"cpuid\n"
"pop %%rbx\n"
: : : "%rax","%rcx","%rdx","cc","memory");
#endif
}
static inline double loadClockCount (unsigned long a[2])
{
double ret;
#ifndef X86_64_SYSTEM
asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) :
"cc","memory");
#else
asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) :
"cc","memory");
#endif
return ret;
}
double dTimerResolution()
{
return 1.0/PENTIUM_HZ;
}
double dTimerTicksPerSecond()
{
return PENTIUM_HZ;
}
#endif
//****************************************************************************
// otherwise, do the implementation based on gettimeofday().
#if !defined(PENTIUM) && !defined(WIN32)
#ifndef macintosh
#include <sys/time.h>
#include <unistd.h>
static inline void getClockCount (unsigned long cc[2])
{
struct timeval tv;
gettimeofday (&tv,0);
cc[0] = tv.tv_usec;
cc[1] = tv.tv_sec;
}
#else // macintosh
#include <MacTypes.h>
#include <Timer.h>
static inline void getClockCount (unsigned long cc[2])
{
UnsignedWide ms;
Microseconds (&ms);
cc[1] = ms.lo / 1000000;
cc[0] = ms.lo - ( cc[1] * 1000000 );
}
#endif
static inline void serialize()
{
}
static inline double loadClockCount (unsigned long a[2])
{
return a[1]*1.0e6 + a[0];
}
double dTimerResolution()
{
unsigned long cc1[2],cc2[2];
getClockCount (cc1);
do {
getClockCount (cc2);
}
while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
do {
getClockCount (cc1);
}
while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
double t1 = loadClockCount (cc1);
double t2 = loadClockCount (cc2);
return (t1-t2) / dTimerTicksPerSecond();
}
double dTimerTicksPerSecond()
{
return 1000000;
}
#endif
//****************************************************************************
// stop watches
void dStopwatchReset (dStopwatch *s)
{
s->time = 0;
s->cc[0] = 0;
s->cc[1] = 0;
}
void dStopwatchStart (dStopwatch *s)
{
serialize();
getClockCount (s->cc);
}
void dStopwatchStop (dStopwatch *s)
{
unsigned long cc[2];
serialize();
getClockCount (cc);
double t1 = loadClockCount (s->cc);
double t2 = loadClockCount (cc);
s->time += t2-t1;
}
double dStopwatchTime (dStopwatch *s)
{
return s->time / dTimerTicksPerSecond();
}
//****************************************************************************
// code timers
// maximum number of events to record
#define MAXNUM 100
static int num = 0; // number of entries used in event array
static struct {
unsigned long cc[2]; // clock counts
double total_t; // total clocks used in this slot.
double total_p; // total percentage points used in this slot.
int count; // number of times this slot has been updated.
char *description; // pointer to static string
} event[MAXNUM];
// make sure all slot totals and counts reset to 0 at start
static void initSlots()
{
static int initialized=0;
if (!initialized) {
for (int i=0; i<MAXNUM; i++) {
event[i].count = 0;
event[i].total_t = 0;
event[i].total_p = 0;
}
initialized = 1;
}
}
void dTimerStart (const char *description)
{
initSlots();
event[0].description = const_cast<char*> (description);
num = 1;
serialize();
getClockCount (event[0].cc);
}
void dTimerNow (const char *description)
{
if (num < MAXNUM) {
// do not serialize
getClockCount (event[num].cc);
event[num].description = const_cast<char*> (description);
num++;
}
}
void dTimerEnd()
{
if (num < MAXNUM) {
serialize();
getClockCount (event[num].cc);
event[num].description = "TOTAL";
num++;
}
}
//****************************************************************************
// print report
static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt)
{
if (a >= 0.999999) {
fprintf (f,fmt,a);
return;
}
a *= 1000.0;
if (a >= 0.999999) {
fprintf (f,fmt,a);
fprintf (f,"m");
return;
}
a *= 1000.0;
if (a >= 0.999999) {
fprintf (f,fmt,a);
fprintf (f,"u");
return;
}
a *= 1000.0;
fprintf (f,fmt,a);
fprintf (f,"n");
}
void dTimerReport (FILE *fout, int average)
{
int i;
size_t maxl;
double ccunit = 1.0/dTimerTicksPerSecond();
fprintf (fout,"\nTimer Report (");
fprintDoubleWithPrefix (fout,ccunit,"%.2f ");
fprintf (fout,"s resolution)\n------------\n");
if (num < 1) return;
// get maximum description length
maxl = 0;
for (i=0; i<num; i++) {
size_t l = strlen (event[i].description);
if (l > maxl) maxl = l;
}
// calculate total time
double t1 = loadClockCount (event[0].cc);
double t2 = loadClockCount (event[num-1].cc);
double total = t2 - t1;
if (total <= 0) total = 1;
// compute time difference for all slots except the last one. update totals
double *times = (double*) ALLOCA (num * sizeof(double));
for (i=0; i < (num-1); i++) {
double t1 = loadClockCount (event[i].cc);
double t2 = loadClockCount (event[i+1].cc);
times[i] = t2 - t1;
event[i].count++;
event[i].total_t += times[i];
event[i].total_p += times[i]/total * 100.0;
}
// print report (with optional averages)
for (i=0; i<num; i++) {
double t,p;
if (i < (num-1)) {
t = times[i];
p = t/total * 100.0;
}
else {
t = total;
p = 100.0;
}
fprintf (fout,"%-*s %7.2fms %6.2f%%",maxl,event[i].description,
t*ccunit * 1000.0, p);
if (average && i < (num-1)) {
fprintf (fout," (avg %7.2fms %6.2f%%)",
(event[i].total_t / event[i].count)*ccunit * 1000.0,
event[i].total_p / event[i].count);
}
fprintf (fout,"\n");
}
fprintf (fout,"\n");
}

374
ode/src/util.cpp Normal file
View File

@ -0,0 +1,374 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#include "ode/ode.h"
#include "objects.h"
#include "joint.h"
#include "util.h"
#define ALLOCA dALLOCA16
//****************************************************************************
// Auto disabling
void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
{
dxBody *bb;
for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
{
// don't freeze objects mid-air (patch 1586738)
if ( bb->firstjoint == NULL ) continue;
// nothing to do unless this body is currently enabled and has
// the auto-disable flag set
if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
// if sampling / threshold testing is disabled, we can never sleep.
if ( bb->adis.average_samples == 0 ) continue;
//
// see if the body is idle
//
#ifndef dNODEBUG
// sanity check
if ( bb->average_counter >= bb->adis.average_samples )
{
dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
// something is going wrong, reset the average-calculations
bb->average_ready = 0; // not ready for average calculation
bb->average_counter = 0; // reset the buffer index
}
#endif // dNODEBUG
// sample the linear and angular velocity
bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
bb->average_counter++;
// buffer ready test
if ( bb->average_counter >= bb->adis.average_samples )
{
bb->average_counter = 0; // fill the buffer from the beginning
bb->average_ready = 1; // this body is ready now for average calculation
}
int idle = 0; // Assume it's in motion unless we have samples to disprove it.
// enough samples?
if ( bb->average_ready )
{
idle = 1; // Initial assumption: IDLE
// the sample buffers are filled and ready for calculation
dVector3 average_lvel, average_avel;
// Store first velocity samples
average_lvel[0] = bb->average_lvel_buffer[0][0];
average_avel[0] = bb->average_avel_buffer[0][0];
average_lvel[1] = bb->average_lvel_buffer[0][1];
average_avel[1] = bb->average_avel_buffer[0][1];
average_lvel[2] = bb->average_lvel_buffer[0][2];
average_avel[2] = bb->average_avel_buffer[0][2];
// If we're not in "instantaneous mode"
if ( bb->adis.average_samples > 1 )
{
// add remaining velocities together
for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
{
average_lvel[0] += bb->average_lvel_buffer[i][0];
average_avel[0] += bb->average_avel_buffer[i][0];
average_lvel[1] += bb->average_lvel_buffer[i][1];
average_avel[1] += bb->average_avel_buffer[i][1];
average_lvel[2] += bb->average_lvel_buffer[i][2];
average_avel[2] += bb->average_avel_buffer[i][2];
}
// make average
dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
average_lvel[0] *= r1;
average_avel[0] *= r1;
average_lvel[1] *= r1;
average_avel[1] *= r1;
average_lvel[2] *= r1;
average_avel[2] *= r1;
}
// threshold test
dReal av_lspeed, av_aspeed;
av_lspeed = dDOT( average_lvel, average_lvel );
if ( av_lspeed > bb->adis.linear_average_threshold )
{
idle = 0; // average linear velocity is too high for idle
}
else
{
av_aspeed = dDOT( average_avel, average_avel );
if ( av_aspeed > bb->adis.angular_average_threshold )
{
idle = 0; // average angular velocity is too high for idle
}
}
}
// if it's idle, accumulate steps and time.
// these counters won't overflow because this code doesn't run for disabled bodies.
if (idle) {
bb->adis_stepsleft--;
bb->adis_timeleft -= stepsize;
}
else {
// Reset countdowns
bb->adis_stepsleft = bb->adis.idle_steps;
bb->adis_timeleft = bb->adis.idle_time;
}
// disable the body if it's idle for a long enough time
if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
{
bb->flags |= dxBodyDisabled; // set the disable flag
// disabling bodies should also include resetting the velocity
// should prevent jittering in big "islands"
bb->lvel[0] = 0;
bb->lvel[1] = 0;
bb->lvel[2] = 0;
bb->avel[0] = 0;
bb->avel[1] = 0;
bb->avel[2] = 0;
}
}
}
//****************************************************************************
// body rotation
// return sin(x)/x. this has a singularity at 0 so special handling is needed
// for small arguments.
static inline dReal sinc (dReal x)
{
// if |x| < 1e-4 then use a taylor series expansion. this two term expansion
// is actually accurate to one LS bit within this range if double precision
// is being used - so don't worry!
if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
else return dSin(x)/x;
}
// given a body b, apply its linear and angular rotation over the time
// interval h, thereby adjusting its position and orientation.
void dxStepBody (dxBody *b, dReal h)
{
int j;
// handle linear velocity
for (j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
}
//****************************************************************************
// island processing
// this groups all joints and bodies in a world into islands. all objects
// in an island are reachable by going through connected bodies and joints.
// each island can be simulated separately.
// note that joints that are not attached to anything will not be included
// in any island, an so they do not affect the simulation.
//
// this function starts new island from unvisited bodies. however, it will
// never start a new islands from a disabled body. thus islands of disabled
// bodies will not be included in the simulation. disabled bodies are
// re-enabled if they are found to be part of an active island.
void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
{
dxBody *b,*bb,**body;
dxJoint *j,**joint;
// nothing to do if no bodies
if (world->nb <= 0) return;
// handle auto-disabling of bodies
dInternalHandleAutoDisabling (world,stepsize);
// make arrays for body and joint lists (for a single island) to go into
body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
int bcount = 0; // number of bodies in `body'
int jcount = 0; // number of joints in `joint'
// set all body/joint tags to 0
for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
// allocate a stack of unvisited bodies in the island. the maximum size of
// the stack can be the lesser of the number of bodies or joints, because
// new bodies are only ever added to the stack by going through untagged
// joints. all the bodies in the stack must be tagged!
int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
// get bb = the next enabled, untagged body, and tag it
if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
bb->tag = 1;
// tag all bodies and joints starting from bb.
int stacksize = 0;
b = bb;
body[0] = bb;
bcount = 1;
jcount = 0;
goto quickstart;
while (stacksize > 0) {
b = stack[--stacksize]; // pop body off stack
body[bcount++] = b; // put body on body list
quickstart:
// traverse and tag all body's joints, add untagged connected bodies
// to stack
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
if (!n->joint->tag) {
n->joint->tag = 1;
joint[jcount++] = n->joint;
if (n->body && !n->body->tag) {
n->body->tag = 1;
stack[stacksize++] = n->body;
}
}
}
dIASSERT(stacksize <= world->nb);
dIASSERT(stacksize <= world->nj);
}
// now do something with body and joint lists
stepper (world,body,bcount,joint,jcount,stepsize);
// what we've just done may have altered the body/joint tag values.
// we must make sure that these tags are nonzero.
// also make sure all bodies are in the enabled state.
int i;
for (i=0; i<bcount; i++) {
body[i]->tag = 1;
body[i]->flags &= ~dxBodyDisabled;
}
for (i=0; i<jcount; i++) joint[i]->tag = 1;
}
// if debugging, check that all objects (except for disabled bodies,
// unconnected joints, and joints that are connected to disabled bodies)
// were tagged.
# ifndef dNODEBUG
for (b=world->firstbody; b; b=(dxBody*)b->next) {
if (b->flags & dxBodyDisabled) {
if (b->tag) dDebug (0,"disabled body tagged");
}
else {
if (!b->tag) dDebug (0,"enabled body not tagged");
}
}
for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
(j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
if (!j->tag) dDebug (0,"attached enabled joint not tagged");
}
else {
if (j->tag) dDebug (0,"unattached or disabled joint tagged");
}
}
# endif
}

38
ode/src/util.h Normal file
View File

@ -0,0 +1,38 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 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. *
* *
*************************************************************************/
#ifndef _ODE_UTIL_H_
#define _ODE_UTIL_H_
#include "objects.h"
void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize);
void dxStepBody (dxBody *b, dReal h);
typedef void (*dstepper_fn_t) (dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *_joint, int nj, dReal stepsize);
void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper);
#endif