source: orxonox.OLD/trunk/src/lib/collision_reaction/cr_physics_full_walk.cc @ 10549

/*
   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: Patrick Boenzli
   co-programmer: ...
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_REACTION

#include "collision.h"
#include "collision_event.h"

#include "physics_interface.h"

#include "world_entity.h"
#include "cr_physics_full_walk.h"
#include "collision_reaction.h"

#include <vector>

#include "debug.h"

#include "aabb.h"

#include "cr_defs.h"

namespace CoRe
{

  ObjectListDefinition(CRPhysicsFullWalk);
  /**
   *  standard constructor
   */
  CRPhysicsFullWalk::CRPhysicsFullWalk ()
      : CollisionReaction()
  {
    this->registerObject(this, CRPhysicsFullWalk::_objectList);
  }


  /**
   *  standard deconstructor
   */
  CRPhysicsFullWalk::~CRPhysicsFullWalk ()
  {}


  /**
   * caluculates and applys the reaction to a specific collision
   *  @param collision the collision
   */
  void CRPhysicsFullWalk::reactToCollision(Collision* collision)
  {

    AABB* box = collision->getEntityA()->getModelAABB();
    WorldEntity* entity = collision->getEntityA();

    if( box == NULL)
    {
      PRINTF(2)("this model has no aabb box so there is no correct collision reaction implemented. skipping\n");
      return;
    }


    float CR_MAX_WALK_HEIGHT = 15.0f;
//    float CR_THRESHOLD = 0.2f;

    float height = 0.0f;
    float front = 0.0f;
    float back = 0.0f;
    float right = 0.0f;
    float left = 0.0f;


    std::vector<CollisionEvent*>::const_iterator it = collision->begin();
    for(; it != collision->end(); it++)
    {

      CollisionEvent* ce = (*it);
      Vector normal = ce->getGroundNormal();

      // calculate the collision position
      Vector collPos =  collision->getEntityA()->getAbsCoor()  + box->center - ce->getCollisionPosition();

      // test the 3 axis differently
      switch( ce->getType())
      {
          /* collision in the X-AXIS */
        case CoRe::CREngine::CR_COLLISION_TYPE_AXIS_X:
          front = collPos.len() - box->halfLength[0];

          // object is beneath the plane (ground)
          if( front <= 0.0f )
          {
            Vector dirX = entity->getAbsDirX();
            dirX.y = 0.0f;
            dirX.normalize();
            Vector backoff = dirX * front;

            entity->shiftCoor(backoff);
          }
          else if( ce->isInWall())
          {
            // object is already in the wall
            entity->setAbsCoor(entity->getLastAbsCoor());
          }
          break;

        case CoRe::CREngine::CR_COLLISION_TYPE_AXIS_X_NEG:
          back = collPos.len() - box->halfLength[0];

          // object is beneath the plane (ground)
          if( back <= 0.0f)
          {
            Vector dirX = entity->getAbsDirX();
            dirX.y = 0.0f;
            dirX.normalize();
            Vector backoff = dirX * back * -1.0f;

            entity->shiftCoor(backoff);
          }
          else if( ce->isInWall())
          {
            // object is already in the wall
            entity->setAbsCoor(entity->getLastAbsCoor());
          }
          break;


          /* collision in the Y-AXIS */
        case CoRe::CREngine::CR_COLLISION_TYPE_AXIS_Y_NEG:
          // calulate the height above ground
          height = collPos.len() - box->halfLength[1];


          // object is beneath the plane (ground)
          //         if(height >= 0.0f && height <= 0.0001f) break ;// Do nothing
          if( height < 0.0f && -height < CR_MAX_WALK_HEIGHT)
          {
            entity->shiftCoor(Vector(0.0f, -height + 0.00001, 0.0f));
            entity->setOnGround(true);
          }
          // object is already in the wall
          else if( ce->isInWall())
          {
            entity->setAbsCoor(entity->getLastAbsCoor());
            PRINTF(0)("ground collision: reset pos\n");
          }
          else
          {
            // entity is not on ground
            entity->setOnGround(false);
          }
          break;


          /* collision in the Z-AXIS */
        case CoRe::CREngine::CR_COLLISION_TYPE_AXIS_Z:

          right = collPos.len()  - box->halfLength[2];

          // object is beneath the plane (ground)
          if( right <= 0.0f )
          {
            Vector dirZ = entity->getAbsDirZ();
            dirZ.y = 0.0f;
            dirZ.normalize();
            Vector backoff = dirZ * right;
            entity->shiftCoor(backoff);
          }
          else if( ce->isInWall())
          {
            // object is already in the wall
            entity->setAbsCoor(entity->getLastAbsCoor());
          }
          break;


          // collision in the z-axis
        case CoRe::CREngine::CR_COLLISION_TYPE_AXIS_Z_NEG:

          left = collPos.len()  - box->halfLength[2];

          // object is beneath the plane (ground)
          if( left <= 0.0f )
          {
            Vector dirZ = entity->getAbsDirZ();
            dirZ.y = 0.0f;
            dirZ.normalize();
            Vector backoff = dirZ * left*-1.0f;
            entity->shiftCoor(backoff);
          }
          // object is already in the wall
          else if( ce->isInWall())
          {
            entity->setAbsCoor(entity->getLastAbsCoor());
          }
          break;
      }
    }
    //PRINTF(0)("collision distances: x: %f, y: %f, z: %f\n", front, height, side);
  }




}