/*
    Copyright (C) 2000-2003 by Jorrit Tyberghein
    Written by Daniel Gudbjartsson

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

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "cssysdef.h"

#include "csqsqrt.h"
#include "csgeom/math3d.h"
#include "csgeom/trimesh.h"
#include "csgeom/quaternion.h"
#include "csgeom/transfrm.h"

// For backward-compatibility, collider.cpp must invoke its own deprecated
// constructors. Suppress the deprecation warning for this special case.
#define CS_DEPRECATION_SUPPRESS_HACK
#include "cstool/collider.h"
#undef CS_DEPRECATION_SUPPRESS_HACK
#include "iengine/collection.h"

#include "iengine/camera.h"
#include "iengine/engine.h"
#include "iengine/mesh.h"
#include "iengine/movable.h"
#include "iengine/portal.h"
#include "iengine/portalcontainer.h"
#include "iengine/region.h"
#include "iengine/sector.h"
#include "iengine/viscull.h"
#include "iengine/scenenode.h"

#include "imesh/objmodel.h"
#include "igeom/trimesh.h"
#include "imesh/object.h"
#include "ivaria/collider.h"

#define COLLIDE_ACTOR_DEBUG
//----------------------------------------------------------------------

CS_LEAKGUARD_IMPLEMENT (csColliderWrapper);



csColliderWrapper::csColliderWrapper (csObject& parent,
	iCollideSystem* collide_system,
	iTriangleMesh* mesh)
  : scfImplementationType (this)
{
  parent.ObjAdd (this);
  csColliderWrapper::collide_system = collide_system;
  collider = collide_system->CreateCollider (mesh);
}

csColliderWrapper::csColliderWrapper (iObject* parent,
        iCollideSystem* collide_system,
        iTriangleMesh* mesh)
  : scfImplementationType (this)
{
  parent->ObjAdd (this);
  csColliderWrapper::collide_system = collide_system;
  collider = collide_system->CreateCollider (mesh);
}
csColliderWrapper::csColliderWrapper (iObject* parent,
	iCollideSystem* collide_system,
	iTerraFormer* terrain)
  : scfImplementationType (this)
{
  parent->ObjAdd (this);
  csColliderWrapper::collide_system = collide_system;
  collider = collide_system->CreateCollider (terrain);
}

csColliderWrapper::csColliderWrapper (iObject* parent,
    iCollideSystem* collide_system,
  	iTerrainSystem* terrain)
  : scfImplementationType (this)
{
  parent->ObjAdd (this);
  csColliderWrapper::collide_system = collide_system;
  collider = collide_system->CreateCollider (terrain);
}

csColliderWrapper::csColliderWrapper (iObject* parent,
	iCollideSystem* collide_system,
	iCollider* collider)
  : scfImplementationType (this)
{
  parent->ObjAdd (this);
  csColliderWrapper::collide_system = collide_system;
  csColliderWrapper::collider = collider;
}

csColliderWrapper::~csColliderWrapper ()
{
}

bool csColliderWrapper::Collide (csObject& otherObject,
                          csReversibleTransform* pThisTransform,
                          csReversibleTransform* pOtherTransform)
{
  csColliderWrapper *pOtherCollider = GetColliderWrapper (otherObject);
  if (pOtherCollider)
    return Collide (*pOtherCollider, pThisTransform, pOtherTransform);
  else
    return false;
}

bool csColliderWrapper::Collide (iObject* otherObject,
                          csReversibleTransform* pThisTransform,
                          csReversibleTransform* pOtherTransform)
{
  csColliderWrapper *pOtherCollider = GetColliderWrapper (otherObject);
  if (pOtherCollider)
    return Collide (*pOtherCollider, pThisTransform, pOtherTransform);
  else
    return false;
}

bool csColliderWrapper::Collide (csColliderWrapper& otherCollider,
                          csReversibleTransform* pTransform1,
                          csReversibleTransform* pTransform2)
{
  if (!collider) return false;
  csColliderWrapper *pCollider2 = &otherCollider;
  if (pCollider2 == this) return false;

  return collide_system->Collide (collider, pTransform1,
  	pCollider2->collider, pTransform2);
}

csColliderWrapper* csColliderWrapper::GetColliderWrapper (csObject &object)
{
  csRef<csColliderWrapper> w (CS::GetChildObject<csColliderWrapper> (&object));
  return w;	// This will DecRef() but that's ok in this case.
}

csColliderWrapper* csColliderWrapper::GetColliderWrapper (iObject* object)
{
  csRef<csColliderWrapper> w (CS::GetChildObject<csColliderWrapper> (object));
  return w;	// This will DecRef() but that's ok in this case.
}

void csColliderWrapper::UpdateCollider (iTriangleMesh* mesh)
{
  collider = collide_system->CreateCollider (mesh);
}

void csColliderWrapper::UpdateCollider (iTerraFormer* terrain)
{
  collider = collide_system->CreateCollider (terrain);
}
//----------------------------------------------------------------------

csColliderWrapper* csColliderHelper::InitializeCollisionWrapper (
	iCollideSystem* colsys, iMeshWrapper* mesh)
{
  iMeshFactoryWrapper* factory = mesh->GetFactory ();

  iObjectModel* obj_objmodel = mesh->GetMeshObject ()->GetObjectModel ();
  csStringID basemesh_id = colsys->GetBaseDataID ();
  csStringID trianglemesh_id = colsys->GetTriangleDataID ();

  // do_trimesh will be set to true if we're using the new trianglemesh
  // system (instead of polygonmesh). We detect that we want to use the
  // new system when the base mesh of the object model is given (even
  // if the collision detection mesh might not be set).
  bool do_trimesh = obj_objmodel->IsTriangleDataSet (basemesh_id);
  // Also check factory.
  if (!do_trimesh && factory)
  {
    iObjectModel* fact_objmodel = factory->GetMeshObjectFactory ()
      ->GetObjectModel ();
    if (fact_objmodel)
      do_trimesh = fact_objmodel->IsTriangleDataSet (basemesh_id);
  }

  iTriangleMesh* obj_trimesh;
  bool obj_trimesh_set = false;
  if (do_trimesh)
  {
    // If the collision detection triangle mesh is not set then we
    // use the base mesh instead.
    obj_trimesh_set = obj_objmodel->IsTriangleDataSet (trianglemesh_id);
    if (obj_trimesh_set)
      obj_trimesh = obj_objmodel->GetTriangleData (trianglemesh_id);
    else
      obj_trimesh = obj_objmodel->GetTriangleData (basemesh_id);
  }
  else
  {
    obj_trimesh = obj_objmodel->GetTriangleData (colsys->GetTriangleDataID ());
  }

  iTerraFormer* obj_terraformer = obj_objmodel->GetTerraFormerColldet ();
  iTerrainSystem* obj_terrain = obj_objmodel->GetTerrainColldet ();

  csRef<csColliderWrapper> cw;
  if (factory)
  {
    iObjectModel* fact_objmodel = factory->GetMeshObjectFactory ()
    	->GetObjectModel ();
    if (fact_objmodel)
    {
      if (fact_objmodel->GetTerraFormerColldet ())
      {
        iTerraFormer* fact_terraformer = fact_objmodel
	  ->GetTerraFormerColldet ();

        if (fact_terraformer && (fact_terraformer == obj_terraformer
	      || !obj_terraformer))
        {
          // First check if the parent factory has a collider wrapper.
          iCollider* collider;
          csColliderWrapper* cw_fact = csColliderWrapper::GetColliderWrapper (
            factory->QueryObject ());
          if (cw_fact)
          {
            collider = cw_fact->GetCollider ();
          }
          else
          {
            csColliderWrapper *cw_fact = new csColliderWrapper (
              factory->QueryObject (), colsys, fact_terraformer);
            cw_fact->SetName (factory->QueryObject ()->GetName());
            collider = cw_fact->GetCollider ();
            cw_fact->DecRef ();
          }

          // Now add the collider wrapper to the mesh. We need a new
          // csColliderWrapper because the csObject system is strictly
          // a tree and one csColliderWrapper cannot have multiple parents.
          cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
            colsys, collider));
          cw->SetName (mesh->QueryObject ()->GetName());
          cw = 0;
          // Clear object triangle mesh so we will no longer try to create
          // a collider from that (we already have a collider).
          obj_terraformer = 0;
        }
      }
      else 
      {
	if (do_trimesh)
	{
	  if (!obj_trimesh_set)
	  {
	    bool fact_trimesh_set = fact_objmodel->IsTriangleDataSet (
	        trianglemesh_id);
            iTriangleMesh* fact_trimesh;
	    // If the collision detection triangle mesh is not set then we
	    // use the base mesh instead.
	    if (fact_trimesh_set)
	      fact_trimesh  = fact_objmodel->GetTriangleData (trianglemesh_id);
	    else
	      fact_trimesh  = fact_objmodel->GetTriangleData (basemesh_id);

	    if (fact_trimesh)
	    {
              // First check if the parent factory has a collider wrapper.
              iCollider* collider;
              csColliderWrapper* cw_fact =
		csColliderWrapper::GetColliderWrapper (factory->QueryObject ());
              if (cw_fact)
              {
                collider = cw_fact->GetCollider ();
              }
              else
              {
                csColliderWrapper *cw_fact = new csColliderWrapper (
                  factory->QueryObject (), colsys, fact_trimesh);
                cw_fact->SetName (factory->QueryObject ()->GetName());
                collider = cw_fact->GetCollider ();
                cw_fact->DecRef ();
              }

              // Now add the collider wrapper to the mesh. We need a new
              // csColliderWrapper because the csObject system is strictly
              // a tree and one csColliderWrapper cannot have multiple parents.
              cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
                colsys, collider));
              cw->SetName (mesh->QueryObject ()->GetName());
	    }
            // Clear object polygon mesh so we will no longer try to create
            // a collider from that (we already have a collider).
            obj_trimesh = 0;
	  }
	}
	else
	{
          iTriangleMesh* fact_trimesh = fact_objmodel->GetTriangleData (colsys->GetTriangleDataID ());
          if (fact_trimesh && (fact_trimesh == obj_trimesh || !obj_trimesh))
          {
            // First check if the parent factory has a collider wrapper.
            iCollider* collider;
            csColliderWrapper* cw_fact = csColliderWrapper::GetColliderWrapper (
              factory->QueryObject ());
            if (cw_fact)
            {
              collider = cw_fact->GetCollider ();
            }
            else
            {
              csColliderWrapper *cw_fact = new csColliderWrapper (
                factory->QueryObject (), colsys, fact_trimesh);
              cw_fact->SetName (factory->QueryObject ()->GetName());
              collider = cw_fact->GetCollider ();
              cw_fact->DecRef ();
            }

            // Now add the collider wrapper to the mesh. We need a new
            // csColliderWrapper because the csObject system is strictly
            // a tree and one csColliderWrapper cannot have multiple parents.
            cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
              colsys, collider));
            cw->SetName (mesh->QueryObject ()->GetName());
            // Clear object triangle mesh so we will no longer try to create
            // a collider from that (we already have a collider).
            obj_trimesh = 0;
          }
	}
      }
    }
  }

  if (obj_terraformer)
    cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
      colsys, obj_terraformer));
  else if (obj_terrain)
    cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
      colsys, obj_terrain));
  else if (do_trimesh && obj_trimesh)
    cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
      colsys, obj_trimesh));
  else if (obj_trimesh)
    cw.AttachNew (new csColliderWrapper (mesh->QueryObject (),
      colsys, obj_trimesh));
  if (cw)
    cw->SetName (mesh->QueryObject ()->GetName());

  const csRef<iSceneNodeArray> ml = 
    mesh->QuerySceneNode ()->GetChildrenArray ();
  size_t i;
  for (i = 0 ; i < ml->GetSize () ; i++)
  {
    iMeshWrapper* child = ml->Get (i)->QueryMesh ();
    // @@@ What if we have a light containing another mesh?
    if (child)
      InitializeCollisionWrapper (colsys, child);
  }

  return cw;
}

void csColliderHelper::InitializeCollisionWrappers (iCollideSystem* colsys,
  	iEngine* engine, iCollection* region)
{
  // Initialize all mesh objects for collision detection.
  int i;
  iMeshList* meshes = engine->GetMeshes ();
  for (i = 0 ; i < meshes->GetCount () ; i++)
  {
    iMeshWrapper* sp = meshes->Get (i);
    if (region && !region->IsParentOf (sp->QueryObject ())) continue;
    InitializeCollisionWrapper (colsys, sp);
  }
}

bool csColliderHelper::CollideArray (
	iCollideSystem* colsys,
	iCollider* collider,
  	const csReversibleTransform* trans,
  	int num_colliders,
	iCollider** colliders,
	csReversibleTransform **transforms)
{
  int i;
  for (i = 0 ; i < num_colliders ; i++)
  {
    bool rc = colsys->Collide (collider, trans,
    	colliders[i], transforms[i]);
    if (rc) return rc;
  }
  return false;
}

int csColliderHelper::CollidePath (
	iCollideSystem* colsys,
	iCollider* collider,
  	const csReversibleTransform* trans,
	float nbrsteps,
	csVector3& newpos,
	int num_colliders,
	iCollider** colliders,
	csReversibleTransform** transforms)
{
  csReversibleTransform test = *trans;
  csVector3 start = test.GetOrigin ();
  csVector3 end = newpos;
  csVector3 testpos;
  float step = 1.0f / nbrsteps;
  float curdist = 0;
  bool rc = false;
  bool firsthit = true;
  for (;;)
  {
    testpos = start+curdist * (end-start);
    test.SetOrigin (testpos);
    colsys->ResetCollisionPairs ();
    rc = CollideArray (colsys, collider, &test,
    	num_colliders, colliders, transforms);
    if (rc) break;
    firsthit = false;

    if (curdist >= 1) break;
    curdist += step;
    if (curdist > 1) curdist = 1;
  }

  if (rc)
  {
    // We had a collision.
    if (firsthit)
    {
      // The collision happened on the start point. In that case
      // we cannot move at all. Return -1.
      return -1;
    }

    // Here we try to find more exactly where the collision occured by
    // doing a binary search.
    end = testpos;
    while (csSquaredDist::PointPoint (start, end) > .05)
    {
      testpos = (start+end) / 2;
      test.SetOrigin (testpos);
      colsys->ResetCollisionPairs ();
      rc = CollideArray (colsys, collider, &test,
      	num_colliders, colliders, transforms);
      if (rc)
      {
        // Use left segment.
        end = testpos;
      }
      else
      {
        // Use right segment.
	start = testpos;
      }
    }
    // We know that the object can move to the 'start' position safely
    // because of the way we handle the binary search and the starting
    // condition that firsthit == false.
    newpos = start;

    // But first we set the collision detection array to the position
    // which resulted in collision.
    test.SetOrigin (end);
    colsys->ResetCollisionPairs ();
    CollideArray (colsys, collider, &test,
    	num_colliders, colliders, transforms);

    return 0;
  }
  else
  {
    // There was no collision.
    return 1;
  }
}

csTraceBeamResult csColliderHelper::TraceBeam (iCollideSystem* cdsys,
	iSector* sector, const csVector3& start, const csVector3& end,
	bool traverse_portals)
{
  csTraceBeamResult rc;
  rc.sqdistance = TraceBeam (cdsys, sector, start, end, traverse_portals,
  	rc.closest_tri, rc.closest_isect, &rc.closest_mesh, &rc.end_sector);
  return rc;
}

float csColliderHelper::TraceBeam (iCollideSystem* cdsys, iSector* sector,
	const csVector3& start, const csVector3& end,
	bool traverse_portals,
	csIntersectingTriangle& closest_tri,
	csVector3& closest_isect,
	iMeshWrapper** closest_mesh,
	iSector** end_sector)
{
  if (!sector)
  {
    if (closest_mesh) *closest_mesh = 0;
	if (end_sector) *end_sector = 0;
    return -1.0f;
  }
  if (end_sector) *end_sector = sector;

  iVisibilityCuller* culler = sector->GetVisibilityCuller ();
  csRef<iVisibilityObjectIterator> it = culler->IntersectSegmentSloppy (
  	start, end);

  // We loop over all objects that intersect with the beam. For every
  // such object we will use CollideRay() to find colliding triangles. We
  // will look for the colliding triangle closest to 'start'.
  float best_squared_dist = 10000000000.0;
  csSegment3 seg (start, end);
  iMeshWrapper* best_mesh = 0;
  bool have_hit = false;
  // This will be set to the hit portal index if the best polygon we
  // hit so far is actually a portal.
  int last_portal_index = -1;
 
  while (it->HasNext ())
  {
    iVisibilityObject* visobj = it->Next ();
    iMeshWrapper* mesh = visobj->GetMeshWrapper ();
    csColliderWrapper* colwrap = csColliderWrapper::GetColliderWrapper (
    	mesh->QueryObject ());
    if (colwrap)
    {
      iMovable* movable = mesh->GetMovable ();
      csReversibleTransform trans = movable->GetFullTransform ();
      if (cdsys->CollideRay (colwrap->GetCollider (), &trans, start, end))
      {
        // This ray hits the mesh.
	const csArray<csIntersectingTriangle>& tris = cdsys->
		GetIntersectingTriangles ();
	size_t i;
	for (i = 0 ; i < tris.GetSize () ; i++)
	{
	  csVector3 isect;
	  csIntersectingTriangle tri;
	  if (movable->IsFullTransformIdentity ())
	  {
	    tri = tris[i];
	  }
	  else
	  {
	    tri.a = trans.This2Other (tris[i].a);
	    tri.b = trans.This2Other (tris[i].b);
	    tri.c = trans.This2Other (tris[i].c);
	  }

	  // The function below should always return true but you never know
	  // due to numerical inaccuracies.
	  if (csIntersect3::SegmentTriangle (seg, tri.a, tri.b, tri.c,
	  	isect))
	  {
	    float squared_dist = csSquaredDist::PointPoint (isect, start);
	    if (squared_dist < best_squared_dist)
	    {
	      have_hit = true;
	      best_squared_dist = squared_dist;
	      closest_tri = tri;
	      closest_isect = isect;
	      // This is not a portal we want to traverse because it
	      // has a collider which means it is solid.
	      last_portal_index = -1;
	      best_mesh = mesh;
	    }
	  }
	}
      }
    }
    if (mesh->GetPortalContainer () && traverse_portals)
    {
      // We have a portal container and we want to specifically traverse
      // portals. In that case we also have to trace a beam to whatever
      // portal we might hit.
      iMovable* movable = mesh->GetMovable ();
      csReversibleTransform trans = movable->GetFullTransform ();
      csVector3 obj_start, obj_end;
      if (movable->IsFullTransformIdentity ())
      {
        obj_start = start;
        obj_end = end;
      }
      else
      {
        obj_start = trans.Other2This (start);
        obj_end = trans.Other2This (end);
      }
      csVector3 obj_isect;
      int polygon_idx;
      if (mesh->GetMeshObject ()->HitBeamObject (obj_start, obj_end,
      		obj_isect, 0, &polygon_idx))
      {
        if (!movable->IsFullTransformIdentity ())
	  obj_isect = trans.This2Other (obj_isect);
	float squared_dist = csSquaredDist::PointPoint (obj_isect, start);
	if (squared_dist < best_squared_dist)
	{
	  have_hit = true;
	  best_squared_dist = squared_dist;
	  closest_isect = obj_isect;
	  last_portal_index = polygon_idx;
	  best_mesh = mesh;
	}
      }
    }
  }

  // If our best hit is a portal then we must traverse it here.
  if (last_portal_index != -1)
  {
    iPortal* portal = best_mesh->GetPortalContainer ()->GetPortal (
    	last_portal_index);
    // First we calculate a new beam starting in the new sector from
    // the intersection point. We make sure the new start is slightly
    // beyond the intersection point to avoid numerical inaccuracies.
    // We take a point 0.1% on the way from 'closest_isect' to 'end'.
    csVector3 new_start = closest_isect + 0.001 * (end-closest_isect);
    csVector3 new_end = end;

    // Now we have to consider space warping for the portal.
    if (portal->GetFlags ().Check (CS_PORTAL_WARP))
    {
      iMovable* movable = best_mesh->GetMovable ();
      csReversibleTransform trans = movable->GetFullTransform ();

      csReversibleTransform warp_wor;
      portal->ObjectToWorld (trans, warp_wor);
      new_start = portal->Warp (warp_wor, new_start);
      new_end = portal->Warp (warp_wor, new_end);
    }

    if (end_sector) *end_sector = portal->GetSector();

    // Recurse with the new beam to the new sector.
    float new_squared_dist = TraceBeam (cdsys, portal->GetSector (),
	new_start, new_end, traverse_portals,
	closest_tri, closest_isect,
	closest_mesh, end_sector);
    if (new_squared_dist >= 0)
    {
      // We have a hit. We have to add the distance so far to the
      // new distance.
      float new_dist = csQsqrt (best_squared_dist) + csQsqrt (new_squared_dist);
      return new_dist * new_dist;
    }
    return -1.0f;
  }

  if (closest_mesh) *closest_mesh = best_mesh;
  if (have_hit)
    return best_squared_dist;
  else
    return -1.0f;
}
//----------------------------------------------------------------------

csColliderActor::csColliderActor ()
{
  gravity = 9.806f;
  onground = false;
  cd = true;
  velWorld.Set (0, 0, 0);
  engine = 0;
  cdsys = 0;
  mesh = 0;
  camera = 0;
  movable = 0;
  do_hit_meshes = false;

  // Only used in case a camera is used.
  rotation.Set (0, 0, 0);
}

void csColliderActor::InitializeColliders (
	const csVector3& legs, const csVector3& body, const csVector3& shift)
{
  csColliderActor::shift = shift;
  bottomSize = legs;
  topSize = body;
  intervalSize.x = MIN(topSize.x, bottomSize.x);
  intervalSize.y = MIN(topSize.y, bottomSize.y);
  intervalSize.z = MIN(topSize.z, bottomSize.z);

  float maxX = MAX(body.x, legs.x)+shift.x;
  float maxZ = MAX(body.z, legs.z)+shift.z;

  csRef<iTriangleMesh> pm;

  float bX2 = body.x / 2.0f;
  float bZ2 = body.z / 2.0f;
  float bYbottom = legs.y;
  float bYtop = legs.y + body.y;

  csBox3 top (csVector3 (-bX2, bYbottom, -bZ2) + shift,
		csVector3 (bX2, bYtop, bZ2) + shift);
  pm = csPtr<iTriangleMesh> (new csTriangleMeshBox (top));
  topCollider = cdsys->CreateCollider (pm);

  float lX2 = legs.x / 2.0f;
  float lZ2 = legs.z / 2.0f;

  csBox3 bot (csVector3 (-lX2, 0, -lZ2) + shift,
		csVector3 (lX2, 0 + legs.y, lZ2) + shift);
  pm = csPtr<iTriangleMesh> (new csTriangleMeshBox (bot));
  bottomCollider = cdsys->CreateCollider (pm);

  boundingBox.Set(csVector3(-maxX / 2.0f, 0, -maxZ / 2.0f) + shift,
    csVector3(maxX / 2.0f, bYtop, maxZ / 2.0f) + shift);

// @@@ Don't know why this is needed!
  csColliderActor::shift.x = -shift.x;
  csColliderActor::shift.y = -shift.y;
  csColliderActor::shift.z = -shift.z;
}

void csColliderActor::InitializeColliders (iMeshWrapper* mesh,
	const csVector3& legs, const csVector3& body, const csVector3& shift)
{
  csColliderActor::mesh = mesh;
  camera = 0;
  movable = mesh ? mesh->GetMovable () : 0;
  InitializeColliders (legs, body, shift);
}

void csColliderActor::InitializeColliders (iCamera* camera,
	const csVector3& legs, const csVector3& body, const csVector3& shift)
{
  mesh = 0;
  movable = 0;
  csColliderActor::camera = camera;
  InitializeColliders (legs, body, shift);
}

void csColliderActor::SetCamera (iCamera* camera, bool adjustRotation)
{ 
  this->camera = camera; 
  if (adjustRotation)
  {
    csQuaternion quat; quat.SetMatrix (camera->GetTransform().GetT2O());
    rotation = quat.GetEulerAngles ();
    // Angle fixups.
    /* @@@ FIXME: Are those right in the math sense or do they indicate
     * csQuaternion bugs? */
    if ((ABS(rotation.z - PI) < EPSILON)
      || (ABS(rotation.z + PI) < EPSILON))
    {
      rotation.x = PI-rotation.x;
      rotation.y = PI-rotation.y;
      rotation.z = 0;
    }
    else
      rotation.x = -rotation.x;
  }
}

