/*****************************
File:      CollidableDynamic.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 CollidableDynamic.h
/// \brief Header file for CollidableDynamic, a class that allows inter-object collision and response.


#ifndef CollidableDynamic_H
#define CollidableDynamic_H


#include "H3DNetworkingUtils/Config.h"
#include "H3DNetworkingUtils/Dynamic.h"
#include "H3DNetworkingUtils/CollisionSphereHierarchy.h"
#include <H3DUtil/Matrix4f.h>
#include <stack>
#include <H3D/SFInt32.h>
#include <H3D/X3DShapeNode.h>
#include <H3D/X3DComposedGeometryNode.h>
#include "H3DNetworkingUtils/CollisionSphere.h"

namespace H3DNetworkingUtils {

/// \class CollidableDynamic
/// The CollidableDynamic class is a Dynamic that has an associated hierarchy of spheres roughly covering the geometry.
/// These spheres can be used by a CollisionGroup node to detect collisions between the CollidableDynamics.
/// The spheres can be set manually, via the collisionSpheres field. 
/// They can also be generated automatically from the geometry, if autoGenerateCollisionSpheres is true.
/// collisionResolution gives an approximate diameter of the smallest collisionSphere automatically generated.
/// The hierarchy starts with a sphere covering the bounding box. This is then split into 3 along each dimentsion
/// for the next level in the hierarchy, and so on, until the collisionResolution is reached. If the enclosed geometry is 
/// an X3DComposedGeometryNode, (e.g. IndexedFaceSet) any generated spheres which do not contain any coordinates, 
/// are discarded.
/// When CollisionGroup performs collision detection, it starts with the top layer of the hierarchy. 
/// If there is a collision at this top layer, it then iterates throught the next layer, etc. 
/// A valid collision is only returned if a collision occurs at the lowestlayer.
/// 
/// collisionStiffness determines the force applied to objects when they collide
/// in much the same way as a Surface stiffness affects the feel of 
/// stylus-object collisions. Values should be between 0 & 1. Values greater than 1.0 may cause instability
///
/// collisionResolution is the diameter of the smallest collision sphere in the hierarchy.
/// If collisionResolution is not set, the smallest bounding box dimension is used.
///
/// debugCollisionSphereLayer provides a visual rendering of the collision spheres at a particular hierarchy layer.
/// If this is set to 0 (the default), no visual rendering occurs (but the collision spheres are still there).
/// This should always be the case when not debugging, as rendering uses resources unnecessarily.
/// If debugCollisionSphereLayer is set to 1, the outer hierarchy sphere is shown. 
/// If debugCollisionSphereLayer is set to 2, the next (smaller) spheres are shown, etc.
/// collisionResolution cannot be less that 0.001
///
/// CollidableDynamic is used by CollisionGroup to test for the actual collisions.
///
class H3D_NETWORKING_UTILS_DLL_SPEC CollidableDynamic : public Dynamic {

public:

   class H3D_NETWORKING_UTILS_DLL_SPEC MFCollidableGeometry : public H3D::TypedMFNode< CollisionGeometry > {
   public:
      virtual void onAdd( Node *n);
      virtual void onRemove( Node *n);
   };

   /// Constructor
   CollidableDynamic( 
            H3D::Inst< AddChildren              > _addChildren        = 0,
            H3D::Inst< RemoveChildren           > _removeChildren     = 0,
            H3D::Inst< MFChild                  > _children           = 0,
            H3D::Inst< H3D::SFNode              > _metadata           = 0,
            H3D::Inst< SFBound                  > _bound              = 0,
            H3D::Inst< H3D::SFVec3f             > _bboxCenter         = 0,
            H3D::Inst< H3D::SFVec3f             > _bboxSize           = 0,
            H3D::Inst< SFTransformedBound       > _transformedBound   = 0,
            H3D::Inst< SFMatrix4f               > _matrix             = 0,
            H3D::Inst< SFMatrix4f               > _accumulatedForward = 0,
            H3D::Inst< SFMatrix4f               > _accumulatedInverse = 0,
            H3D::Inst< H3D::SFVec3f             > _position           = 0,
            H3D::Inst< H3D::SFRotation          > _orientation        = 0,
            H3D::Inst< SFVelocity               > _velocity           = 0,
            H3D::Inst< H3D::SFVec3f             > _momentum           = 0,

            H3D::Inst< H3D::SFVec3f             > _force              = 0,
            H3D::Inst< SFAngularVelocity        > _angularVelocity    = 0,
            H3D::Inst< H3D::SFVec3f             > _angularMomentum    = 0, 
            H3D::Inst< SFSpin                   > _spin               = 0,
            H3D::Inst< H3D::SFVec3f             > _torque             = 0,
            H3D::Inst< H3D::SFFloat             > _mass               = 0,
            H3D::Inst< H3D::SFMatrix3f          > _inertiaTensor      = 0,
            H3D::Inst< SFMotion                 > _motion             = 0,
            
            H3D::Inst<H3D::MFVec3f              > _contactForces      = 0,
            H3D::Inst<H3D::MFVec3f              > _contactTorques     = 0,
            H3D::Inst<SumMFVec3f                > _totalContactForce  = 0,
            H3D::Inst<SumMFVec3f                > _totalContactTorque = 0,
            H3D::Inst<SumForces                 > _externalForces     = 0,
            H3D::Inst<SumTorques                > _externalTorques    = 0,
            H3D::Inst<H3D::SFVec3f              > _freedom            = 0,
            H3D::Inst<H3D::SFVec3f              > _angularFreedom            = 0,
            H3D::Inst<H3D::SFFloat              > _linearDamping      = 0,
            H3D::Inst<H3D::SFFloat              > _rotationalDamping  = 0,
            H3D::Inst<H3D::SFFloat              > _linearSpringStiffness = 0,
            H3D::Inst<H3D::SFFloat              > _angularSpringStiffness = 0,
            H3D::Inst<H3D::SFVec3f              > _globalPosition = 0,

            H3D::Inst< MFCollidableGeometry     > _collisionSpheres     = 0,
            H3D::Inst< H3D::SFFloat             > _collisionStiffness   = 0,
            H3D::Inst< H3D::SFFloat             > _collisionResolution  = 0);

  static H3D::H3DNodeDatabase database;

  virtual void initialize();
  
  /// The spheres that are used for collision testing \n
  /// access type: inputOutput \n
  /// basic type: MFNode \n
  /// default value: 
  auto_ptr< MFCollidableGeometry >    collisionSpheres;
  
  /// The stiffness used in the rebound calculation, determines the force applied to objects when they collide \n
  /// access type: inputOutput \n
  /// basic type: SFFloat \n
  /// default value: 0.4
  auto_ptr< H3D::SFFloat  >           collisionStiffness;

  /// The approximate minimum collision sphere diameter \n
  /// If -1, the minimum dimension of the bounding box will be used \n
  /// access type: inputOutput \n
  /// basic type: SFFloat \n
  /// default value: 0.02
  auto_ptr< H3D::SFFloat>             collisionResolution;

  /// If true, collison spheres will be generated at initialize time \n
  /// access type: inputOutput \n
  /// basic type: SFBool \n
  /// default value: FALSE
  auto_ptr< H3D::SFBool>              autoGenerateCollisionSpheres;

  /// If non_zero, the given layer will be graphically rendered \n
  /// Use for debugging and setup only - it may impinge on performance \n
  /// access type: inputOutput \n
  /// basic type: SFInt32 \n
  /// default value: 0
  auto_ptr< H3D::SFInt32>             debugCollisionSphereLayer;

  /// The proportion the collision spheres overlap \n
  /// access type: inputOutput \n
  /// basic type: SFFloat \n
  /// default value: 0.15
  auto_ptr< H3D::SFFloat>             overlap;
  
  /// Return a matrfix to convert global to local coords
  H3D::Matrix4f getGlobalToLocal() const {
    return accumulatedInverse->getValue();
  }
  /// Return a matrfix to convert local to global coords
  H3D::Matrix4f getLocalToGlobal() const {
    return accumulatedForward->getValue();
  }

 
protected:
   virtual void buildSphereHierarchy(const H3D::Matrix4f & trans, X3DGroupingNode * grpP, 
                                             CollisionSphere * parent_sphP, u_int layer,
                                             H3D::H3DFloat scale);

   virtual void createSpheres(const H3D::Matrix4f & trans, 
                              H3D::X3DShapeNode * shapeP, 
                              CollisionSphere * parent_sphP, 
                              u_int layer, H3D::H3DFloat scale);

   virtual void createSpheresOverGeometry(const H3D::Matrix4f & trans, 
                                          const H3D::Vec3f & offset_in_geom, 
                                          const H3D::Vec3f & size, 
                                          CollisionSphere * parent_sphP,
                                          u_int layer, H3D::X3DGeometryNode * geomP,
                                          H3D::H3DFloat scale);

   virtual CollisionSphere * createSphere(const H3D::Matrix4f & trans, bool leaf_node, u_int layer, 
                                           H3D::H3DFloat dia, const H3D::Vec3f & rel_pos);

   virtual bool containsPoints(CollisionSphere * sphP, 
                               H3D::X3DComposedGeometryNode * coord_setP);

private:
   u_int max_dim, middle_dim, min_dim;
   static const u_int split;
   H3D::Matrix4f identity_trans;
};

}

#endif 
