Contacts.h

Go to the documentation of this file.

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material unless having a written signed agreement with Algoryx Simulation AB, or having been
advised so by Algoryx Simulation AB for a time limited evaluation, or having purchased a
valid commercial license from Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#pragma once
 
#ifdef _MSC_VER
# pragma warning(push)
# pragma warning( disable : 4275 ) //  warning C4275: non dll-interface class
#endif
 
#include <agx/agxPhysics_export.h>
 
#include <agxCollide/agxCollide.h>
#include <agx/agx.h>
#include <agx/Referenced.h>
#include <agx/RigidBody.h>
#include <agx/Vec3.h>
#include <agxCollide/GeometryPair.h>
 
#include <agx/Material.h>
#include <agx/Vector.h>
#include <agx/Physics/GeometryEntity.h>
#include <agx/Physics/ContactPointEntity.h>
#include <agx/Physics/GeometryContactEntity.h>
#include <agx/Physics/BroadPhasePairEntity.h>
#include <agxCollide/LocalContactPoint.h>
 
 
namespace agxSDK
{
  class MaterialManager;
}
 
namespace agxCollide
{
  class Geometry;
 
 
  class AGXPHYSICS_EXPORT ContactPoint : public agx::Physics::ContactPointPtr
  {
    public:
      enum ContactForceComponents
      {
        NORMAL_FORCE = 0,     
        TANGENTIAL_FORCE_U,   
        TANGENTIAL_FORCE_V    
      };
 
      enum ContactPointState : agx::UInt8
      {
        IMPACTING = 1 << 0,  
        NONHOLONOMIC = 1 << 1,  
        NODAMPING = 1 << 2,
 
        HAVE_AREA = 1 << 3  
      };
 
    public:
      ContactPoint();
 
      ContactPoint(agx::Physics::ContactPointPtr contact);
 
      ContactPoint(agx::Vec3 thePoint, agx::Vec3f theNormal, agx::Real theDepth);
 
      ContactPoint(agx::Vec3 point, agx::Vec3f normal, agx::Real depth, agx::Real area, agx::Real volume);
 
 
 
 
      void clear();
 
      using agx::Physics::ContactPointPtr::localForce;
 
#ifndef SWIG
      agx::Real& localForce( size_t idx );
#endif
      agx::Real localForce( size_t idx ) const;
 
      agx::Vec3 getForce() const;
 
      agx::Real getForceMagnitude() const;
 
      agx::Vec3 getNormalForce() const;
 
      agx::Real getNormalForceMagnitude() const;
 
      agx::Real getTangentialForceUMagnitude() const;
 
      agx::Real getTangentialForceVMagnitude() const;
 
      agx::Real getTangentialForceMagnitude() const;
 
      agx::Vec3 getTangentialForce() const;
 
      agx::UInt32& faceIndex(size_t ith);
 
      agx::UInt32 const& faceIndex(size_t ith) const;
 
      agx::UInt8& faceFeature(size_t ith);
 
      agx::UInt8 const& faceFeature(size_t ith) const;
 
      agx::Physics::Geometry::ShapePtr& shape(size_t ith);
 
      agx::Physics::Geometry::ShapePtr const& shape(size_t ith) const;
 
      agx::Vec3 getWitnessPoint(size_t ith) const;
 
      agx::Bool hasState( ContactPointState contactPointState ) const;
 
      void setIsHolonomic( agx::Bool isHolonomic );
 
      agx::Bool isHolonomic() const;
 
      agx::Real getYoungsModulus() const;
 
      void setYoungsModulus( agx::Real youngsModulus );
 
      agx::Real getFrictionCoefficient(agx::ContactMaterial::FrictionDirection direction) const;
 
      void setFrictionCoefficient(agx::Real coefficient, agx::ContactMaterial::FrictionDirection direction = agx::ContactMaterial::BOTH_PRIMARY_AND_SECONDARY);
 
      agx::Real getRestitution() const;
 
      void setRestitution(agx::Real restitution);
 
      agx::Real getTangentialRestitution(agx::ContactMaterial::FrictionDirection direction);
 