#if 0
// Small helper function that returns the angle when given an x and y
// coordinate.
static float GetAngle (float x, float y)
{
  if (x > 1.0 )  x = 1.0;
  if (x < -1.0 ) x = -1.0;

  float angle = acos (x);
  if (y < 0)
    angle = 2*PI - angle;

  return angle;
}

static float Matrix2YRot (const csMatrix3& mat)
{
  csVector3 vec (0,0,1);
  vec = mat * vec;

  return GetAngle (vec.z, vec.x);
}
#endif

static inline bool FindIntersection (const csCollisionPair& cd,
    			   csVector3 line[2])
{
  csVector3 tri1[3]; tri1[0]=cd.a1; tri1[1]=cd.b1; tri1[2]=cd.c1;
  csVector3 tri2[3]; tri2[0]=cd.a2; tri2[1]=cd.b2; tri2[2]=cd.c2;
  csSegment3 isect;
  bool coplanar, ret;

  ret = csIntersect3::TriangleTriangle (tri1, tri2, isect, coplanar);
  line[0]=isect.Start ();
  line[1]=isect.End ();
  return ret;
}

int csColliderActor::CollisionDetect (
	iCollider *collider,
	iSector* sector,
	csReversibleTransform* transform,
	csReversibleTransform* old_transform)
{
  int hits = 0;

  // Do we need to check if a collision has really occurred because
  // of multiple sectors nearby.
  bool checkSectors = false;

  csVector3 testpos;
  csVector3 line[2];
  csCollisionPair temppair;
  csBox3 playerBoxStart = boundingBox;
  csBox3 playerBoxEnd = boundingBox;

  playerBoxStart.SetCenter (old_transform->GetOrigin()+boundingBox.GetCenter());
  playerBoxEnd.SetCenter (transform->GetOrigin()+boundingBox.GetCenter());

  csCollisionPair* CD_contact;

  csRef<iMeshWrapperIterator> objectIter = engine->GetNearbyMeshes (sector,
        playerBoxStart + playerBoxEnd,
        true);

  // Check if any portal mesh is close to the player object.
  while (objectIter->HasNext () && !checkSectors)
  {
    iMeshWrapper* meshwrap = objectIter->Next();
    if (meshwrap->GetPortalContainer())
      checkSectors = true;
  }
  objectIter->Reset();

  while (objectIter->HasNext ())
  {
    iMeshWrapper* meshWrapper = objectIter->Next ();

    iMovable* meshMovable = meshWrapper->GetMovable ();
    // Avoid hitting the mesh from this entity itself.
    if (meshWrapper != mesh)
    {
      cdsys->ResetCollisionPairs ();
      csReversibleTransform tr = meshMovable->GetFullTransform ();
      csColliderWrapper* otherwrap = csColliderWrapper::GetColliderWrapper (
      	meshWrapper->QueryObject ());
      if (!otherwrap) continue;
      iCollider* othercollider = otherwrap->GetCollider ();
      if (!(othercollider && cdsys->Collide (collider,
          transform, othercollider, &tr)))
        continue;

      // Check if we really collided
      bool reallycollided = false;

      CD_contact = cdsys->GetCollisionPairs ();
      size_t count = cdsys->GetCollisionPairCount();
      iSectorList * sectors = meshMovable->GetSectors();
      int sector_max = sectors->GetCount ();
      csReversibleTransform temptrans(*old_transform);

      for (size_t j = 0; j < count; j++ )
      {
        /*
        * Here we follow a segment from our current position to the
        * position of the collision. If the sector the collision occured
        * in is not the sector of the mesh we collided with,
        * this is invalid.
        */
        int sector_idx;
        iSector* CollisionSector;
        bool mirror=false;

        // Move the triangles from object space into world space
        temppair.a1 = transform->This2Other (CD_contact[j].a1);
        temppair.b1 = transform->This2Other (CD_contact[j].b1);
        temppair.c1 = transform->This2Other (CD_contact[j].c1);
        if (meshWrapper->GetMovable()->IsFullTransformIdentity())
        {
          temppair.a2 = CD_contact[j].a2;
          temppair.b2 = CD_contact[j].b2;
          temppair.c2 = CD_contact[j].c2;
        }
        else
        {
          temppair.a2 = tr.This2Other (CD_contact[j].a2);
          temppair.b2 = tr.This2Other (CD_contact[j].b2);
          temppair.c2 = tr.This2Other (CD_contact[j].c2);
        }
        if (checkSectors)
        {
          if(!FindIntersection (temppair, line))
            continue;
          // Collided at this line segment. Pick a point in the middle of
          // the segment to test.
          testpos=(line[0]+line[1])/2;

          // This follows a line segment from start to finish and returns
          // the sector you are ultimately in.
          CollisionSector = sector->FollowSegment (temptrans,
            testpos, mirror, true);

          // Iterate through all the sectors of the destination mesh,
          // incase it's in multiple sectors.
          for (sector_idx=0 ; sector_idx<sector_max ; sector_idx++)
          {
            // Check to see if this sector is the sector of the collision.
            if (CollisionSector == sectors->Get (sector_idx))
            {
              reallycollided = true;
              our_cd_contact.Push (temppair);
              // One valid sector is enough
              break;
            }
          }
        }
        else
        {
          reallycollided = true;
          our_cd_contact.Push (temppair);
        }
      }

      // We don't increase hits unless a collision really occurred after
      // all tests.
      if (reallycollided)
      {
        hits++;
	if (do_hit_meshes)
	  hit_meshes.Add (meshWrapper);
        if (cdsys->GetOneHitOnly ()) return 1;
      }
    }
  }

  return hits;
}

