source: orxonox.OLD/orxonox/branches/chris/core/collision.cc @ 1975

/* 
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Christian Meyer
   co-programmer: ...
*/


#include "collision.h"


using namespace std;

CollisionCluster::CollisionCluster (float rad = 1.0, Vector mid = Vector(0,0,0))
{
  root = (CC_Tree*) malloc( sizeof( CC_Tree));
  root->n = 0;
  root->data.ID = 0;
  root->r = rad;
  root->m = mid;
}

CollisionCluster::CollisionCluster (char* filename)
{
  FILE* stream;
  char hbuffer[3];
  stream = fopen (filename, "rb");
  if( stream == NULL)
  {
    root = NULL;
    return;
  }
  if( fread (hbuffer, sizeof( char), 2, stream) != 2)
  {
    root = NULL;
    return;
  }
  
  hbuffer[2] = 0;
  if( strcmp (hbuffer, "CC"))
  {
    root = NULL;
    return;
  }
  
  root = load_CC_Tree (stream);
  fclose (stream);
}

CollisionCluster::~CollisionCluster ()
{
  free_CC_Tree( root);
}

int CollisionCluster::store (char* filename)
{
  int r;
  FILE* stream;
  stream = fopen( filename, "wb");
  if( stream == NULL) return -1;
  r = save_CC_Tree (root, stream);
  fclose (stream);
  return r;
}

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);
}

bool check_collision (const Placement* pa, const CollisionCluster* a, unsigned long* ahitflags, const Placement* pb, const CollisionCluster* b, unsigned long* bhitflags)
{
  CC_Tree* ta, *tb;
  if( (ta = a->root) == NULL) return false;
  if( (tb = b->root) == NULL) return false;
  
  return cctree_iterate(pa, ta, ahitflags, pb, tb, bhitflags);
}

bool sphere_sphere_collision( Vector m1, float r1, Vector m2, float r2)
{
  if ((m1-m2).len() < r1+r2) return true;
  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;
}

bool cctree_iterate(const Placement* pa, CC_Tree* ta, unsigned long* ahitflags, const Placement* pb, CC_Tree* tb, unsigned long* bhitflags)
{
  bool r = false;
  int ia, ib;
  Vector mra = pa->r + rotate_vector( ta->m, pa->w);
  Vector mrb = pb->r + rotate_vector( tb->m, pb->w);
  CC_Tree* use_a, *use_b;
  
  if( use_a == NULL || use_b == NULL) return false;
  
  if( sphere_sphere_collision( mra, ta->r, mrb, tb->r))
  {
    if( ta->n == 0 && tb->n == 0)
    {
      setflag( ahitflags, ta->data.ID);
      setflag( bhitflags, tb->data.ID);
      return true;
    }
    for( ia = 0; ia < ta->n || ta->n == 0; ia++)
    {
      if( ta->n == 0) use_a = ta;
      else use_a = ta->data.b[ia];
      for( ib = 0; ib < tb->n || ta->n == 0; ib++)
      {
        if( ta->n == 0) use_b = ta;
        else use_b = ta->data.b[ib];
        
        r = r || cctree_iterate( pa, use_a, ahitflags, pb, use_b, bhitflags);
        
        if( tb->n == 0) break;
      }
      if( ta->n == 0) break;
    }
    return r;
  }
  
  return false;
}

void setflag( unsigned long* flags, unsigned long ID)
{
  unsigned long f;
  f = 1;
  for( int i = 0; i < ID; i++)
  {
    f *= 2;
  }
  *flags = *flags | f;
  return;
}

void free_CC_Tree( CC_Tree* tree)
{
  if (tree == NULL) return;
  for (int i = 0; i < tree->n; i++)
  {
    free_CC_Tree( tree->data.b[i]);
  }
  free( tree);
}

CC_Tree* load_CC_Tree (FILE* stream)
{
  CC_Tree* tree = NULL;
  CC_Tree** branches = NULL;
  float buf[4];
  unsigned long n;
  unsigned long ID;
  
  // first load vector and radius
  if (fread( buf, sizeof (float), 4, stream) != 4) return NULL;
  // then the amount of subbranches
  if (fread( &n, sizeof(unsigned long), 1, stream) != 1) return NULL;
  // then ID or the subbranches
  if ( n == 0)
  {
    if (fread( &ID, sizeof(unsigned long), 1, stream) != 1) return NULL;
  }
  else
  {
    branches = (CC_Tree**)malloc( sizeof(CC_Tree*) * n);
    for( int i = 0; i < n; i++)
    {
      if ((branches[i] = load_CC_Tree (stream)) == NULL)
      {
        for( int j = 0; j < i; j++)
        {
          free_CC_Tree (branches[j]);
          free (branches);
          return NULL;
        }
      }
    }
  }
  
  // assemble
  tree = (CC_Tree*) malloc (sizeof(CC_Tree));
  tree->m.x = buf[0];
  tree->m.y = buf[1];
  tree->m.z = buf[2];
  tree->r = buf[3];
  tree->n = n;
  if( n == 0) tree->data.ID = ID;
  else tree->data.b = branches;
  
  // then return
  return tree;
}

int save_CC_Tree (CC_Tree* tree, FILE* stream)
{
  float buf[4];
  
  buf[0] = tree->m.x;
  buf[1] = tree->m.y;
  buf[2] = tree->m.z;
  buf[3] = tree->r;
  
  // first save vector and radius
  if (fwrite( buf, sizeof (float), 4, stream) != 4) return -1;
  // then the amount of subbranches
  if (fwrite( &(tree->n), sizeof(unsigned long), 1, stream) != 1) return -1;
  // then ID or the subbranches
  if ( tree->n == 0)
  {
    if (fwrite( &(tree->data.ID), sizeof(unsigned long), 1, stream) != 1) return -1;
  }
  else
  {
    for( int i = 0; i < tree->n; i++)
    {
      if ( save_CC_Tree (tree->data.b[i], stream) == -1) return -1;
    }
  }
    
  // then return
  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;
}