      void setTangentialRestitution(agx::Real restitution, agx::ContactMaterial::FrictionDirection direction = agx::ContactMaterial::BOTH_PRIMARY_AND_SECONDARY);
 
      agx::Real getTangentialCohesion( agx::ContactMaterial::FrictionDirection direction ) const;
 
      void setTangentialCohesion( agx::Real tanCohesion, agx::ContactMaterial::FrictionDirection direction = agx::ContactMaterial::BOTH_PRIMARY_AND_SECONDARY );
  };
 
 
  template<typename T1, typename T2>
  void copyContactPoint(const T1& from, T2& to)
  {
    to.point() = from.point();
    to.normal() = from.normal();
    to.depth() = from.depth();
    to.area() = from.area();
    to.volume() = from.volume();
 
    to.velocity() = from.velocity();
    to.enabled() = from.enabled();
 
    to.shape1() = from.shape1();
    to.shape2() = from.shape2();
 
    to.faceIndex1() = from.faceIndex1();
    to.faceIndex2() = from.faceIndex2();
    to.faceFeature1() = from.faceFeature1();
    to.faceFeature2() = from.faceFeature2();
    to.state() = from.state();
  }
 
#ifndef SWIG
  template void copyContactPoint(const ContactPoint&, LocalContactPoint&);
  template void copyContactPoint(const LocalContactPoint&, ContactPoint&);
  template void copyContactPoint(const LocalContactPoint&, agx::Physics::ContactPointPtr&);
  template void copyContactPoint(const agx::Physics::ContactPointPtr&, LocalContactPoint&);
 
 
  AGX_STATIC_ASSERT(sizeof(ContactPoint) == sizeof(agx::Physics::ContactPointPtr));
#endif
  typedef agxData::Array<ContactPoint> ContactPointVector;
 
  class AGXPHYSICS_EXPORT GeometryContact : public agx::Physics::GeometryContactPtr
  {
    public:
      typedef ContactPoint PointType;
 
    public:
      enum ContactState {
        NO_CONTACT       = 0, 
        IMPACT_STATE     = 1, 
        CONTACT_STATE    = 2, 
        SEPARATION_STATE = 3  
      };
 
    public:
      GeometryContact();
 
      GeometryContact( agx::Physics::GeometryContactPtr contact );
 
      void clear();
 
      agx::Vec3f calculateRelativeVelocity( size_t pointIndex = 0 ) const;
 
      static bool calculateSurfaceVelocity( const agxCollide::Geometry* geometry1, const agxCollide::Geometry* geometry2, LocalContactPoint& p );
 
      static bool calculateSurfaceVelocity( const agxCollide::Geometry* geometry1, const agxCollide::Geometry* geometry2, ContactPoint& p );
 
      agxCollide::Geometry* geometry( size_t ith );
 
      const agxCollide::Geometry* geometry( size_t ith ) const;
 
      agx::RigidBody* rigidBody( size_t ith );
 
      const agx::RigidBody* rigidBody( size_t ith ) const;
 
      ContactPointVector& points();
 
      const ContactPointVector& points() const;
 
      void* getCustomData();
 
      const void* getCustomData() const;
 
      void setCustomData( void* customData );
 
      agx::ContactMaterial* getMaterial();
 
      const agx::ContactMaterial* getMaterial() const;
 
      const agx::ContactMaterial* getMaterial( bool ) const;
 
      agx::UInt8 state() const;
 
      void removeContactPoint( ContactPoint& point );
 
      void setEnable(bool flag);
 
      int contains(agx::RigidBody * body) const;
 
      int contains(agxCollide::Geometry * geometry) const;
 
      bool isEnabled() const;
 
      agx::Bool hasImpactingPoints() const;
 
 
      DOXYGEN_START_INTERNAL_BLOCK()
 
 
      
      void setGeometry( size_t ith, Geometry* geom );
 
      /***
    When called, this contact should be removed before entering the solver
      NOTE: Contacts are never removed, only marked as disabled
    \param immediately - Should the contact be removed immediately
    */
      void markForRemoval( bool immediately );
 