int csColliderActor::CollisionDetectIterative (
	iCollider *collider,
	iSector* sector,
	csReversibleTransform* transform,
	csReversibleTransform* old_transform, csVector3& maxmove)
{
  // The maximum position it's possible for the player to move to
  // If we collide at the start point or don't collide at the end point
  // then there is no need for recursion.
  int hits = CollisionDetect (collider, sector, transform, old_transform);
  if (hits == 0)
  {
    maxmove = transform->GetOrigin ();
    return hits;
  }

  cdsys->ResetCollisionPairs ();
  our_cd_contact.Empty ();

  maxmove = old_transform->GetOrigin();
  hits = CollisionDetect (collider, sector, old_transform, old_transform);
  if (hits > 0)
    return hits;

  // The upper and lower bounds
  csVector3 upper = transform->GetOrigin();
  csVector3 lower = old_transform->GetOrigin();

  csMatrix3 id;

  // The last hit that was made, used so that a hit will always be returned
  csVector3 lastHit = upper;

  //cdsys->SetOneHitOnly(true);
  // Repeatedly split the range with which to test the collision against
  while ((upper - lower).SquaredNorm() > 0.01f)
  {
    // Test in the middle between upper and lower bounds
    csOrthoTransform current (id, lower + (upper - lower)/2);
    cdsys->ResetCollisionPairs ();
    our_cd_contact.Empty ();

    hits = CollisionDetect (collider, sector, &current, old_transform);
    
    // Adjust bounds
    if (hits > 0)
    {
      lastHit = lower + (upper - lower)/2;
      upper = lastHit;
    }
    else
    {
      maxmove = lower + (upper - lower) / 2;
      lower = maxmove;
    }
  }
  if (hits == 0)
  {
    cdsys->SetOneHitOnly (false);
    // Make sure we actually return a hit
    csOrthoTransform current (id, lastHit);
    cdsys->ResetCollisionPairs ();
    our_cd_contact.Empty ();
    hits = CollisionDetect (collider, sector, &current, old_transform) > 0;
  }

  return hits;
}

