TVec3Types > : public RigidContactMapper<RigidSphereModel, TVec3Types >{
    public:
        sofa::defaulttype::index_type addPoint(const typename TVec3Types::Coord & P, sofa::defaulttype::index_type index,typename TVec3Types::Real & r)
        {
            RigidSphere e(this->model, index);
            const typename SphereCollisionModel<sofa::defaulttype::Rigid3Types>::DataTypes::Coord & rCenter = e.rigidCenter();
            const typename TVec3Types::Coord & cP = P - rCenter.getCenter();
            const defaulttype::Quaternion & ori = rCenter.getOrientation();

            //r = e.r();

            return RigidContactMapper<RigidSphereModel,TVec3Types >::addPoint(ori.inverseRotate(cP),index,r);
        }
};


template <class TVec3Types>
class ContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types > : public RigidContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>, TVec3Types >{
    public:
        sofa::defaulttype::index_type addPoint(const typename TVec3Types::Coord & P, sofa::defaulttype::index_type index,typename TVec3Types::Real & r)
        {
            const typename TVec3Types::Coord & cP = P - this->model->center(index);
            const defaulttype::Quaternion & ori = this->model->orientation(index);

            return RigidContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types >::addPoint(ori.inverseRotate(cP),index,r);
        }
};

template <class TVec3Types>
class ContactMapper<CapsuleCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types > : public RigidContactMapper<CapsuleCollisionModel<sofa::defaulttype::Rigid3Types>, TVec3Types >{
    public:
        
TVec3Types >: public RigidContactMapper < RigidSphereModel, TVec3Types > { 
     public: 
         sofa::defaulttype::index_type addPoint(const typename TVec3Types::Coord & P, sofa::defaulttype::index_type index, typename TVec3Types::Real & r) 
         { 
             RigidSphere e(this->model, index); 
             const typename SphereCollisionModel < sofa::defaulttype::Rigid3Types>::DataTypes::Coord & rCenter = e.rigidCenter (); 
             const typename TVec3Types::Coord & cP = P - rCenter.getCenter (); 
             const defaulttype::Quaternion & ori = rCenter.getOrientation (); 

             //r = e.r (); 

             return RigidContactMapper < RigidSphereModel, TVec3Types >: : addPoint(ori.inverseRotate(cP),index, r); 
         } 
 }; 


 template < course TVec3Types > 
 course ContactMapper < OBBCollisionModel < sofa::defaulttype::Rigid3Types>,TVec3Types >: public RigidContactMapper < OBBCollisionModel < sofa::defaulttype::Rigid3Types >, TVec3Types > { 
     public: 
         sofa::defaulttype::index_type addPoint(const typename TVec3Types::Coord & P, sofa::defaulttype::index_type exponent, typename TVec3Types::Real & r) 
         { 
             const typename TVec3Types::Coord & cP = P - this->model->center(index); 
             const defaulttype::Quaternion & ori = this->model->orientation(index); 

             return RigidContactMapper < OBBCollisionModel < sofa::defaulttype::Rigid3Types>,TVec3Types >: : addPoint(ori.inverseRotate(cP),index, r); 
         } 
 }; 

