[Add] Use new collider in Lephisto. It's incomplete and slow due to a

lack of object instance space accel

src/collider/collider.h

@@ -1,5 +1,5 @@
 #pragma once
-#include "../vector.h"
+#include "../vector3.h"
 #include "geom_tree.h"
 #include "collision_space.h"
 #include "geom.h"

src/collider/collision_space.cpp

@@ -8,6 +8,7 @@ CollisionSpace::CollisionSpace(void) {
 
 void CollisionSpace::AddGeom(Geom* geom) {
   m_geoms.push_back(geom);
+  printf("%d geoms in space\n", m_geoms.size());
 }
 
 void CollisionSpace::RemoveGeom(Geom* geom) {
@@ -27,3 +28,17 @@ void CollisionSpace::TraceRay(const vector3d& start, const vector3d& dir, isect_
   }
 }
 
+void CollisionSpace::CollideGeoms(Geom* a, Geom* b, void (*callback)(CollisionContact*)) {
+  a->Collide(b, callback);
+  b->Collide(a, callback);
+}
+
+void CollisionSpace::Collide(void (*callback)(CollisionContact*)) {
+  /* For now, do a totally stupid intersection of every body on every other. */
+  for(std::list<Geom*>::iterator i = m_geoms.begin(); i != m_geoms.end(); ++i) {
+    for(std::list<Geom*>::iterator j = m_geoms.begin(); j != m_geoms.end(); ++j) {
+      if((*i) != (*j))CollideGeoms(*i, *j, callback);
+    }
+  }
+}
+

src/collider/collision_space.h

@@ -4,6 +4,7 @@
 
 class Geom;
 struct isect_t;
+class CollisionContact;
 
 class CollisionSpace {
 public:
@@ -11,7 +12,9 @@ public:
   void AddGeom(Geom*);
   void RemoveGeom(Geom*);
   void TraceRay(const vector3d& start, const vector3d& dir, isect_t* isect);
+  void Collide(void (*callback)(CollisionContact*));
 private:
+  void CollideGeoms(Geom* a, Geom* b, void (*callback)(CollisionContact*));
   std::list<Geom*> m_geoms;
 };
 

src/collider/geom.cpp

@@ -1,29 +1,66 @@
+#include <float.h>
 #include "geom.h"
+#include "geom_tree.h"
+#include "collider.h"
 
 Geom::Geom(GeomTree* geomtree) {
   m_geomtree = geomtree;
-  m_orient = matrix4x4d::Identity();
+  m_orient[0] = matrix4x4d::Identity();
+  m_orient[1] = matrix4x4d::Identity();
   m_invOrient = matrix4x4d::Identity();
+  m_orientIdx = 0;
   m_active = true;
+  m_data = 0;
 }
 
-void Geom::SetPosition(vector3d pos) {
+void Geom::MoveTo(const matrix4x4d& m) {
-  m_orient[12] = pos.x;
+  m_orientIdx = !m_orientIdx;
-  m_orient[13] = pos.y;
+  m_orient[m_orientIdx] = m;
-  m_orient[14] = pos.z;
+  m_invOrient = m.InverseOf();
-  m_invOrient = m_orient.InverseOf();
 }
 
-void Geom::SetOrientation(const matrix4x4d& rot) {
+void Geom::MoveTo(const matrix4x4d& m, const vector3d pos) {
-  m_orient[0] = rot[0];
+  m_orientIdx = !m_orientIdx;
-  m_orient[1] = rot[1];
+  m_orient[m_orientIdx] = m;
-  m_orient[2] = rot[2];
+  m_orient[m_orientIdx][12] = pos.x;
-  m_orient[4] = rot[4];
+  m_orient[m_orientIdx][13] = pos.y;
-  m_orient[5] = rot[5];
+  m_orient[m_orientIdx][14] = pos.z;
-  m_orient[6] = rot[6];
+  m_invOrient = m_orient[m_orientIdx].InverseOf();
-  m_orient[8] = rot[8];
+}
-  m_orient[9] = rot[9];
+
-  m_orient[10] = rot[10];
+void Geom::Collide(Geom* b, void(*callback)(CollisionContact*)) {
-  m_invOrient = m_orient.InverseOf();
+  for(int i = 0; i < m_geomtree->m_numVertices; i++) {
+    vector3d v(&m_geomtree->m_vertices[3*i]);
+    vector3d from = m_orient[!m_orientIdx] * v;
+    vector3d to = m_orient[m_orientIdx] * v;
+    from = b->m_invOrient * from;
+    to = b->m_invOrient * to;
+    vector3d dir = to - from;
+    const double len = dir.Length();
+    dir *= 1.0f/len;
+
+    vector3f _from(from.x, from.y, from.z);
+    vector3f _dir(dir.x, dir.y, dir.z);
+
+    isect_t isect;
+    isect.dist = len;
+    isect.triIdx = -1;
+    _dir.Normalize();
+    b->m_geomtree->TraceRay(_from, _dir, &isect);
+    if(isect.triIdx != -1) {
+      CollisionContact c;
+      /* In world coords. */
+      c.pos = b->GetTransform() * (from + dir*isect.dist);
+      vector3f n = b->m_geomtree->GetTriNormal(isect.triIdx);
+      c.normal = vector3d(n.x, n.y, n.z);
+      c.normal = b->GetTransform().ApplyRotationOnly(c.normal);
+
+      c.depth = len - isect.dist;
+      c.triIdx = isect.triIdx;
+      c.g1 = this;
+      c.g2 = b;
+      (*callback)(&c);
+    }
+  }
 }
 

src/collider/geom.h

@@ -3,20 +3,29 @@
 #include "../vector3.h"
 
 class GeomTree;
+class isect_t;
+class CollisionContact;
 
 class Geom {
 public:
   Geom(GeomTree*);
-  void SetPosition(vector3d pos);
+  void MoveTo(const matrix4x4d& m);
-  void SetOrientation(const matrix4x4d& rot);
+  void MoveTo(const matrix4x4d& m, const vector3d pos);
   const matrix4x4d& GetInvTransform(void) { return m_invOrient; }
+  const matrix4x4d& GetTransform(void) { return m_orient[m_orientIdx]; }
   void Enable(void) { m_active = true; }
   void Disable(void) {m_active = false; }
   bool IsEnabled(void) { return m_active; }
   GeomTree* GetGeomTree(void) { return m_geomtree; }
+  void Collide(Geom* b, void(*callback)(CollisionContact*));
+  void SetUserData(void* d) { m_data = d; }
+  void* GetUserData(void) { return m_data; }
 private:
-  matrix4x4d m_orient, m_invOrient;
+  /* double-buffer position so we can keep previous position. */
+  matrix4x4d m_orient[2], m_invOrient;
+  int m_orientIdx;
   bool m_active;
   GeomTree* m_geomtree;
+  void* m_data;
 };
 

src/collider/geom_tree.cpp

@@ -78,7 +78,7 @@ GeomTree::~GeomTree(void) {
   delete [] m_triAlloc;
 }
 
-GeomTree::GeomTree(int numTris, float* vertices, int* indices) {
+GeomTree::GeomTree(int numVerts, int numTris, float* vertices, int* indices) : m_numVertices(numVerts) {
   m_vertices = vertices;
   m_indices = indices;
   m_aabb.min = vector3d(FLT_MAX, FLT_MAX, FLT_MAX);
@@ -255,15 +255,11 @@ void GeomTree::BihTreeGhBuild(BIHNode* a_node, Aabb& a_box, Aabb& a_splitBox, in
   else right->SetLeaf(true);
 }
 
-void GeomTree::TraceRay(vector3f& start, vector3f& dir, isect_t* isect) {
+void GeomTree::TraceRay(const vector3f& start, const vector3f& dir, isect_t* isect) {
-  float len = dir.Length();
+  TraverseRay(start, dir, isect);
-  isect->dist = len;
-  isect->triIdx = -1;
-  vector3f normDir = dir*(1.0f/len);
-  TraverseRay(start, normDir, isect);
 }
 
-void GeomTree::TraverseRay(vector3f& a_origin, vector3f& a_dir, isect_t* isect) {
+inline void GeomTree::TraverseRay(const vector3f& a_origin, const vector3f& a_dir, isect_t* isect) {
   float entry_t = 0, exit_t = isect->dist;
   vector3f rcpD = vector3f(1.0f/a_dir.x, 1.0f/a_dir.y, 1.0f/a_dir.z);
   int Dneg[3];
@@ -374,25 +370,24 @@ pop_bstack:
 }
 
 #define EPSILON 0.00001f
-void GeomTree::RayTriIntersect(vector3f& origin, vector3f& dir, int triIdx, isect_t* isect) {
+void GeomTree::RayTriIntersect(const vector3f& origin, const vector3f& dir, int triIdx, isect_t* isect) {
-  vector3f a(&m_vertices[3*m_indices[triIdx]]);
+  const vector3f a(&m_vertices[3*m_indices[triIdx]]);
-  vector3f b(&m_vertices[3*m_indices[triIdx+1]]);
+  const vector3f b(&m_vertices[3*m_indices[triIdx+1]]);
-  vector3f c(&m_vertices[3*m_indices[triIdx+2]]);
+  const vector3f c(&m_vertices[3*m_indices[triIdx+2]]);
 
   vector3f v0_cross, v1_cross, v2_cross;
-  const vector3f n = vector3f::Cross(c-a, b-a);
 
+  v0_cross = vector3f::Cross(c-origin, b-origin);
   v1_cross = vector3f::Cross(b-origin, a-origin);
   v2_cross = vector3f::Cross(a-origin, c-origin);
-  v0_cross = vector3f::Cross(c-origin, b-origin);
-  float nominator = vector3f::Dot(n, (a-origin));
 
   const float v0d = vector3f::Dot(v0_cross,dir);
   const float v1d = vector3f::Dot(v1_cross,dir);
   const float v2d = vector3f::Dot(v2_cross,dir);
 
-  if(((v0d > 0) && (v1d > 0) && (v2d > 0)) ||
+  if((v0d > 0) && (v1d > 0) && (v2d > 0)) {
-      ((v0d < 0) && (v1d < 0 && v2d < 0))) {
+    const vector3f n = vector3f::Cross(c-a, b-a);
+    const float nominator = vector3f::Dot(n, (a-origin));
     const float dist = nominator / vector3f::Dot(dir, n);
     if((dist > EPSILON) && (dist < isect->dist)) {
       isect->dist = dist;
@@ -401,3 +396,11 @@ void GeomTree::RayTriIntersect(vector3f& origin, vector3f& dir, int triIdx, isec
   }
 }
 
+vector3f GeomTree::GetTriNormal(int triIdx) const {
+  const vector3f a(&m_vertices[3*m_indices[triIdx]]);
+  const vector3f b(&m_vertices[3*m_indices[triIdx+1]]);
+  const vector3f c(&m_vertices[3*m_indices[triIdx+2]]);
+
+  return vector3f::Normalize(vector3f::Cross(b-a, c-a));
+}
+

src/collider/geom_tree.h

@@ -12,15 +12,24 @@ struct isect_t {
 
 class GeomTree {
 public:
-  GeomTree(int numTris, float* vertices, int* indices);
+  GeomTree(int numVerts, int numTris, float* vertices, int* indices);
   ~GeomTree(void);
   Aabb GetAabb(void) { return m_aabb; }
-  void TraceRay(vector3f& start, vector3f& dir, isect_t* isect);
+  /*
+   * dir should be unit length,
+   * isect.dist should be ray length.
+   * isect.triIdx should be -1 unless repeat calls with same isect_t.
+   */
+  void TraceRay(const vector3f& start, const vector3f& dir, isect_t* isect);
+  vector3f GetTriNormal(int triIdx) const;
+
+  const int m_numVertices;
+  const float* m_vertices;
 
 private:
   friend class BIHNode;
-  void RayTriIntersect(vector3f& a_origin, vector3f& a_dir, int triIdx, isect_t* isect);
+  void RayTriIntersect(const vector3f& a_origin, const vector3f& a_dir, int triIdx, isect_t* isect);
-  void TraverseRay(vector3f& a_origin, vector3f& a_dir, isect_t* isect);
+  void TraverseRay(const vector3f& a_origin, const vector3f& a_dir, isect_t* isect);
   BIHNode* AllocNode(void);
   void BihTreeGhBuild(BIHNode* a_node, Aabb& a_box, Aabb& a_splitBox, int a_depth, int a_prims);
 
@@ -32,7 +41,6 @@ private:
   int m_triAllocPos;
   int m_triAllocSize;
 
-  const float* m_vertices;
   const int* m_indices;
 };
 

src/frame.cpp

@@ -1,6 +1,7 @@
 #include "frame.h"
 #include "space.h"
 #include "serializer.h"
+#include "collider/collider.h"
 
 Frame::Frame(void) {
   Init(NULL, "", 0);
@@ -74,6 +75,7 @@ void Frame::Init(Frame* parent, const char* label, unsigned int flags) {
   m_angVel    = vector3d(0.0);
   m_orient    = matrix4x4d::Identity();
   m_dSpaceID  = dHashSpaceCreate(0);
+  m_collisionSpace = new CollisionSpace();
   if(m_parent) {
     m_parent->m_children.push_back(this);
   }
@@ -82,9 +84,13 @@ void Frame::Init(Frame* parent, const char* label, unsigned int flags) {
 
 Frame::~Frame(void) {
   dSpaceDestroy(m_dSpaceID);
+  delete m_collisionSpace;
   for(std::list<Frame*>::iterator i = m_children.begin(); i != m_children.end(); ++i) delete *i;
 }
 
+void Frame::AddGeom(Geom* g) { m_collisionSpace->AddGeom(g); }
+void Frame::RemoveGeom(Geom* g) { m_collisionSpace->RemoveGeom(g); }
+
 void Frame::ApplyLeavingTransform(matrix4x4d& m) const {
   m = matrix4x4d::Translation(m_pos) * m_orient * m;
 }

src/frame.h

@@ -5,6 +5,8 @@
 #include "star_system.h"
 
 class Body;
+class CollisionSpace;
+class Geom;
 
 /* Frame of reference. */
 class Frame {
@@ -30,9 +32,12 @@ public:
   void SetOrientation(const matrix4x4d& m)      { m_orient = m; }
   void SetRadius(double radius)                 { m_radius = radius; }
   void RemoveChild(Frame* f);
-  void AddGeom(dGeomID g)                       { dSpaceAdd(m_dSpaceID, g); }
+  void _AddGeom(dGeomID g)                       { dSpaceAdd(m_dSpaceID, g); }
-  void RemoveGeom(dGeomID g)                    { dSpaceRemove(m_dSpaceID, g); }
+  void _RemoveGeom(dGeomID g)                    { dSpaceRemove(m_dSpaceID, g); }
+  void AddGeom(Geom*);
+  void RemoveGeom(Geom*);
   dSpaceID GetSpaceID(void) const               { return m_dSpaceID; }
+  CollisionSpace* GetCollisionSpace(void) const { return m_collisionSpace; }
   void RotateInTimestep(double step);
 
 
@@ -64,5 +69,6 @@ private:
   double m_radius;
   int m_flags;
   dSpaceID m_dSpaceID;
+  CollisionSpace* m_collisionSpace;
 };
 

src/main.cpp

@@ -173,6 +173,13 @@ void L3D::HandleEvents(void) {
         body->SetFrame(L3D::player->GetFrame());
         body->SetPosition(L3D::player->GetPosition()+vector3d(0,0,-1000));
         Space::AddBody(body);
+#if 0
+        SpaceStation* station = new SpaceStation(SpaceStation::JJHOOP);
+        station->SetLabel("Poemi-chan's Folly");
+        station->SetFrame(L3D::player->GetFrame());
+        station->SetPosition(L3D::player->GetPosition()+vector3d(0,0,-1000));
+        Space::AddBody(station);
+#endif
       }
 #endif
       if(event.key.keysym.sym == SDLK_F11) SDL_WM_ToggleFullScreen(L3D::scrSurface);

src/model_body.cpp

@@ -7,10 +7,12 @@
 #include "world_view.h"
 #include "model_coll_mesh_data.h"
 #include "serializer.h"
+#include "collider/collider.h"
 
 ModelBody::ModelBody(void): Body() {
   m_triMeshLastMatrixIndex = 0;
   m_collMeshSet = 0;
+  m_geom = 0;
 }
 
 ModelBody::~ModelBody(void) {
@@ -54,8 +56,11 @@ void ModelBody::GetAabb(Aabb& aabb) {
 
 void ModelBody::SetModel(int sbreModel) {
   assert(geoms.size() == 0);
+  assert(m_geom == 0);
   CollMeshSet* mset = GetModelCollMeshSet(sbreModel);
 
+  m_geom = new Geom(mset->m_geomTree);
+
   geomColl.resize(mset->numMeshParts);
   geoms.resize(mset->numMeshParts);
 
@@ -69,6 +74,10 @@ void ModelBody::SetModel(int sbreModel) {
 }
 
 void ModelBody::SetPosition(vector3d p) {
+  matrix4x4d m;
+  GetRotMatrix(m);
+  m_geom->MoveTo(m, p);
+
   for(unsigned int i = 0; i < geoms.size(); i++) {
     dGeomSetPosition(geoms[i], p.x, p.y, p.z);
   }
@@ -136,14 +145,16 @@ void ModelBody::TransformToModelCoords(const Frame* camFrame) {
 void ModelBody::SetFrame(Frame* f) {
   if(GetFrame()) {
     for(unsigned int i = 0; i < geoms.size(); i++) {
-      GetFrame()->RemoveGeom(geoms[i]);
+      GetFrame()->_RemoveGeom(geoms[i]);
     }
+    GetFrame()->RemoveGeom(m_geom);
   }
   Body::SetFrame(f);
   if(f) {
     for(unsigned int i = 0; i < geoms.size(); i++) {
-      f->AddGeom(geoms[i]);
+      f->_AddGeom(geoms[i]);
     }
+    f->AddGeom(m_geom);
   }
 }
 
@@ -156,6 +167,13 @@ void ModelBody::TriMeshUpdateLastPos(void) {
   t[ 4] = r[4]; t[5] = r[5]; t[ 6] = r[ 6]; t[ 7] = 0;
   t[ 8] = r[8]; t[9] = r[9]; t[10] = r[10]; t[11] = 0;
   t[12] = pos.x; t[13] = pos.y; t[14] = pos.z; t[15] = 1;
+
+  /* REESTED! */
+  matrix4x4d wtf;
+  memcpy(&wtf[0], t, 16*sizeof(double));
+  m_geom->MoveTo(wtf);
+  m_geom->SetUserData(dGeomGetBody(geoms[0]));
+
   m_triMeshLastMatrixIndex = !m_triMeshLastMatrixIndex;
   for(unsigned int i = 0; i < geoms.size(); i++) {
     dGeomTriMeshSetLastTransform(geoms[i], *(dMatrix4*)(m_triMeshTrans + 16*m_triMeshLastMatrixIndex));

src/model_body.h

@@ -7,6 +7,7 @@
 
 class ObjMesh;
 class CollMeshSet;
+class Geom;
 
 class ModelBody: public Body {
 public:
@@ -32,19 +33,20 @@ public:
 
   void RenderSbreModel(const Frame* camFrame, int model, ObjParams* params);
   
-  class Geom : public Object {
+  class GeomBit : public Object {
   public:
-    OBJDEF(Geom, Object, GEOM);
+    OBJDEF(GeomBit, Object, GEOM);
     Body* parent;
     int flags;
   };
 protected:
   virtual void Save(void);
   virtual void Load(void);
-  std::vector<Geom> geomColl;
+  std::vector<GeomBit> geomColl;
 private:
   CollMeshSet* m_collMeshSet;
   std::vector<dGeomID> geoms;
+  Geom* m_geom;
   dReal m_triMeshTrans[32];
   int m_triMeshLastMatrixIndex;
 };

src/model_coll_mesh_data.cpp

@@ -3,6 +3,7 @@
 #include "libs.h"
 #include "model_coll_mesh_data.h"
 #include "sbre/sbre.h"
+#include "collider/collider.h"
 
 /*
  * In order to do space station doors and other flagged bits of collision
@@ -81,6 +82,7 @@ CollMeshSet::CollMeshSet(int sbreModel) {
         aabb.max[a] = sbreCollMesh->pVertex[i+a];
     }
   }
+  m_geomTree = new GeomTree(sbreCollMesh->nv, sbreCollMesh->ni/3, sbreCollMesh->pVertex, sbreCollMesh->pIndex);
 
   triIndices = new coltri_t[sbreCollMesh->ni/3];
   int tidx = 0;

src/model_coll_mesh_data.h

@@ -2,6 +2,8 @@
 #include "libs.h"
 #include "sbre/sbre.h"
 
+class GeomTree;
+
 struct coltri_t {
   int v1, v2, v3, flags;
 };
@@ -21,6 +23,8 @@ public:
   int numMeshParts;
   Aabb aabb;
 
+  GeomTree* m_geomTree;
+
   CollMeshSet(int sbreModel);
 private:
   void GetMeshParts(void);

src/planet.cpp

@@ -911,8 +911,8 @@ void Planet::Render(const Frame* a_camFrame) {
 }
 
 void Planet::SetFrame(Frame* f) {
-  if(GetFrame()) GetFrame()->RemoveGeom(geom);
+  if(GetFrame()) GetFrame()->_RemoveGeom(geom);
   Body::SetFrame(f);
-  if(f) f->AddGeom(geom);
+  if(f) f->_AddGeom(geom);
 }
 

src/sbre_viewer.cpp

@@ -2,9 +2,7 @@
 #include "sbre/sbre.h"
 #include "glfreetype.h"
 #include "gui.h"
-#include "collider/geom_tree.h"
+#include "collider/collider.h"
-#include "collider/collision_space.h"
-#include "collider/geom.h"
 
 static SDL_Surface* g_screen;
 static int g_width, g_height;
@@ -157,11 +155,12 @@ static void render_coll_mesh(const CollMesh* m) {
   glEnable(GL_LIGHTING);
 }
 
-float foo[512][512];
+#define TEXSIZE 512
+float foo[TEXSIZE][TEXSIZE];
 float aspectRatio = 1.0;
 float camera_zoom = 1.0;
 static void raytraceCollMesh(vector3d camPos, vector3d camera_up, vector3d camera_forward, CollisionSpace* space) {
-  memset(foo, 0, sizeof(float)*512*512);
+  memset(foo, 0, sizeof(float)*TEXSIZE*TEXSIZE);
 
   vector3d toPoint, xMov, yMov;
 
@@ -174,20 +173,21 @@ static void raytraceCollMesh(vector3d camPos, vector3d camera_up, vector3d camer
 
   xMov = topRight - topLeft;
   yMov = botRight - topRight;
-  float xstep = 1.0f / 512;
+  float xstep = 1.0f / TEXSIZE;
-  float ystep = 1.0f / 512;
+  float ystep = 1.0f / TEXSIZE;
   float xpos, ypos;
   ypos = 0.0f;
 
   Uint32 t = SDL_GetTicks();
-  for(int y = 0; y < 512; y++, ypos += ystep) {
+  for(int y = 0; y < TEXSIZE; y++, ypos += ystep) {
     xpos = 0.0f;
-    for(int x = 0; x < 512; x++, xpos += xstep) {
+    for(int x = 0; x < TEXSIZE; x++, xpos += xstep) {
       toPoint = topLeft + (xMov * xpos) + (yMov*ypos);
       toPoint.Normalize();
-      toPoint *= 10000;
 
       isect_t isect;
+      isect.triIdx = -1;
+      isect.dist = 10000;
       space->TraceRay(camPos, toPoint, &isect);
 
       if(isect.triIdx != -1) {
@@ -197,8 +197,8 @@ static void raytraceCollMesh(vector3d camPos, vector3d camera_up, vector3d camer
       }
     }
   }
-  printf("%3f million rays/sec\n", (512*512)/(1000.0*(SDL_GetTicks()-t)));
+  printf("%3f million rays/sec\n", (TEXSIZE*TEXSIZE)/(1000.0*(SDL_GetTicks()-t)));
-  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 512, 512, 0, GL_LUMINANCE, GL_FLOAT, foo);
+  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, TEXSIZE, TEXSIZE, 0, GL_LUMINANCE, GL_FLOAT, foo);
 
   glDisable(GL_DEPTH_TEST);
   glDisable(GL_LIGHTING);
@@ -231,6 +231,11 @@ static void raytraceCollMesh(vector3d camPos, vector3d camera_up, vector3d camer
   glDisable(GL_TEXTURE_2D);
 }
 
+static void onCollision(CollisionContact* c) {
+  printf("depth %f\n", c->depth);
+  //printf("%d: %d\n", SDL_GetTicks(), c->triIdx);
+}
+
 void Viewer::MainLoop(void) {
   matrix4x4d rot = matrix4x4d::Identity();
   float distance = 100;
@@ -246,11 +251,15 @@ void Viewer::MainLoop(void) {
   }
 
   Uint32 t= SDL_GetTicks();
-  GeomTree* geomtree = new GeomTree(cmesh->ni/3, cmesh->pVertex, cmesh->pIndex);
+  GeomTree* geomtree = new GeomTree(cmesh->nv, cmesh->ni/3, cmesh->pVertex, cmesh->pIndex);
   printf("Geom tree build in $dms\n", SDL_GetTicks() -t);
   Geom* geom = new Geom(geomtree);
+  Geom* geom2 = new Geom(geomtree);
   CollisionSpace* space = new CollisionSpace();
+  space->AddGeom(geom2);
   space->AddGeom(geom);
+  geom2->MoveTo(rot, vector3d(80,0,0));
+  geom2->MoveTo(rot, vector3d(80,0,0));
 
   for(;;) {
     PollEvents();
@@ -267,6 +276,7 @@ void Viewer::MainLoop(void) {
       rot = matrix4x4d::RotateXMatrix(rx) * rot;
       rot = matrix4x4d::RotateYMatrix(ry) * rot;
     }
+    geom->MoveTo(rot, vector3d(0,0,0));
 
     glMatrixMode(GL_PROJECTION);
     glLoadIdentity();
@@ -299,7 +309,6 @@ void Viewer::MainLoop(void) {
       glPopMatrix();
       //sbreRenderCollMesh(cmesh, &p, &m);
     } else {
-      geom->SetOrientation(rot);
       vector3d camPos   = vector3d(0, 0, distance);
       vector3d forward  = vector3d(0,0,-1);
       vector3d up       = vector3d(0, 1, 0);
@@ -311,6 +320,8 @@ void Viewer::MainLoop(void) {
     SDL_GL_SwapBuffers();
     g_frameTime = (SDL_GetTicks() - lastFoo) * 0.001;
     lastFoo = SDL_GetTicks();
+
+    space->Collide(onCollision);
   }
   delete geomtree;
 }

src/space.cpp

@@ -12,6 +12,7 @@
 #include "space_station.h"
 #include "sbre/sbre.h"
 #include "serializer.h"
+#include "collider/collider.h"
 
 dWorldID Space::world;
 std::list<Body*> Space::bodies;
@@ -302,12 +303,12 @@ static bool _OnCollision(dGeomID g1, dGeomID g2, Object* o1, Object* o2,
     int flags = 0;
     /* Geom bodies point to their parents. */
     if(o1->IsType(Object::GEOM)) {
-      pb1 = static_cast<ModelBody::Geom*>(o1)->parent;
+      pb1 = static_cast<ModelBody::GeomBit*>(o1)->parent;
-      flags |= static_cast<ModelBody::Geom*>(o1)->flags;
+      flags |= static_cast<ModelBody::GeomBit*>(o1)->flags;
     } else pb1 = static_cast<Body*>(o1);
     if(o2->IsType(Object::GEOM)) {
-      pb2 = static_cast<ModelBody::Geom*>(o2)->parent;
+      pb2 = static_cast<ModelBody::GeomBit*>(o2)->parent;
-      flags |= static_cast<ModelBody::Geom*>(o2)->flags;
+      flags |= static_cast<ModelBody::GeomBit*>(o2)->flags;
     } else pb2 = static_cast<Body*>(o2);
 
     if((pb1 && !pb1->OnCollision(pb2, flags)) || (pb2 && !pb2->OnCollision(pb1, flags))) return false;
@@ -315,6 +316,46 @@ static bool _OnCollision(dGeomID g1, dGeomID g2, Object* o1, Object* o2,
   return true;
 }
 
+static void dump_contact(const dContact* c) {
+  printf("pos %f,%f,%f\n", c->geom.pos[0], c->geom.pos[1], c->geom.pos[2]);
+  printf("normal %f,%f,%f\n", c->geom.normal[0], c->geom.normal[1], c->geom.normal[2]);
+  printf("depth %f\n", c->geom.depth);
+  printf("side1:side2 %d:%d\n", c->geom.side1, c->geom.side2);
+  printf("fdir1 %f,%f,%f\n", c->fdir1[0], c->fdir1[1], c->fdir1[2]);
+}
+
+#define MAX_CONTACTS 10
+static int contact_num;
+static void hitCallback(CollisionContact* c) {
+  if(contact_num++ >= MAX_CONTACTS) return;
+  dContact contact;
+
+  contact.surface.mode = dContactBounce;
+  contact.surface.mu = 0.8;
+  contact.surface.mu2 = 0;
+  contact.surface.bounce = 0.1;
+  contact.surface.bounce_vel = 0.1;
+
+  contact.geom.pos[0] = c->pos.x;
+  contact.geom.pos[1] = c->pos.y;
+  contact.geom.pos[2] = c->pos.z;
+  contact.geom.pos[3] = 1;
+  contact.geom.normal[0] = c->normal.x;
+  contact.geom.normal[1] = c->normal.y;
+  contact.geom.normal[2] = c->normal.z;
+  contact.geom.normal[3] = 1;
+  contact.geom.depth = c->depth;
+  contact.geom.g1 = 0;
+  contact.geom.g2 = 0;
+  contact.fdir1[0] = 0;
+  contact.fdir1[1] = 0;
+  contact.fdir1[2] = 0;
+
+  dJointID j = dJointCreateContact(Space::world, _contactgroup, &contact);
+  dJointAttach(j, (dBodyID)c->g1->GetUserData(), (dBodyID)c->g2->GetUserData());
+}
+
+#if 0
 static void nearCallback(void* data, dGeomID oO, dGeomID o1) {
   /* Create an array of dContact objects to hold the contact joints. */
   static const int MAX_CONTACTS = 100;
@@ -339,13 +380,15 @@ static void nearCallback(void* data, dGeomID oO, dGeomID o1) {
      * which is just one of the many different types available.
      */
     for(int i = 0; i < numc; i++) {
+      printf("\nODE collision:\n);
+      dump_contact(contact+i);
       /*
        * dJointCreateContact needs to know which world and joint group to work
        * with as well as the dContact object itself.
        * It returns a new dJointID which we then use with dJointAttach to
        * finally create the temp contact joint between the two geom bodies.
        */
-      dJointID c = dJointCreateContact(Space::world, _contactgroup, contact + i);
+      //dJointID c = dJointCreateContact(Space::world, _contactgroup, contact + i);
 #if 0
       struct dContactGeom {
         dVector3  pos;    /* Constact position. */
@@ -355,13 +398,15 @@ static void nearCallback(void* data, dGeomID oO, dGeomID o1) {
       };
 #endif
 
-      dJointAttach(c, b1, b2);
+      //dJointAttach(c, b1, b2);
     }
   }
 }
+#endif
 
 void Space::CollideFrame(Frame* f) {
-  dSpaceCollide(f->GetSpaceID(), NULL, &nearCallback);
+  f->GetCollisionSpace()->Collide(&hitCallback);
+  //dSpaceCollide(f->GetSpaceID(), NULL, &nearCallback);
   for(std::list<Frame*>::iterator i = f->m_children.begin(); i != f->m_children.end(); ++i) {
     CollideFrame(*i);
   }
@@ -396,6 +441,7 @@ void Space::ApplyGravity(void) {
 void Space::TimeStep(float step) {
   ApplyGravity();
 
+  contact_num = 0;
   CollideFrame(rootFrame);
   dWorldQuickStep(world, step);
   dJointGroupEmpty(_contactgroup);