Changeset 1975 in orxonox.OLD for orxonox/branches/chris/core/collision.cc

Timestamp:

Jun 18, 2004, 7:56:18 AM (21 years ago)

Author:

chris

Message:

orxonox/branches/chris: Implemented traceing (line-sphere collision)

File:

• orxonox/branches/chris/core/collision.cc (modified) (3 diffs)

orxonox/branches/chris/core/collision.cc

@@ -74 +74 @@
 }
 
-bool check_trace (const Placement& pa, const CollisionCluster& a, unsigned long* ahitflags, const Line& trace, Vector* impactpoint)
-{
-  return false;
+bool check_trace (const Placement* pa, const CollisionCluster* a, unsigned long* ahitflags, const Line* trace, Vector* impactpoint)
+{
+        CC_Tree* t;
+        if( (t = a->root) == NULL) return false;
+        
+  return cctree_trace( pa, t, ahitflags, trace, impactpoint);
 }
 
@@ -94 +97 @@
 }
 
-bool trace_sphere_collision( Vector m, float r, Line& l, Vector* impactpoint)
-{
-  return false;
+bool trace_sphere_collision( Vector m, float r, const Line* l, Vector* impactpoint)
+{
+  float A, B, C, D, t[2];
+  Vector d = l->r - m;
+  int i;
+  
+  A = l->a * l->a;
+  B = 2 * (l->a * d);
+  C = (d*d) - r*r;
+  D = B*B - 4*A*C;
+  
+  if (D < 0) return false;
+  
+  t[0] = (-B+sqrt(D))/(2*A);
+  t[1] = (-B-sqrt(D))/(2*A);
+  
+  if( (t[0] > 1 || t[0] < 0) && (t[1] < 0 || t[1] > 1)) return false;
+  if( t[0] > t[1]) i = 0;
+  else i = 1;
+
+  impactpoint->x = (l->r + (l->a * t[i])).x;
+  impactpoint->y = (l->r + (l->a * t[i])).y;
+  impactpoint->z = (l->r + (l->a * t[i])).z;
+  
+  return true;
 }
 
@@ -237 +262 @@
   return 0;
 }
+
+bool cctree_trace( const Placement* p, CC_Tree* t, unsigned long* hitflags, const Line* trace, Vector* impactpoint)
+{
+  bool r = false;
+  int i;
+  Vector mr = p->r + rotate_vector( t->m, p->w);
+  CC_Tree* use_t;
+  Vector* ips;
+  unsigned long* hfs;
+  
+  if( trace_sphere_collision (mr, t->r, trace, impactpoint))
+  {
+        if( t->n == 0)
+        {
+                setflag (hitflags, t->data.ID);
+                return true;
+        }
+        else
+        {
+                ips = new Vector[t->n];
+                hfs = new unsigned long[t->n];
+                for (i = 0; i < t->n; i++) hfs[i] = 0;
+                for (i = 0; i < t->n; i++)
+                {
+                        r = r || cctree_trace (p, t->data.b[i], &(hfs[i]), trace, &(ips[i]));
+                }
+                if( r)
+                {
+                        float kl = 0.0;
+                        float l = 0.0;
+                        int k = 0;
+                        for (i = 0; i < t->n; i++)
+                        {
+                                if( (kl = (trace->r - ips[i]).len()) > l)
+                                {
+                                        l = kl;
+                                        k = i;
+                                }
+                        }
+                        impactpoint->x = ips[k].x;
+                        impactpoint->y = ips[k].y;
+                        impactpoint->z = ips[k].z;
+                        *hitflags = hfs[k];
+                }
+                delete ips;
+                delete hfs;
+        }
+        return r;
+  }
+  
+  return false;
+}