Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/branches/buerli/src/collision.cc @ 3161

Last change on this file since 3161 was 2190, checked in by bensch, 20 years ago

orxonox/trunk: merged and copied all files from branches/chris into trunk. it all seems to be in propper order.

File size: 9.3 KB
Line 
1
2
3/*
4   orxonox - the future of 3D-vertical-scrollers
5
6   Copyright (C) 2004 orx
7
8   This program is free software; you can redistribute it and/or modify
9   it under the terms of the GNU General Public License as published by
10   the Free Software Foundation; either version 2, or (at your option)
11   any later version.
12
13   ### File Specific:
14   main-programmer: Christian Meyer
15   co-programmer: ...
16*/
17
18
19#include "collision.h"
20
21using namespace std;
22
23/**
24   \brief simple CollisionCluster constructor
25   \param rad: the radius of the base sphere
26   \param mid: the middle point of the sphere
27*/
28CollisionCluster::CollisionCluster (float rad = 1.0, Vector mid = Vector(0,0,0))
29{
30  root = (CC_Tree*) malloc( sizeof( CC_Tree));
31  root->n = 0;
32  root->data.ID = 0;
33  root->r = rad;
34  root->m = mid;
35}
36
37/**
38   \brief loads a CollisionCluster from a data file
39   \param filename: self-explanatory
40*/
41CollisionCluster::CollisionCluster (char* filename)
42{
43  FILE* stream;
44  char hbuffer[3];
45  stream = fopen (filename, "rb");
46  if( stream == NULL)
47  {
48    root = NULL;
49    return;
50  }
51  if( fread (hbuffer, sizeof( char), 2, stream) != 2)
52  {
53    root = NULL;
54    return;
55  }
56 
57  hbuffer[2] = 0;
58  if( strcmp (hbuffer, "CC"))
59  {
60    root = NULL;
61    return;
62  }
63 
64  root = load_CC_Tree (stream);
65  fclose (stream);
66}
67
68/**
69   \brief Simple destructor that cleans up all the mess
70*/
71CollisionCluster::~CollisionCluster ()
72{
73  free_CC_Tree( root);
74}
75
76/**
77   \brief stores the CollisionCluster into a file
78   \param filename: self-explanatory
79   \return zero on success, -1 on error
80*/
81int CollisionCluster::store (char* filename)
82{
83  int r;
84  FILE* stream;
85  stream = fopen( filename, "wb");
86  if( stream == NULL) return -1;
87  r = save_CC_Tree (root, stream);
88  fclose (stream);
89  return r;
90}
91
92/**
93   \brief performs a collision check between a CollisionCluster and a Line
94   \param pa: pointer to the Placement of the CollisionCluster
95   \param a: pointer to the CollisionCluster
96   \param ahitflags: pointer to an unsigned long to store hitlocation data
97   \param trace: pointer to the Line
98   \param impactpoint: pointer to a Vector where the point of intersection is stored in
99   \return true on collision, false otherwise. If true is returned, the flag in ahitflags that symbolises the hit subsphere is set, and impactpoint is set to the Location where the intersection occured
100*/
101bool check_trace (const Placement* pa, const CollisionCluster* a, unsigned long* ahitflags, const Line* trace, Vector* impactpoint)
102{
103        CC_Tree* t;
104        if( (t = a->root) == NULL) return false;
105       
106  return cctree_trace( pa, t, ahitflags, trace, impactpoint);
107}
108
109/**
110   \brief performs a collision check between two CollisionClusters
111   \param pa: pointer to the Placement of the first CollisionCluster
112   \param a: pointer to the first CollisionCluster
113   \param ahitflags: pointer to an unsigned long to store hitlocation data on the first CollisionCluster
114   \param pb: pointer to the Placement of the second CollisionCluster
115   \param b: pointer to the second CollisionCluster
116   \param bhitflags: pointer to an unsigned long to store hitlocation data on the second CollisionCluster
117   \return true on collision, false otherwise
118   If true is returned, all flags in ahitflags and bhitflags that symbolize intersecting subspheres in the respective CollisionCluster are set
119*/
120bool check_collision (const Placement* pa, const CollisionCluster* a, unsigned long* ahitflags, const Placement* pb, const CollisionCluster* b, unsigned long* bhitflags)
121{
122  CC_Tree* ta, *tb;
123  if( (ta = a->root) == NULL) return false;
124  if( (tb = b->root) == NULL) return false;
125 
126  return cctree_iterate(pa, ta, ahitflags, pb, tb, bhitflags);
127}
128
129/**
130   \brief performs an intersection check between two spheres
131   \param m1: center point of first sphere
132   \param r1: radius of first sphere
133   \param m2: center point of second sphere
134   \param r2: radius of second sphere
135   \return true on intersection, false otherwise
136*/
137bool sphere_sphere_collision( Vector m1, float r1, Vector m2, float r2)
138{
139  if ((m1-m2).len() < r1+r2) return true;
140  return false;
141}
142
143/**
144   \brief performs an intersection check between a Line and a sphere
145   \param m: center point of the sphere
146   \param r: radius of the sphere
147   \param l: pointer to the Line
148   \param impactpoint: pointer to a buffer to store the point of intersection
149   \return true on intersection, false otherwise. If true is returned, impactpoint is set to the loaction where the intersection occured
150*/
151bool trace_sphere_collision( Vector m, float r, const Line* l, Vector* impactpoint)
152{
153  float A, B, C, D, t[2];
154  Vector d = l->r - m;
155  int i;
156 
157  A = l->a * l->a;
158  B = 2 * (l->a * d);
159  C = (d*d) - r*r;
160  D = B*B - 4*A*C;
161 
162  if (D < 0) return false;
163 
164  t[0] = (-B+sqrt(D))/(2*A);
165  t[1] = (-B-sqrt(D))/(2*A);
166 
167  if( (t[0] > 1 || t[0] < 0) && (t[1] < 0 || t[1] > 1)) return false;
168  if( t[0] > t[1]) i = 0;
169  else i = 1;
170
171  impactpoint->x = (l->r + (l->a * t[i])).x;
172  impactpoint->y = (l->r + (l->a * t[i])).y;
173  impactpoint->z = (l->r + (l->a * t[i])).z;
174 
175  return true;
176}
177
178bool cctree_iterate(const Placement* pa, CC_Tree* ta, unsigned long* ahitflags, const Placement* pb, CC_Tree* tb, unsigned long* bhitflags)
179{
180  bool r = false;
181  int ia, ib;
182  Vector mra = pa->r + pa->w.apply(ta->m);
183  Vector mrb = pb->r + pb->w.apply(tb->m);
184  CC_Tree* use_a, *use_b;
185 
186  if( use_a == NULL || use_b == NULL) return false;
187 
188  if( sphere_sphere_collision( mra, ta->r, mrb, tb->r))
189  {
190    if( ta->n == 0 && tb->n == 0)
191    {
192      setflag( ahitflags, ta->data.ID);
193      setflag( bhitflags, tb->data.ID);
194      return true;
195    }
196    for( ia = 0; ia < ta->n || ta->n == 0; ia++)
197    {
198      if( ta->n == 0) use_a = ta;
199      else use_a = ta->data.b[ia];
200      for( ib = 0; ib < tb->n || ta->n == 0; ib++)
201      {
202        if( ta->n == 0) use_b = ta;
203        else use_b = ta->data.b[ib];
204       
205        r = r || cctree_iterate( pa, use_a, ahitflags, pb, use_b, bhitflags);
206       
207        if( tb->n == 0) break;
208      }
209      if( ta->n == 0) break;
210    }
211    return r;
212  }
213 
214  return false;
215}
216
217/**
218   \brief sets the <ID>th flag in flags
219   \param flags: pointer to the long used for flagging
220   \param ID: number of the flag to be set
221*/
222void setflag( unsigned long* flags, unsigned long ID)
223{
224  unsigned long f;
225  f = 1;
226  for( int i = 0; i < ID; i++)
227  {
228    f *= 2;
229  }
230  *flags = *flags | f;
231  return;
232}
233
234/**
235   \brief frees the memory allocated in a CC_Tree
236*/
237void free_CC_Tree( CC_Tree* tree)
238{
239  if (tree == NULL) return;
240  for (int i = 0; i < tree->n; i++)
241  {
242    free_CC_Tree( tree->data.b[i]);
243  }
244  free( tree);
245}
246
247/**
248   \brief loads a CC_Tree from a stream
249*/
250CC_Tree* load_CC_Tree (FILE* stream)
251{
252  CC_Tree* tree = NULL;
253  CC_Tree** branches = NULL;
254  float buf[4];
255  unsigned long n;
256  unsigned long ID;
257 
258  // first load vector and radius
259  if (fread( buf, sizeof (float), 4, stream) != 4) return NULL;
260  // then the amount of subbranches
261  if (fread( &n, sizeof(unsigned long), 1, stream) != 1) return NULL;
262  // then ID or the subbranches
263  if ( n == 0)
264  {
265    if (fread( &ID, sizeof(unsigned long), 1, stream) != 1) return NULL;
266  }
267  else
268  {
269    branches = (CC_Tree**)malloc( sizeof(CC_Tree*) * n);
270    for( int i = 0; i < n; i++)
271    {
272      if ((branches[i] = load_CC_Tree (stream)) == NULL)
273      {
274        for( int j = 0; j < i; j++)
275        {
276          free_CC_Tree (branches[j]);
277          free (branches);
278          return NULL;
279        }
280      }
281    }
282  }
283 
284  // assemble
285  tree = (CC_Tree*) malloc (sizeof(CC_Tree));
286  tree->m.x = buf[0];
287  tree->m.y = buf[1];
288  tree->m.z = buf[2];
289  tree->r = buf[3];
290  tree->n = n;
291  if( n == 0) tree->data.ID = ID;
292  else tree->data.b = branches;
293 
294  // then return
295  return tree;
296}
297
298/**
299   \brief saves a CC_Tree to a stream
300*/
301int save_CC_Tree (CC_Tree* tree, FILE* stream)
302{
303  float buf[4];
304 
305  buf[0] = tree->m.x;
306  buf[1] = tree->m.y;
307  buf[2] = tree->m.z;
308  buf[3] = tree->r;
309 
310  // first save vector and radius
311  if (fwrite( buf, sizeof (float), 4, stream) != 4) return -1;
312  // then the amount of subbranches
313  if (fwrite( &(tree->n), sizeof(unsigned long), 1, stream) != 1) return -1;
314  // then ID or the subbranches
315  if ( tree->n == 0)
316  {
317    if (fwrite( &(tree->data.ID), sizeof(unsigned long), 1, stream) != 1) return -1;
318  }
319  else
320  {
321    for( int i = 0; i < tree->n; i++)
322    {
323      if ( save_CC_Tree (tree->data.b[i], stream) == -1) return -1;
324    }
325  }
326   
327  // then return
328  return 0;
329}
330
331bool cctree_trace( const Placement* p, CC_Tree* t, unsigned long* hitflags, const Line* trace, Vector* impactpoint)
332{
333  bool r = false;
334  int i;
335  Vector mr = p->r + p->w.apply (t->m);
336  CC_Tree* use_t;
337  Vector* ips;
338  unsigned long* hfs;
339 
340  if( trace_sphere_collision (mr, t->r, trace, impactpoint))
341  {
342        if( t->n == 0)
343        {
344                setflag (hitflags, t->data.ID);
345                return true;
346        }
347        else
348        {
349                ips = new Vector[t->n];
350                hfs = new unsigned long[t->n];
351                for (i = 0; i < t->n; i++) hfs[i] = 0;
352                for (i = 0; i < t->n; i++)
353                {
354                        r = r || cctree_trace (p, t->data.b[i], &(hfs[i]), trace, &(ips[i]));
355                }
356                if( r)
357                {
358                        float kl = 0.0;
359                        float l = 0.0;
360                        int k = 0;
361                        for (i = 0; i < t->n; i++)
362                        {
363                                if( (kl = (trace->r - ips[i]).len()) > l)
364                                {
365                                        l = kl;
366                                        k = i;
367                                }
368                        }
369                        impactpoint->x = ips[k].x;
370                        impactpoint->y = ips[k].y;
371                        impactpoint->z = ips[k].z;
372                        *hitflags = hfs[k];
373                }
374                delete ips;
375                delete hfs;
376        }
377        return r;
378  }
379 
380  return false;
381}
Note: See TracBrowser for help on using the repository browser.