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

/* 
   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;

/**
   \brief simple CollisionCluster constructor
   \param rad: the radius of the base sphere
   \param mid: the middle point of the sphere
*/
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;
}

/**
   \brief loads a CollisionCluster from a data file
   \param filename: self-explanatory
*/
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);
}

/**
   \brief Simple destructor that cleans up all the mess
*/
CollisionCluster::~CollisionCluster ()
{
  free_CC_Tree( root);
}

/**
   \brief stores the CollisionCluster into a file
   \param filename: self-explanatory
   \return zero on success, -1 on error
*/
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;
}

/**
   \brief performs a collision check between a CollisionCluster and a Line
   \param pa: pointer to the Placement of the CollisionCluster
   \param a: pointer to the CollisionCluster
   \param ahitflags: pointer to an unsigned long to store hitlocation data
   \param trace: pointer to the Line
   \param impactpoint: pointer to a Vector where the point of intersection is stored in
   \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
*/
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);
}

/**
   \brief performs a collision check between two CollisionClusters
   \param pa: pointer to the Placement of the first CollisionCluster
   \param a: pointer to the first CollisionCluster 
   \param ahitflags: pointer to an unsigned long to store hitlocation data on the first CollisionCluster
   \param pb: pointer to the Placement of the second CollisionCluster
   \param b: pointer to the second CollisionCluster
   \param bhitflags: pointer to an unsigned long to store hitlocation data on the second CollisionCluster
   \return true on collision, false otherwise
   If true is returned, all flags in ahitflags and bhitflags that symbolize intersecting subspheres in the respective CollisionCluster are set
*/
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);
}

/**
   \brief performs an intersection check between two spheres
   \param m1: center point of first sphere
   \param r1: radius of first sphere
   \param m2: center point of second sphere
   \param r2: radius of second sphere
   \return true on intersection, false otherwise
*/
bool sphere_sphere_collision( Vector m1, float r1, Vector m2, float r2)
{
  if ((m1-m2).len() < r1+r2) return true;
  return false;
}

/**
   \brief performs an intersection check between a Line and a sphere
   \param m: center point of the sphere
   \param r: radius of the sphere
   \param l: pointer to the Line
   \param impactpoint: pointer to a buffer to store the point of intersection
   \return true on intersection, false otherwise. If true is returned, impactpoint is set to the loaction where the intersection occured
*/
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 + pa->w.apply(ta->m);
  Vector mrb = pb->r + pb->w.apply(tb->m);
  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;
}

/**
   \brief sets the <ID>th flag in flags
   \param flags: pointer to the long used for flagging
   \param ID: number of the flag to be set
*/
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;
}

/**
   \brief frees the memory allocated in a CC_Tree
*/
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);
}

/**
   \brief loads a CC_Tree from a stream
*/
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;
}

/**
   \brief saves a CC_Tree to a stream
*/
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 + p->w.apply (t->m);
  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;
}