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

/* 
   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)
{
  return false;
}

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, Line& l, Vector* impactpoint)
{
  return false;
}

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