      bool shouldBeCalled() const;
 
      void setMaterial( agx::ContactMaterial* material );
 
      void setHasSurfaceVelocity( bool flag );
 
      GeometryPair getGeometryPair(); 
 
      void setHasInternalMaterial( bool flag );
      bool getHasInternalMaterial( ) const;
 
      void setState( agx::UInt8 state );
      DOXYGEN_END_INTERNAL_BLOCK()
  };
 
 
 
  inline int GeometryContact::contains(agx::RigidBody * body) const
  {
    if (this->rigidBody(0) == body) return 0;
    if (this->rigidBody(1) == body) return 1;
    return -1;
  }
 
  inline int GeometryContact::contains(agxCollide::Geometry * geometry) const
  {
    if (this->geometry(0) == geometry) return 0;
    if (this->geometry(1) == geometry) return 1;
    return -1;
  }
 
 
 
#ifndef SWIG
  //AGX_STATIC_ASSERT(sizeof(GeometryContact) == sizeof(agx::Physics::GeometryContactPtr));
#endif
 
  /* Implementation */
  inline ContactPoint::ContactPoint() : agx::Physics::ContactPointPtr( agx::Physics::ContactPointModel::createInstance() )
  {
  }
 
  inline ContactPoint::ContactPoint(agx::Physics::ContactPointPtr contact) : agx::Physics::ContactPointPtr(contact)
  {
  }
 
 
  AGX_FORCE_INLINE void ContactPoint::clear()
  {
    depth() = agx::Real( 0 );
    enabled() = true;
    point().set(0, 0, 0);
    normal().set(0, 0, 0);
    state() = agx::UInt8( 0 );
    tangentU().set(0, 0, 0);
    tangentV().set(0, 0, 0);
    localForce().set( 0, 0, 0 );
    velocity().set(0, 0, 0);
    faceIndex1() = 0;
    faceIndex2() = 0;
    faceFeature1() = 0;
    faceFeature2() = 0;
    area() = agx::Real( 0 );
    volume() = 0.;
    shape1() = agx::Physics::Geometry::ShapePtr();
    shape2() = agx::Physics::Geometry::ShapePtr();
  }
 
 
  AGX_FORCE_INLINE agx::Real& ContactPoint::localForce( size_t idx )
  {
    agxAssert(idx < 3);
    return localForce()[idx];
  }
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::localForce( size_t idx ) const
  {
    agxAssert(idx < 3);
    return localForce()[idx];
  }
 
 
 
  AGX_FORCE_INLINE agx::Vec3 ContactPoint::getForce() const
  {
    return getNormalForce() + getTangentialForce();
  }
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getForceMagnitude() const
  {
    agx::Real sqr =
      localForce()[ NORMAL_FORCE ] * localForce()[ NORMAL_FORCE ] +
      localForce()[ TANGENTIAL_FORCE_U ] * localForce()[ TANGENTIAL_FORCE_U ] +
      localForce()[ TANGENTIAL_FORCE_V ] * localForce()[ TANGENTIAL_FORCE_V ];
    return sqr > 0 ? std::sqrt( sqr ) : 0;
  }
 
 
  AGX_FORCE_INLINE agx::Vec3 ContactPoint::getNormalForce() const
  {
    return agx::Vec3(normal()) * localForce()[ NORMAL_FORCE ];
  }
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getNormalForceMagnitude() const
  {
    return std::abs(localForce()[ NORMAL_FORCE ]);
  }
 
 
  AGX_FORCE_INLINE agx::Vec3 ContactPoint::getTangentialForce() const
  {
    const agx::Vec3 forceU = agx::Vec3(tangentU()) * localForce()[ TANGENTIAL_FORCE_U ];
    const agx::Vec3 forceV = agx::Vec3(tangentV()) * localForce()[ TANGENTIAL_FORCE_V ];
    return forceU + forceV;
  }
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getTangentialForceUMagnitude() const
  {
    return std::abs(localForce()[ TANGENTIAL_FORCE_U ]);
  }
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getTangentialForceVMagnitude() const
  {
    return std::abs(localForce()[ TANGENTIAL_FORCE_V ]);
  }
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getTangentialForceMagnitude() const
  {
    agx::Real sqr = localForce()[ TANGENTIAL_FORCE_U ] * localForce()[ TANGENTIAL_FORCE_U ] + localForce()[ TANGENTIAL_FORCE_V ] * localForce()[ TANGENTIAL_FORCE_V ];
    return sqr > 0 ? std::sqrt( sqr ) : 0;
  }
 