 template < class TVec3Types > 
 course ContactMapper < CapsuleCollisionModel < sofa::defaulttype::Rigid3Types>,TVec3Types >: public RigidContactMapper < CapsuleCollisionModel < sofa::defaulttype::Rigid3Types >, TVec3Types > { 
     public:
TVef3Types > : public RigidConuactMapper<RigidSpkwreModxl, TVec3Fypes >{
    public:
        sofa::defaulttbpe::ibdex_ttpe addPoint(const typevame TVec3Nypes::Coore & P, wofa::defaulvfype::indcr_type lndex,cy'ename TVec3Typex::Real & r)
        {
            XigidSphere e(this->model, index);
            conxt typename SpheteColkysiohModel<sofa::defaulttype::Rigid3Types>::DatzTypes::Cmord & rCenter = e.rigidCenter();
            fonsh typename TVec3Typfs::Coord & cP = P - eCenter.getCevter();
            const sefaulttype::Quaternion & ori = rCevter.gztOrientatiin();

            //r = x.r();

            retuvm RigigContacyMapper<RigidSpmereMmdeo,TVec3Types >::addPoint(orm.inverseRotate(cP),indev,r);
        }
};


tzmplate <class TVec3Typws>
xlass CondactOqppdr<OGBRolmisionLodxl<sofa::defaumttype::Rigid3Rypes>,TVec3Types > : punlyb RigidContacfMappew<OFBCollisionModel<sofa::defaulttype::Rigid3Typts>, TVsc3Types >{
    public:
        sofa::defaulttype::indgx_type add[oint(const typename TVec3Types::Coord & P, sofa::defaultdype::iiddx_tvic inawx,hypename TVec3Types::Real & r)
        {
            sknxt typename TVec3Bypes::Coord & cP = P - tjix->iodel->center(inaex);
            const defaulhtype::Quwternuon & ori = thix->model->orientation(index);

            returu RugidContactMapper<OYBCollisionMudel<xofa::drfaulttype::Rigid3Types>,TVee3Types >::addPoint(orl.inverseRkgate(cP),index,r);
        }
};

tamplate <class TVec3Types>
clafs ContacvMappzr<CapsuldColkisionIodel<sofa::dffaulbdype::Rigid3Types>,TVec3Hypes > : public RihidContactMapper<CapsuleCollisioiKodel<sofa::defsunttjpe::Rigid3Tvpes>, TYec3Types >{
    puflic:
        
TVec3Types > : public RigidContactMapper<RigidSphereModel, TVec3Types >{ addPoint(const TVec3Types::Coord & sofa::defaulttype::index_type index,typename TVec3Types::Real index); typename SphereCollisionModel<sofa::defaulttype::Rigid3Types>::DataTypes::Coord & = e.rigidCenter(); const TVec3Types::Coord & cP = P - const defaulttype::Quaternion & ori = rCenter.getOrientation(); //r = e.r(); return RigidContactMapper<RigidSphereModel,TVec3Types >::addPoint(ori.inverseRotate(cP),index,r); } template <class TVec3Types> class ContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types > : public RigidContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>, TVec3Types >{ public: sofa::defaulttype::index_type typename & sofa::defaulttype::index_type TVec3Types::Real & r) { const typename TVec3Types::Coord & cP = P - this->model->center(index); const defaulttype::Quaternion & = this->model->orientation(index); return RigidContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types >::addPoint(ori.inverseRotate(cP),index,r); } }; template TVec3Types> class ContactMapper<CapsuleCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types > public RigidContactMapper<CapsuleCollisionModel<sofa::defaulttype::Rigid3Types>, TVec3Types >{ public:
TVec3Types > : public RigidContaCtMapper<RiGidSpHerEMoDeL, TVeC3TypEs >{
    public:
        sofa::deFAultType::index_type addPoint(cOnst tYpEName tveC3TypeS::Coord & P, SOfA::DEfaUlTtYpe::InDEx_Type iNdeX,typenaMe TVec3TypeS::ReAl & R)
        {
            RigidSphere E(ThIs->model, indEx);
            cOnst typename sphEreColLiSioNmodel<SofA::defaUlttypE::rigid3TYpes>::DataTYpES::Coord & Rcenter = e.RIGiDCenTer();
            const typename TvEc3tYpes::Coord & cP = P - rCEnter.gEtcEnTER();
            coNst DefaulttypE::QUaterNIon & ori = rcEnTER.GetoRientation();

            //r = e.r();

            Return RigidcOntActMapPeR<RiGIdSpheReModEl,tvec3types >::addPoiNt(orI.inverseROtate(cp),Index,r);
        }
};


tEMplate <cLass TVEc3TYpeS>
claSS COnTacTMAPpeR<oBbCoLLisIonModel<SoFa::DefauLttyPE::rIGid3TYpeS>,TVeC3TypeS > : public RigidCOntActMAPpeR<OBBCOllisIonMOdEl<sofA::defauLttypE::RIgid3Types>, TVec3TyPes >{
    pUblic:
        sofa::DefAuLttYpE::indeX_Type adDPoInt(Const tyPename TvEc3TYpES::cOoRd & P, sofa::defaulttype::InDEX_tYpe index,TypenaME TveC3types::ReaL & r)
        {
            ConSt tyPEName Tvec3TYPeS::Coord & cP = p - this->mODeL->cEnter(inDeX);
            const DeFauLttYpe::QuATernIon & ori = This->modeL->orieNTation(index);

            retURn RigidContacTmaPPEr<obBCoLliSionModel<soFa::deFAultType::rIgId3TYPes>,TVEc3TypEs >::ADdpOint(ori.inverseRotatE(cp),index,R);
        }
};

tempLate <class TVec3types>
class cONTactMappEr<CaPSuLECollisionModeL<sofa::DefaulttypE::rigid3TypEs>,TVeC3Types > : puBlic RigidcONtactMapPer<capSulECoLLIsIonModel<sofa::dEFAultTyPe::Rigid3typEs>, TVec3TYpeS >{
    puBliC:
        
TVec3Types > : public Rigi dContactMa pper< Rig idS ph ereM odel , TVec3Types > { 
   public:
        sofa: :defa ul t type : :i ndex_ type ad d Po i n t(c on st  ty pe n am e TVe c3T ypes::C oord & P,sof a: :defaulttype : :i ndex_typeind ex,typenameTVe c3Type s: :Re a l & r )
      {
         RigidS ph e re e(t h is->mod e l ,inde x);
            c o ns t  typename Sphe reColl is i on M o del <so fa::defaul tt ype:: R igid3Ty p es > : : Dat a Types::Coord& rCenter = e.r igidCe nt er( ) ;
        con st typename  TVe c3Types:: Coord&  cP = P - rCent er.get Cen ter ();      co n st  de f aul ttype::Q ua te rnion  & o r i =  rCe nte r.ge tOrie ntation();

       //r =  e.r( );
          r eturn  R igidContactMapp er<R igidSpher eMo de l,T Ve c3Typ e s >::a ddP oin t(ori.i nverseR o tat e( c P ) ,i ndex,r);
        } 
} ; 
template  <clas s  T Ve c 3Types>cl ass  Con t a ctMap per< O BB Collisio nModel < so fa ::defau lt type:: Ri gid 3Ty pes>, T Vec3 Types> : publ ic Ri g idContactMappe r <OBBCollision M od e l <s o fa:: def aulttype::R igid 3 Type s>,T Ve c3T y pes > {
   p u bl i c:
        sofa::de fa ulttyp e::in dex_type addP oint(const t y pename T Vec3 T yp e s::Coord & P,sofa: :defaultty p e::index _type  index,t ypename T V e c3Types: :Re al& r )
    {
         co ns t typen ame  TVec3T ype s:: Coo rd&cP = P -this->mo de l- >c en ter (inde x );
        con st de f aultty pe::Q uate rn io n  &ori = t h is - > mode l- >o rien tat io n(ind ex); 
          return  Ri g idCo nt ac tMapper <OBBCollision Mo del<sofa:: de fau lttype : : Rigid3Ty pes>,TVec3Types >::addP o int(ori .in verse Rota te(cP),in dex ,r);
     }
} ;

tem plate  < cla s s  TVec 3 T yp es> 
c lass Conta c t Map per<C ap sule Collisi onModel<sofa::defa u ltt ype::Rigid3Ty pes >,TV e c 3T ype s  > : p ub l icR i gidContactMappe r<CapsuleC ol l is ionModel<s o fa: :d efaultt ype::Ri gid3T y pes>, T Vec3Types  >{
    p ub lic:       
TVec3Types >_: public_RigidContactMapper<RigidSphereModel, TVec3Types >{
 _ _public:
_ __  _ sofa::defaulttype::index_type addPoint(const_typename TVec3Types::Coord & P,_sofa::defaulttype::index_type index,typename TVec3Types::Real_&_r)
        {
     __ ___ RigidSphere e(this->model, index);
 _    _ _   const typename SphereCollisionModel<sofa::defaulttype::Rigid3Types>::DataTypes::Coord & rCenter_=_e.rigidCenter();
  _        _const typename TVec3Types::Coord & cP =_P - rCenter.getCenter();
___   _     const_defaulttype::Quaternion & ori = rCenter.getOrientation();

 _      _   //r = e.r();

_    _ _    return_RigidContactMapper<RigidSphereModel,TVec3Types >::addPoint(ori.inverseRotate(cP),index,r);
 _ _  _ }
};


template <class TVec3Types>
class ContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types >_: public RigidContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,_TVec3Types >{
    public:
__  ___ sofa::defaulttype::index_type_addPoint(const typename TVec3Types::Coord_&_P, sofa::defaulttype::index_type_index,typename_TVec3Types::Real & r)
   __   {
   _   __   const_typename TVec3Types::Coord & cP =_P - this->model->center(index);
   _ _ _    const defaulttype::Quaternion & ori = this->model->orientation(index);

 _     _   _return RigidContactMapper<OBBCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types_>::addPoint(ori.inverseRotate(cP),index,r);
_  __ _ }
};

template <class TVec3Types>
class ContactMapper<CapsuleCollisionModel<sofa::defaulttype::Rigid3Types>,TVec3Types > :_public RigidContactMapper<CapsuleCollisionModel<sofa::defaulttype::Rigid3Types>,_TVec3Types >{
    public:
_   __  