bool csColliderActor::AdjustForCollisions (
	const csVector3& oldpos,
	csVector3& newpos,
	const csVector3& vel,
	float delta)
{
  if (movable && movable->GetSectors()->GetCount() == 0)
    return true;

  bool hitsurface = false;
  int hits;
  size_t i;
  iSector* current_sector;
  if (movable) current_sector = movable->GetSectors ()->Get (0);
  else current_sector = camera->GetSector ();

  csMatrix3 id;

  csOrthoTransform transform_oldpos (id, oldpos);
  csOrthoTransform transform_newpos (id, newpos);
  csVector3 maxmove;

  // Part1: find body collisions => movement
  // Find possible colliding sectors.
  csVector3 localvel = (vel * delta);

  // Travel all relevant sectors and do collision detection.

  our_cd_contact.Empty ();
  cdsys->SetOneHitOnly (false);
  cdsys->ResetCollisionPairs ();

  // Perform recursive collision testing to minimise hits and
  // find distance we can travel
  if (cd)
  {
      if(onground)
          hits = CollisionDetect (topCollider, current_sector,
      &transform_newpos, &transform_oldpos);
      else
          hits = CollisionDetect (bottomCollider, current_sector,
                                           &transform_newpos, &transform_oldpos);
  }
  else
  {
    hits = 0;
    maxmove = transform_newpos.GetOrigin ();
  }

  // localvel is smaller because we can partly move the object in that direction
  maxmove = transform_oldpos.GetOrigin ();
  //localvel -= maxmove - oldpos;
  csVector3 correctedVel(localvel);
    
    bool bounced = false;
    csVector3 bestBounce;
  for (i = 0; i < our_cd_contact.GetSize () ; i++ )
  {
    csCollisionPair& cd = our_cd_contact[i];
    csPlane3 obstacle (cd.a2, cd.b2, cd.c2);
    csVector3 normal = obstacle.Normal ();
      
      // Check, have we collided against an 'inner' face
      if (normal * localvel > 0)
      {
           
          continue;
      }
    float norm = normal.Norm ();
    if (fabs (norm) < SMALL_EPSILON) continue;

      
    csVector3 unit = normal / norm;

    // No sliding forces from ground-like surfaces
    // sin(Pi/4) == 0.7
    if(unit.y >= 0.7) continue;
    

      csVector3 bounce(unit * (unit * localvel));
    
    if((localvel - bounce).y > 0)
    {
         
        unit.y = 0;
        unit = unit / unit.Norm();
        bounce = unit * (unit * localvel);
    }
    // Bounce back
    if(bounced && (localvel - bounce).Norm() < (localvel - bestBounce).Norm())
        bestBounce = bounce;
    
    bounced = true;
      
    correctedVel = localvel - 1.1 * bestBounce; //(-(localvel % unit) % unit);    

      
  }
#ifdef COLLIDE_ACTOR_DEBUG
     
#endif
    localvel = correctedVel;
  //newpos = maxmove + localvel;
    newpos = oldpos + localvel;
    
  // Part2: legs
  our_cd_contact.Empty ();

  transform_newpos = csOrthoTransform (csMatrix3(), newpos);

  cdsys->ResetCollisionPairs ();	

  if (cd)
    hits = CollisionDetect (bottomCollider, current_sector,
      &transform_newpos, &transform_oldpos);
  else
    hits = 0;

  bool stepDown = true;
    
    for (i = 0; i < our_cd_contact.GetSize (); i++ )
    {
        csCollisionPair cd = our_cd_contact[i];
        csPlane3 obstacle (cd.a2, cd.b2, cd.c2);
        csVector3 normal = obstacle.Normal();
        float norm = normal.Norm ();
        
        // Ensure this is a big-enough triangle to count as a collision.
        if (fabs (norm) < 1e-4f ) continue;
        
        csVector3 n = normal / norm;
        
        csVector3 line[2];
        
        // This needs to be done for numerical inaccuracies in this test
        // versus the collision system test.
        if(!FindIntersection (cd,line))
            continue;
        
        // Is it a collision with a ground polygon?
        //  (this tests for the angle between ground and colldet
        //  triangle)
        // sin(Pi/4) == 0.7
        if(n.y >= 0.7)
        {
            stepDown = false;
        }
    }
    
    
  // Only able to step down if we aren't jumping or falling
  if (vel.y != 0)
    stepDown = false;
  if(stepDown)
  {
    // Try stepping down
    newpos.y -= bottomSize.y/2;
    transform_newpos = csOrthoTransform (csMatrix3(), newpos);

    our_cd_contact.Empty ();

    cdsys->ResetCollisionPairs ();	

    if (cd)
      hits = CollisionDetect (bottomCollider, current_sector,
        &transform_newpos, &transform_oldpos);
    else
      hits = 0;
  }

  // Falling unless proven otherwise
  onground = false;

  float maxJump = newpos.y + bottomSize.y;
  float max_y = -1e9;
    float max_y_steep = -1e9;

  // Keep moving the model up until it no longer collides
  while (hits > 0 && newpos.y < maxJump)
  {
    hitsurface = false;
    for (i = 0; i < our_cd_contact.GetSize (); i++ )
    {
      csCollisionPair cd = our_cd_contact[i];
      csPlane3 obstacle (cd.a2, cd.b2, cd.c2);
      csVector3 normal = obstacle.Normal();
      float norm = normal.Norm ();

      // Ensure this is a big-enough triangle to count as a collision.
      if (fabs (norm) < 1e-4f ) continue;

      csVector3 n = normal / norm;

      csVector3 line[2];

      // This needs to be done for numerical inaccuracies in this test
      // versus the collision system test.
      if(!FindIntersection (cd,line))
        continue;

      // Is it a collision with a ground polygon?
      //  (this tests for the angle between ground and colldet
      //  triangle)
      // sin(Pi/4) == 0.7
      if((fabs(n.y) >= 0.7))
      {
          onground = true;
          // This is a ground triangle so we can move on top of it
          max_y = MAX(MAX(line[0].y, line[1].y)+shift.y,max_y);
      }
     
          // This is not a ground polygon so we can move down to rest on it
          max_y_steep = MAX(MAX(line[0].y, line[1].y)+shift.y, max_y_steep);
      hitsurface = true;
    }

    // This prevents us from going up if there is no surface to rest on.
    if(!onground && max_y_steep > oldpos.y)
    {
        // in this case we do not accept the move
        // hitsurface must be true so this will be adjusted down later
        newpos = oldpos;
        newpos.y = oldpos.y + 0.01f;
        break;
    }
        
    if (hitsurface)
    {
      // Temporarily lift the model up so that it passes the final check
      if(onground)
        newpos.y = max_y + 0.01f;
      else
          newpos.y = max_y_steep + 0.01f;
      break;
        
      our_cd_contact.Empty ();

      transform_newpos = csOrthoTransform (csMatrix3(), newpos);

      cdsys->ResetCollisionPairs ();	

      if (cd)
        hits = CollisionDetect (bottomCollider, current_sector,
          &transform_newpos, &transform_oldpos);
      else
        hits = 0;
    }
    else
      hits = 0;
  }

  if (hits == 0)
  {
    // Reaction force - Disabled because no distinction made between physics
    // engine predicted velocity and player controlled velocity

    // vel = (mat.GetInverse()*localvel)/delta;

    if (stepDown)
      // No steps here, so readjust position back up
      newpos.y += bottomSize.y / 2;
  }

#ifdef COLLIDE_ACTOR_DEBUG
     
#endif
  // Part 2.5: check top again and revert move if we're still in a wall.
  our_cd_contact.Empty ();
  cdsys->ResetCollisionPairs ();
  transform_newpos = csOrthoTransform (csMatrix3(), newpos);

  // Bring the model back down now that it has passed the final check
  if (onground)
    newpos.y -= 0.02f;
  else if(hitsurface)
    newpos.y -= 0.01f;

  if (cd)
    hits = CollisionDetect (topCollider, current_sector,
      &transform_newpos,&transform_oldpos);
  else
    cd = 0;

  hits = 0;
  for (i = 0; i < our_cd_contact.GetSize () ; i++ )
  {
    csCollisionPair& cd = our_cd_contact[i];
    csPlane3 obstacle (cd.a2, cd.b2, cd.c2);
    csVector3 normal = obstacle.Normal ();

    float norm = normal.Norm ();
    if (fabs (norm) < SMALL_EPSILON) continue;
    csVector3 unit = normal / norm;

    if (unit * (newpos - oldpos) > 0) continue;
    hits++;
    break;
  }

  if (hits > 0)
  {
    // No move possible without a collision with the torso
    newpos = oldpos;
    return false;
  }

  return true;
}

