/*
0         1         2         3         4         5         6         7
01234567890123456789012345678901234567890123456789012345678901234567890123456789
--------------------------------------------------------------------------------
-- Description : Geometric object collision part : Elt - Edg
--------------------------------------------------------------------------------
-- Creation date : 15 may 1997               Author : FPI
--------------------------------------------------------------------------------
*/
 
#include "acp_base.h"
#include "MTH.h"
#include "MEC.h"
#include "GMT.h"
#include "GEO.h"
#include "GLI.h"
 
#include "COL/CsEltEdg.h"
#include "COL/CsSphElt.h"
 
/*--------------------------------------------------------------------------------*/
/*-- Description : COL_fn_vCollideDynamicSphereWithStaticEdge()*/
/*--------------------------------------------------------------------------------*/
/*-- Creation date : 08 jan 1996               Author : FPI*/
/*--------------------------------------------------------------------------------*/
/* used*/
void COL_fn_vCollideStaticSphereWithStaticEdge (COL_tdstGVForCollision *p_stGV,
/* ANNECY AV {*/
                                                MTH3D_tdstVector * p_stNormal)
/* END ANNECY AV }*/
{
    /* WARNING : p_stVertex2 is now the point which DOES NOT BELONG to the edge and not the second point of the edge*/
 
#define C_xTwoEpsilon         (MTH_M_xDoubleToReal (0.002))
#define C_xOneMinusTwoEpsilon (MTH_M_xDoubleToReal (0.998))
 
  COL_fn_vUseEnlargeSphere (p_stGV);

  if (INT_fn_bDetectIntersectSegmentWithSphere ( p_stGV->p_stVertex1, &p_stGV->stEdgeVector,
                                                 &p_stGV->stDinST0Point, p_stGV->xDynamicRadius))
  {
      /* Here, we have collision with the 'end' position. We test now a collision with the 'start' position*/
 
      MTH3D_tdstVector  stMovementVector;
      /* stDinST01Vector is the translation to go from the 'end' position to the 'start' position of the sphere center*/
      /* and we need the translation to go from the 'start' position to the 'end' position*/
      MTH3D_M_vNegVector (& stMovementVector, & p_stGV->stDinST01Vector);
 

/* jt 220399 : restore real radius of ZDM */
	  COL_fn_vUseInitialSphere (p_stGV);

	  if (INT_fn_bDetectIntersectSegmentWithSphere (p_stGV->p_stVertex1, & p_stGV->stEdgeVector,
                                                    & p_stGV->stDinST1Point, p_stGV->xDynamicRadius))
      {
          /* Here, we have collision with the 'end' and the 'start' position*/
          MTH3D_tdstVector stVertex1ToCenter;
          MTH_tdxReal      xDotProduct;
          MTH3D_tdstVector stTempHit;
          MTH3D_tdstVector stTempNormal;
          MTH3D_tdstVector stTempDistance;
          MTH3D_tdstVector stEndPosition;
          ACP_tdxIndex     xEdgeEntity;
 
          /* The hit is the projection of the center of the sphere onto the edge*/
          MTH3D_M_vSubVector (& stVertex1ToCenter , & p_stGV->stDinST0Point , p_stGV->p_stVertex1);
          xDotProduct = MTH3D_M_xDotProductVector (& stVertex1ToCenter , & p_stGV->stEdgeVector);
          xDotProduct = MTH_M_xDiv (xDotProduct , MTH3D_M_xSqrVector (& p_stGV->stEdgeVector));
 
          /* DotProduct must be between 2 x epsilon and 1 - 2 x epsilon*/
          if (MTH_M_bLess    (xDotProduct , C_xTwoEpsilon        ))  xDotProduct = C_xTwoEpsilon;
          else
          if (MTH_M_bGreater (xDotProduct , C_xOneMinusTwoEpsilon))  xDotProduct = C_xOneMinusTwoEpsilon;
 
          MTH3D_M_vMulAddVector (& stTempHit , xDotProduct , & p_stGV->stEdgeVector , p_stGV->p_stVertex1);
 
          /* The normal is the normalized vector from the hit to the center*/
          MTH3D_M_vSubVector       (& stTempDistance , & p_stGV->stDinST0Point , & stTempHit);
          POS_fn_vMulMatrixVector  (& stTempDistance , p_stGV->p_stStaticGeomObjMatrix , & stTempDistance);
/* ANNECY AV {*/
/*          MTH3D_M_vNormalizeVector (& stTempNormal , & stTempDistance);*/
          /* Modification : the normal is the normal of the triangle : this function MUST be called for a specific triangle*/
		  /* If the static object has a zoom, we must not use it to compute the normal, so we use RotateVector and not MulMatrixVector*/
/*         POS_fn_vRotateVector (& stTempNormal , p_stGV->p_stStaticGeomObjMatrix , p_stNormal);*/
		   MTH3D_M_vMulMatrixVectorWithoutBuffer (& stTempNormal, & p_stGV->p_stStaticGeomObjMatrix->stRotationMatrix, p_stNormal);
/* END ANNECY AV }*/
 
          /* The translation vector is Radius x normal - stTempDistance*/
/* ANNECY AV {*/
/*          MTH3D_M_vNegVector (& stTempDistance , & stTempDistance);*/
/*          MTH3D_M_vMulAddVector (& stTempDistance , p_stGV->xDynamicRadius , & stTempNormal , & stTempDistance);*/
 
/* jt 130499 : il faut enlever le scale au rayon de la zone !!! */		   
		   xDotProduct = MTH_M_xSub ( MTH_M_xMul(p_stGV->xDynamicRadius,p_stGV->xStaticScale) , MTH3D_M_xDotProductVector (& stTempDistance , & stTempNormal));
/* end jt 130499 */
		   
		   MTH3D_M_vMulScalarVector (& stTempDistance , xDotProduct , & stTempNormal);
/* END ANNECY AV }*/
 
          if (MTH_M_bGreaterEqual (MTH3D_M_xGetZofVector (p_stGV->p_stVertex2), MTH3D_M_xGetZofVector (& stTempHit)))
               xEdgeEntity = COL_C_xHighEdgeEntity;
          else xEdgeEntity = COL_C_xLowEdgeEntity;
 
          POS_fn_vMulMatrixVertex (& stTempHit , p_stGV->p_stStaticGeomObjMatrix , & stTempHit);
 
          POS_fn_vMulMatrixVector (& stMovementVector , p_stGV->p_stStaticGeomObjMatrix , & stMovementVector);
          POS_fn_vMulMatrixVertex (& stEndPosition    , p_stGV->p_stStaticGeomObjMatrix , & p_stGV->stDinST0Point);
 
          /* We register the collision with Time = -1 to say that there was already a collision at the 'start' position*/
          COL_fn_vAddInStaticCollisionTable (MTH_C_MinusONE , & stTempDistance , & stTempHit , & stTempNormal ,
                                             p_stGV->hDynamicMaterial , p_stGV->hStaticMaterial ,
                                             COL_C_xSphereEntity , xEdgeEntity ,
                                             p_stGV->p_vParameter1 , p_stGV->sParameter2 ,
                                             & stMovementVector,p_stGV->p_stDynamicIndexedSphere->xRadius, & stEndPosition);
      }
      else
      {
          /* Here, we have collision during the movement : we call the dynamic collision*/
 
          MTH_tdxReal       xT;
          MTH3D_tdstVector  stTempVector;
          MTH3D_tdstVector  stHit;
          MTH3D_tdstVector  stNormal;
          MTH3D_tdstVector  stTempHit;
          MTH3D_tdstVector  stTempDistance;
          MTH3D_tdstVector  stTempNormal;
          MTH3D_tdstVector  stEndPosition;
          ACP_tdxIndex     xEdgeEntity;
 
          if (INT_fn_bIntersectSegmentWithCylinder (& p_stGV->stDinST1Point , & stMovementVector ,
                                                    p_stGV->p_stVertex1 , & p_stGV->stEdgeVector , p_stGV->xDynamicRadius ,
                                                    &xT, &stHit, &stNormal )) /* normal is the hit on the edge !*/
 
          {
              if (MTH_M_bGreaterEqual (MTH3D_M_xGetZofVector (p_stGV->p_stVertex2), MTH3D_M_xGetZofVector (& stNormal)))
                   xEdgeEntity = COL_C_xHighEdgeEntity;
              else xEdgeEntity = COL_C_xLowEdgeEntity;
 
              /* The hit*/
              POS_fn_vMulMatrixVertex (& stTempHit , p_stGV->p_stStaticGeomObjMatrix , & stNormal );
 
              /* The normal*/
              MTH3D_M_vSubVector ( &stTempVector, &stHit, &stNormal );
              MTH3D_M_vDivScalarVector ( &stTempVector, &stTempVector, p_stGV->xDynamicRadius);
/* ANNECY AV {*/
/*              POS_fn_vMulMatrixVector ( &stTempNormal, p_stGV->p_stStaticGeomObjMatrix, &stTempVector );*/
              /* Modification : the normal is the normal of the triangle : this function MUST be called for a specific triangle*/
/*              POS_fn_vMulMatrixVector  (& stTempNormal , p_stGV->p_stStaticGeomObjMatrix , p_stNormal);*/
			  MTH3D_M_vMulMatrixVectorWithoutBuffer (& stTempNormal, & p_stGV->p_stStaticGeomObjMatrix->stRotationMatrix, p_stNormal);
/* END ANNECY AV }*/
 
              /* The translation is not used*/
              MTH3D_M_vNullVector (& stTempDistance);
 
              POS_fn_vMulMatrixVector (& stMovementVector , p_stGV->p_stStaticGeomObjMatrix , & stMovementVector);
              POS_fn_vMulMatrixVertex (& stEndPosition    , p_stGV->p_stStaticGeomObjMatrix , & p_stGV->stDinST0Point);
 
              COL_fn_vAddInStaticCollisionTable (xT, & stTempDistance, & stTempHit, & stTempNormal,
                                                 p_stGV->hDynamicMaterial, p_stGV->hStaticMaterial,
                                                 COL_C_xSphereEntity, xEdgeEntity,
                                                 p_stGV->p_vParameter1, p_stGV->sParameter2,
                                                 & stMovementVector, p_stGV->p_stDynamicIndexedSphere->xRadius, & stEndPosition);
          }

/* ??? If there is no collision during the movement, the collision is in fact with the point and with be treated somewhere else
*/          
	
/* jt 240399 */		  
		   else
          {
				/* test collision with point */
				if (INT_fn_bDetectIntersectSegmentWithSphere ( p_stGV->p_stVertex1, &p_stGV->stEdgeVector, &p_stGV->stDinST0Point, p_stGV->xDynamicRadius))
				{
				  MTH3D_tdstVector stVertex1ToCenter;
				  MTH_tdxReal      xDotProduct;
 
				  // The hit is the projection of the center of the sphere onto the edge
				  MTH3D_M_vSubVector (& stVertex1ToCenter , & p_stGV->stDinST0Point , p_stGV->p_stVertex1);
				  xDotProduct = MTH3D_M_xDotProductVector (& stVertex1ToCenter , & p_stGV->stEdgeVector);
				  xDotProduct = MTH_M_xDiv (xDotProduct , MTH3D_M_xSqrVector (& p_stGV->stEdgeVector));
				  MTH3D_M_vMulAddVector (& stTempHit , xDotProduct , & p_stGV->stEdgeVector , p_stGV->p_stVertex1);
 
				  // The normal is the normalized vector from the hit to the center
				  MTH3D_M_vSubVector       (& stTempDistance , & p_stGV->stDinST0Point , & stTempHit);
				  POS_fn_vMulMatrixVector  (& stTempDistance , p_stGV->p_stStaticGeomObjMatrix , & stTempDistance);
				  MTH3D_M_vNormalizeVector (& stTempNormal , & stTempDistance);

				  // The translation vector is Radius x normal - stTempDistance
				  MTH3D_M_vNegVector (& stTempDistance , & stTempDistance);
				  MTH3D_M_vMulAddVector (& stTempDistance , p_stGV->xDynamicRadius , & stTempNormal , & stTempDistance);
 
				  POS_fn_vMulMatrixVertex (& stTempHit , p_stGV->p_stStaticGeomObjMatrix , & stTempHit);
 
     			  /* use real normal */
				  MTH3D_M_vMulMatrixVectorWithoutBuffer (& stTempNormal, & p_stGV->p_stStaticGeomObjMatrix->stRotationMatrix, p_stNormal);
				  POS_fn_vMulMatrixVector (& stMovementVector , p_stGV->p_stStaticGeomObjMatrix , & stMovementVector);
				  POS_fn_vMulMatrixVertex (& stEndPosition    , p_stGV->p_stStaticGeomObjMatrix , & p_stGV->stDinST0Point);

				  // we register with Time=1.5 to say :
				  // - we don't know the real xRate 
				  // - translation is ok
				  // - point type collision
				  COL_fn_vAddInStaticCollisionTable (MTH_M_xDoubleToReal(1.5) , & stTempDistance , & stTempHit , & stTempNormal ,
													 p_stGV->hDynamicMaterial , p_stGV->hStaticMaterial ,
													 COL_C_xSphereEntity , COL_C_xLowEdgeEntity ,
													 p_stGV->p_vParameter1 , p_stGV->sParameter2 , & stMovementVector,
													 p_stGV->p_stDynamicIndexedSphere->xRadius, & stEndPosition);

				}
		  }
/* end jt 240399 */
      }
  }
}