  inline agx::Bool ContactPoint::hasState( ContactPoint::ContactPointState contactPointState ) const
  {
    return ( state() & contactPointState ) != 0;
  }
 
  AGX_FORCE_INLINE agx::Bool ContactPoint::isHolonomic() const
  {
    return !hasState( NONHOLONOMIC );
  }
 
  AGX_FORCE_INLINE void ContactPoint::setIsHolonomic( agx::Bool isHolonomic )
  {
    if ( isHolonomic )
    {
      // remove non-holonomic bit
      state() = (agx::UInt8)( state() & ~NONHOLONOMIC );
    }
    else
    {
      // add non-holonomic bit
      state() |= ( agx::UInt8 )( NONHOLONOMIC );
    }
  }
 
  AGX_FORCE_INLINE  GeometryContact::GeometryContact()
  {
 
  }
 
  AGX_FORCE_INLINE  GeometryContact::GeometryContact(agx::Physics::GeometryContactPtr contact) :
    agx::Physics::GeometryContactPtr(contact)
 
  {
  }
 
  AGX_FORCE_INLINE agxCollide::Geometry* GeometryContact::geometry( size_t ith )
  {
    agxAssert(ith < 2);
    if(ith == 0)
      return geometry1() ? geometry1().model() : nullptr;
    else
      return geometry2() ? geometry2().model() : nullptr;
  }
 
  AGX_FORCE_INLINE const agxCollide::Geometry* GeometryContact::geometry( size_t ith ) const
  {
    agxAssert(ith < 2);
    if(ith == 0)
      return geometry1() ? geometry1().model() : nullptr;
    else
      return geometry2() ? geometry2().model() : nullptr;
  }
 
  AGX_FORCE_INLINE GeometryPair GeometryContact::getGeometryPair()
  {
    return GeometryPair(geometry(0), geometry(1));
  }
 
  AGX_FORCE_INLINE void GeometryContact::setGeometry( size_t ith, Geometry* geom )
  {
    agxAssert(ith < 2 );
    if(ith == 0)
      geometry1() = geom->getEntity();
    else
      geometry2() = geom->getEntity();
  }
 
  AGX_FORCE_INLINE agx::Vec3f GeometryContact::calculateRelativeVelocity(size_t pointIndex) const
  {
    agx::Vec3 v1, v2;
    if (pointIndex > points().size() - 1)
      return agx::Vec3f();
 
    const agx::RigidBody* rb1 = rigidBody(0);
    const agx::RigidBody* rb2 = rigidBody(1);
 
    if (rb1 != nullptr)
      v1 = rb1->getVelocity() + (rb1->getAngularVelocity() ^ (points()[pointIndex].point() - rb1->getCmPosition()));
    if (rb2 != nullptr)
      v2 = rb2->getVelocity() + (rb2->getAngularVelocity() ^ (points()[pointIndex].point() - rb2->getCmPosition()));
 
    return static_cast<agx::Vec3f>(v1 - v2);
  }
 
 
  inline bool GeometryContact::calculateSurfaceVelocity(const agxCollide::Geometry* geometry1,
                                                        const agxCollide::Geometry* geometry2,
                                                        ContactPoint& p)
  {
    LocalContactPoint lcp;
    lcp.point() = p.point();
    bool hasSurfaceVelocity = calculateSurfaceVelocity(geometry1, geometry2, lcp);
    p.velocity() = lcp.velocity();
    return hasSurfaceVelocity;
  }
 
 
  AGX_FORCE_INLINE agx::RigidBody* GeometryContact::rigidBody( size_t ith )
  {
    agxAssert(ith < 2 );
    auto body = ith == 0u ? body1() : body2();
    return body ? body.model() : nullptr;
  }
 