void csColliderActor::SetRotation (const csVector3& rot)
{
  rotation = rot;
  if (camera)
  {
    csMatrix3 rot;
    if (fabs (rotation.x) < SMALL_EPSILON && fabs (rotation.z) < SMALL_EPSILON)
      rot = csYRotMatrix3 (rotation.y);
    else
      rot = csXRotMatrix3 (rotation.x) * csYRotMatrix3 (rotation.y)
    	* csZRotMatrix3 (rotation.z);
    csOrthoTransform ot (rot, camera->GetTransform().GetOrigin ());
    camera->SetTransform (ot);
  }
}

bool csColliderActor::RotateV (float delta,
	const csVector3& angularVelocity)
{
  // rotation
  if (angularVelocity < SMALL_EPSILON)
    return false;

  csVector3 angle = angularVelocity * delta;
#if 0
  if (angleToReachFlag)
  {
    const csMatrix3& transf = movable->GetTransform ().GetT2O ();
    float current_yrot = Matrix2YRot (transf);
    current_yrot = atan2f (sin (current_yrot), cos (current_yrot) );
    float yrot_delta = fabs (atan2f (sin (angleToReach.y - current_yrot),
    	cos (angleToReach.y - current_yrot)));
    if (fabs(angle.y) > yrot_delta)
    {
      angle.y = (angle.y / fabs (angle.y)) * yrot_delta;
      angularVelocity = 0;
      angleToReachFlag = false;
    }
  }
#endif

  if (movable)
  {
    csYRotMatrix3 rotMat (angle.y);
    movable->SetTransform (movable->GetTransform().GetT2O() * rotMat);
  }
  else
  {
    SetRotation (rotation + angle);
  }
  return true;
}

