/*****************************
File:      CollisionGroup.h
Language:   C++ (header)
Project:    H3DNetworkingUtils
The contents of this file are subject to the Mozilla Public License
Version 1.1 (the "License"); you may not use this file except in
compliance with the License. You may obtain a copy of the License at
http://www.mozilla.org/MPL/

Software distributed under the License is distributed on an "AS IS"
basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
License for the specific language governing rights and limitations
under the License.

The Original Code is H3DNetworkingUtils v1.0.

The Initial Developer of the Original Code is CSIRO.
Portions created by the Initial Developer are Copyright (C) 1009 CSIRO. All Rights Reserved.
Contributor(s):
    Chris Gunn  <Chris.Gunn@csiro.au> <ChrisJGunn@gmail.com>
***************************/

/// \file CollisionGroup.h
/// \brief Header file for CollisionGroup, used to calculate inter-object collisions and responses.

#ifndef CollisionGroup_H
#define CollisionGroup_H


#include "H3DNetworkingUtils/Config.h"
#include <H3D/X3DChildNode.h>
#include <H3D/Node.h>
#include <H3D/MFString.h>
#include "H3DNetworkingUtils/CollisionGeometry.h"
#include "H3DNetworkingUtils/CollisionSphere.h"
#include "H3DNetworkingUtils/CollisionSphereHierarchy.h"
//#include "CollidableLine.h"

namespace H3DNetworkingUtils {

/// \class CollisionGeometry
/// The CollisionGroup class contains the algorithm for testing collisions between 
/// CollidableDynamic nodes.
/// It holds 2 lists, collidingChildrenA and collidingChildrenB.
/// It checks for inter-object collision between each object in collidingChildrenA
/// against each object in collidingChildrenB.
/// Objects can be in both groups. An object is not checked against itself.
/// This collision testing uses the collidables field of each CollidableDynamic in the lists.
/// The collidables field holds a hierarchy of CollidableSpheres in layers. For each layer, the 
/// spheres are checked for intersection, and if they intersect, the spheres in the next layer 
/// are then checked.
/// If two spheres in the loweest layer intersect, the amount of intersection along with the 
/// CollidableDynamics' current momentum, are used to apply forces to both CollidabelDynamics
/// to represent the reaction to the collision.
/// Once two spheres in the hierarchy have been found to collide, no more spheres are tested.
/// This algorithm is designed to provide an approximate, but efficient collision mechanism. However
/// if the system frame rate is found to be too slow, it may be due to having too many CollidableSpheres 
/// in the hierarchies of the CollidableDynamics. TO solve this, the collidableDynamic's 
/// collisionResolution field should be increased.  \n
/// <b>Examples:</b>
///   - <a href="../../examples/CollisionGroupTest.x3d">CollisionGroupTest.x3d</a>

class H3D_NETWORKING_UTILS_DLL_SPEC CollisionGroup : public H3D::X3DChildNode {

public:
  
   typedef H3D::TypedMFNode<H3D::X3DChildNode> MFChild;
   
   CollisionGroup();
  static H3D::H3DNodeDatabase database;
  
  /// List of children that are compard to the collidingChildrenB \n
  /// access type: inputOutput \n
  /// basic type: MFNode \n
  /// default value: 
  auto_ptr<MFChild> collidingChildrenA;

  /// List of children that are compard to the collidingChildrenA \n
  /// access type: inputOutput \n
  /// basic type: MFNode \n
  /// default value: 
  auto_ptr<MFChild> collidingChildrenB;

  /// Text reporting on collisions at initialize time. \n
  /// access type: inputOutput \n
  /// basic type: MFString \n
  /// default value: 
  auto_ptr<H3D::MFString> userMsg;

  virtual void traverseSG(H3D::TraverseInfo & ti);

   static int lineSphereIntersection( const H3D::Vec3f &lineOrigin,
			    const H3D::Vec3f &lineGradient,
			    const H3D::Vec3f &sphereCentre,
			    H3D::H3DFloat sphereRadius,
			    H3D::Vec3f &soln1,
			    H3D::Vec3f &soln2 );
  
protected:
  
  void detectCollision( H3D::X3DChildNode*, H3D::X3DChildNode* );
  // performs collisions between a chain and a dynamic
  
  void detectCollision( CollidableDynamic*, CollidableDynamic* );  
  // performs collisions between two CollidableDynamics
  
  void detectCollision( CollisionGeometry*, CollidableDynamic* );
  void detectCollision( CollidableDynamic*, CollisionGeometry* );
  
  // performs collisions between two geometries
  void detectCollision( CollisionGeometry*, CollisionGeometry* );
  void detectCollision( CollisionSphere*, CollisionSphere* );
  void detectCollision( CollisionSphere*, CollisionSphereHierarchy* );
  void detectCollision( CollisionSphereHierarchy*, CollisionSphere* );
  void detectCollision( CollisionSphereHierarchy*, 
      CollisionSphereHierarchy* );
  
    
  // checkIntersection is an overloaded function which checks for 
  // intersections between two geometries.
  bool checkIntersection( CollisionSphere*, CollisionSphere*, H3D::Vec3f & penetration_dir, 
     H3D::H3DFloat & penetration_mag, H3D::Vec3f & global_contact_point );

   virtual void doFirstCollideMessage(CollidableDynamic * coll_A, CollidableDynamic * coll_B, 
                                      const H3D::Vec3f & penetration_vec);
   // Provide a mesage describing how to moigve objects so that they won't start in a collision state.

private:
  bool first_collide;
  int check_seg_num;
  bool collision_found;
};


inline H3D::Rotation inverse( const H3D::Rotation &rot )
{
  return H3D::Rotation( rot.axis, -rot.angle );
}

inline H3D::Matrix3f transpose( H3D::Matrix3f mat )
{
  H3D::Matrix3f ret;
  int i;
  int j;

  for ( i = 0; i < 3; i ++ ) {
    for ( j = 0; j < 3; j ++ ) {
      ret[i][j] = mat[j][i];
    }
  }

  return ret;
}

}

#endif 