  AGX_FORCE_INLINE const agx::RigidBody* GeometryContact::rigidBody( size_t ith ) const
  {
    agxAssert(ith < 2 );
    const auto body = ith == 0u ? body1() : body2();
    return body ? body.model() : nullptr;
  }
 
 
  AGX_FORCE_INLINE ContactPointVector& GeometryContact::points()
  {
    return reinterpret_cast<ContactPointVector&>(agx::Physics::GeometryContactPtr::points());
  }
 
  AGX_FORCE_INLINE const ContactPointVector& GeometryContact::points() const
  {
    return reinterpret_cast<const ContactPointVector&>(agx::Physics::GeometryContactPtr::points());
  }
 
  AGX_FORCE_INLINE void* GeometryContact::getCustomData()
  {
    return customData();
  }
 
  AGX_FORCE_INLINE const void* GeometryContact::getCustomData() const
  {
    return customData();
  }
 
  inline void GeometryContact::setCustomData( void* customData )
  {
    this->customData() = customData;
  }
 
 
  AGX_FORCE_INLINE void GeometryContact::markForRemoval( bool immediately )
  {
    this->immediately() = immediately;
    this->enabled() = false;
  }
 
  AGX_FORCE_INLINE void GeometryContact::setEnable(bool flag)
  {
    this->enabled() = flag;
  }
 
  AGX_FORCE_INLINE bool GeometryContact::isEnabled() const
  {
    return this->enabled();
  }
 
 
  AGX_FORCE_INLINE bool GeometryContact::shouldBeCalled() const
  {
    return !immediately();
  }
 
  AGX_FORCE_INLINE agx::ContactMaterial* GeometryContact::getMaterial()
  {
    if ( material() && material().isExplicit() )
      return material().model();
    return nullptr;
  }
 
  AGX_FORCE_INLINE const agx::ContactMaterial* GeometryContact::getMaterial() const
  {
    if ( material() && material().isExplicit() )
      return material().model();
    return nullptr;
  }
 
  AGX_FORCE_INLINE const agx::ContactMaterial* GeometryContact::getMaterial( bool ) const
  {
    return material() ? material().model() : nullptr;
  }
 
  AGX_FORCE_INLINE void GeometryContact::setMaterial( agx::ContactMaterial* material )
  {
    agxAssert( material != nullptr );
    if (material)
      this->material() = material->getEntity();
  }
 
  AGX_FORCE_INLINE void GeometryContact::setHasSurfaceVelocity( bool flag )
  {
    hasSurfaceVelocity() = flag;
  }
 
 
  AGX_FORCE_INLINE void GeometryContact::setHasInternalMaterial( bool flag )
  {
    hasInternalMaterial() = flag;
  }
 
  AGX_FORCE_INLINE bool GeometryContact::getHasInternalMaterial( ) const
  {
    return hasInternalMaterial();
  }
 
  AGX_FORCE_INLINE agx::UInt8 GeometryContact::state() const
  {
    if ( broadPhasePair() )
      return broadPhasePair().state();
 
    return GeometryContact::NO_CONTACT;
  }
 
  AGX_FORCE_INLINE void GeometryContact::setState( agx::UInt8 state )
  {
    agxAssert(broadPhasePair());
    broadPhasePair().state() = state;
  }
 