#define ABS_MAX_FREEFALL_VELOCITY 107.3f
// Do the actual move
bool csColliderActor::MoveV (float delta,
	const csVector3& velBody)
{
  hit_meshes.Empty ();
  
  if (velBody < SMALL_EPSILON && velWorld < SMALL_EPSILON
  	&& onground)
    return false;  // didn't move anywhere

  csMatrix3 mat;

  // To test collision detection we use absolute position and transformation
  // (this is relevant if we are anchored). Later on we will correct that.
  csReversibleTransform fulltransf;
  if (movable)
  {
    fulltransf = movable->GetFullTransform ();
  }
  else
  {
    //fulltransf = camera->GetTransform ();
    // For camera we ignore the x and z rotation when moving so here we
    // construct a new transform containing only the y rotation.
    fulltransf.SetO2T (csYRotMatrix3 (rotation.y));
    fulltransf.SetOrigin (camera->GetTransform ().GetOrigin ());
  }
  mat = fulltransf.GetT2O ();

  csVector3 worldVel (fulltransf.This2OtherRelative (velBody) + velWorld);
  csVector3 oldpos (fulltransf.GetOrigin ());
  csVector3 newpos (worldVel*delta + oldpos);

  // Check for collisions and adjust position
  if (!AdjustForCollisions (oldpos, newpos, worldVel, delta))
  {
      if(worldVel.y != 0)
      {
          // Try to move again without the x and z vectors
          worldVel = fulltransf.This2OtherRelative (csVector3(0, velBody.y, 0)) + velWorld;
          if(!AdjustForCollisions (oldpos, newpos, worldVel, delta))
              return false;
      }
      else
          return false;                   // We haven't moved so return early
  }
  
  bool mirror = false;

  // Update position to account for portals
  iSector* new_sector;
  if (movable)
    new_sector = movable->GetSectors ()->Get (0);
  else
    new_sector = camera->GetSector ();
  iSector* old_sector = new_sector;

  // @@@ Jorrit: had to do this add!
  // We need to measure slightly above the position of the actor or else
  // we won't really cross a portal.
  float height5 = (bottomSize.y + topSize.y) / 20.0;
  newpos.y += height5;
  csMatrix3 id;
  csOrthoTransform transform_oldpos (id, oldpos + csVector3 (0, height5, 0));

  new_sector = new_sector->FollowSegment (transform_oldpos,
      newpos, mirror, true);
  newpos.y -= height5;
  if (new_sector != old_sector)
  {
    if (movable)
      movable->SetSector (new_sector);
    else
      camera->SetSector (new_sector);
  }

  if (!onground)
  {
    // gravity! move down!
    velWorld.y  -= gravity * delta;
    /*
    * Terminal velocity
    *   ((120 miles/hour  / 3600 second/hour) * 5280 feet/mile)
    *   / 3.28 feet/meter = 53.65 m/s
    */
    // The body velocity is figured in here too.
    if (velWorld.y < 0)
    {
      if (fulltransf.This2OtherRelative(velBody).y
      		+ velWorld.y < -(ABS_MAX_FREEFALL_VELOCITY))
        velWorld.y = -(ABS_MAX_FREEFALL_VELOCITY)
		- fulltransf.This2OtherRelative(velBody).y;
      if (velWorld.y > 0)
        velWorld.y = 0;
    }
  }
  else
  {
    if (velWorld.y < 0)
    {
      velWorld.y = 0;
    }
  }

  if (movable)
  {
    movable->GetTransform ().SetOrigin (newpos);
    mesh->PlaceMesh ();
    movable->UpdateMove ();
  }
  else
  {
    camera->GetTransform ().SetOrigin (newpos);
  }

  return true;
}