  AGX_FORCE_INLINE agx::UInt32& ContactPoint::faceIndex(size_t ith)
  {
    if (ith == 0) return faceIndex1(); else return faceIndex2();
  }
 
 
  AGX_FORCE_INLINE agx::UInt32 const& ContactPoint::faceIndex(size_t ith) const
  {
    if (ith == 0) return faceIndex1(); else return faceIndex2();
  }
 
 
  AGX_FORCE_INLINE agx::UInt8& ContactPoint::faceFeature(size_t ith)
  {
    if (ith == 0) return faceFeature1(); else return faceFeature2();
  }
 
 
  AGX_FORCE_INLINE agx::UInt8 const& ContactPoint::faceFeature(size_t ith) const
  {
    if (ith == 0) return faceFeature1(); else return faceFeature2();
  }
 
 
  AGX_FORCE_INLINE agx::Physics::Geometry::ShapePtr& ContactPoint::shape(size_t ith)
  {
    if (ith == 0) return shape1(); else return shape2();
  }
 
 
  AGX_FORCE_INLINE agx::Physics::Geometry::ShapePtr const& ContactPoint::shape(size_t ith) const
  {
    if (ith == 0) return shape1(); else return shape2();
  }
 
 
  AGX_FORCE_INLINE agx::Vec3 ContactPoint::getWitnessPoint(size_t ith) const
  {
    if (ith == 0)
      return point() - agx::Vec3(normal()) * (depth() * agx::Real(0.5));
    else
      return point() + agx::Vec3(normal()) * (depth() * agx::Real(0.5));
  }
 
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getYoungsModulus() const
  {
    return this->youngsModulus();
  }
 
 
 
  AGX_FORCE_INLINE void ContactPoint::setYoungsModulus( agx::Real youngsModulus )
  {
    this->youngsModulus() = youngsModulus;
  }
 
 
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getFrictionCoefficient(agx::ContactMaterial::FrictionDirection direction) const
  {
    size_t d = agx::clamp<size_t>(direction, 0u, 1u);
    return friction()[d];
  }
 
  AGX_FORCE_INLINE void ContactPoint::setFrictionCoefficient(agx::Real coefficient, agx::ContactMaterial::FrictionDirection direction)
  {
    coefficient = agx::clamp(coefficient, 0.0, agx::Infinity);
 
    if (direction > agx::ContactMaterial::SECONDARY_DIRECTION) {
      friction() = agx::Vec2(coefficient);
    }
    else {
      size_t d = agx::clamp<size_t>(direction, 0u, 1u);
      friction()[d] = coefficient;
    }
  }
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getRestitution() const
  {
    return restitution()[0];
  }
 
  AGX_FORCE_INLINE void ContactPoint::setRestitution(agx::Real restitution)
  {
    this->restitution()[0] = agx::clamp(restitution, 0.0, agx::Infinity);
  }
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getTangentialRestitution(agx::ContactMaterial::FrictionDirection direction)
  {
    size_t d = agx::clamp<size_t>(direction, 0u, 1u);
    return this->restitution()[1+d];
  }
 
  AGX_FORCE_INLINE void ContactPoint::setTangentialRestitution(agx::Real restitution, agx::ContactMaterial::FrictionDirection direction)
  {
    restitution = agx::clamp(restitution, 0.0, agx::Infinity);
 
    if (direction > agx::ContactMaterial::SECONDARY_DIRECTION) {
      this->restitution()[1] = restitution;
      this->restitution()[2] = restitution;
    }
    else {
      size_t d = agx::clamp<size_t>(direction, 0u, 1u);
      this->restitution()[1+d] = restitution;
    }
  }
 
  AGX_FORCE_INLINE agx::Real ContactPoint::getTangentialCohesion( agx::ContactMaterial::FrictionDirection direction ) const
  {
    size_t d = agx::clamp<size_t>( direction, 0u, 1u );
    return tangentialCohesion()[ d ];
  }
 
  AGX_FORCE_INLINE void ContactPoint::setTangentialCohesion( agx::Real tanCohesion, agx::ContactMaterial::FrictionDirection direction )
  {
    tanCohesion = agx::clamp( tanCohesion, 0.0, agx::Infinity );
 
    if ( direction > agx::ContactMaterial::SECONDARY_DIRECTION ) {
      tangentialCohesion() = agx::Vec2( tanCohesion );
    }
    else {
      size_t d = agx::clamp<size_t>( direction, 0u, 1u );
      tangentialCohesion()[ d ] = tanCohesion;
    }
  }
}
 
#ifdef _MSC_VER
# pragma warning(pop)
#endif