#define MAX_CD_INTERVAL 1
#define MAX_CD_ITERATIONS 20

static float ComputeLocalMaxInterval (
	const csVector3& bodyVel,
	const csVector3& intervalSize)
{
  float local_max_interval =
    MIN (MIN ((fabs (bodyVel.y) < SMALL_EPSILON)
  	? MAX_CD_INTERVAL
	: fabs (intervalSize.y/bodyVel.y), (fabs (bodyVel.x) < SMALL_EPSILON)
		? MAX_CD_INTERVAL
		: fabs (intervalSize.x/bodyVel.x)),
			(fabs (bodyVel.z) < SMALL_EPSILON)
			? MAX_CD_INTERVAL
			: fabs (intervalSize.z/bodyVel.z));
  return local_max_interval;
}

bool csColliderActor::Move (float delta, float speed, const csVector3& velBody,
	const csVector3& angularVelocity)
{
  // Artificial cap to avoid falling through objects when the framerate
  // is extremely low.
  if (delta > .3f) delta = .3f;

  //float local_max_interval;
  bool rc = false;

  csReversibleTransform fulltransf;
  if (movable)
  {
    fulltransf = movable->GetFullTransform ();
  }
  else
  {
    //fulltransf = camera->GetTransform ();
    // For camera we ignore the x and z rotation when moving so here we
    // construct a new transform containing only the y rotation.
    fulltransf.SetO2T (csYRotMatrix3 (rotation.y));
    fulltransf.SetOrigin (camera->GetTransform ().GetOrigin ());
  }
  //const csMatrix3& transf = fulltransf.GetT2O ();
  //float yrot = Matrix2YRot (transf);

  // Calculate the total velocity (body and world) in OBJECT space.
  csVector3 bodyVel (fulltransf.Other2ThisRelative (velWorld) + velBody);

  float local_max_interval = ComputeLocalMaxInterval (bodyVel, intervalSize - csVector3(0.005f));

  // Compensate for speed
  local_max_interval /= speed;

  int maxiter = MAX_CD_ITERATIONS;

  while (delta > local_max_interval && maxiter > 0)
  {
    maxiter--;
    rc |= MoveV (local_max_interval * speed, velBody);

    rc |= RotateV (local_max_interval * speed, angularVelocity);
    //yrot = Matrix2YRot (transf);

    if (!rc) return rc;

    // The velocity may have changed by now
    bodyVel = fulltransf.Other2ThisRelative(velWorld) + velBody;

    delta -= local_max_interval;
    local_max_interval = ComputeLocalMaxInterval (bodyVel, intervalSize);

    // Compensate for speed
    local_max_interval /= speed;
    // Err on the side of safety
    local_max_interval -= 0.005f;
  }

  if (delta)
  {
    rc |= MoveV (delta * speed, velBody);
    rc |= RotateV (delta * speed, angularVelocity);
  }

  return rc;
}