From: kgv Date: Wed, 22 Sep 2021 19:32:15 +0000 (+0300) Subject: 0032583: Visualization, AIS - add Bullet Physics wrapper X-Git-Url: http://git.dev.opencascade.org/gitweb/?a=commitdiff_plain;h=refs%2Fheads%2FCR32583;p=occt.git 0032583: Visualization, AIS - add Bullet Physics wrapper Added new package Dynamics and optional dependency CSF_BULLET. --- diff --git a/adm/UDLIST b/adm/UDLIST index 4cc783fdd6..5e7ec0998c 100644 --- a/adm/UDLIST +++ b/adm/UDLIST @@ -216,6 +216,7 @@ n SelectMgr n StdPrs n StdSelect n V3d +n Dynamics n Wasm n WNT n Xw diff --git a/adm/genconf.tcl b/adm/genconf.tcl index ff2444fa8c..f33b61678d 100644 --- a/adm/genconf.tcl +++ b/adm/genconf.tcl @@ -238,6 +238,15 @@ proc wokdep:gui:UpdateList {} { set aDummy {} wokdep:SearchStandardLibrary anIncErrs anLib32Errs anLib64Errs aDummy aDummy "draco" "draco/compression/decode.h" "draco" {"draco"} } + if { "$::HAVE_BULLET" == "true" } { + set aDummy {} + wokdep:SearchStandardLibrary anIncErrs anLib32Errs anLib64Errs aDummy aDummy "bullet" "bullet/btBulletDynamicsCommon.h" "BulletDynamics" {"bullet"} + set aHeaderPath1 [wokdep:SearchHeader "btBulletDynamicsCommon.h"] + set aHeaderPath2 [wokdep:SearchHeader "bullet/btBulletDynamicsCommon.h"] + if { "$aHeaderPath1" == "" && "$aHeaderPath2" != "" } { + lappend ::CSF_OPT_INC [file dirname "$aHeaderPath2"] + } + } if {"$::BUILD_Inspector" == "true" } { set ::CHECK_QT "true" @@ -465,6 +474,8 @@ checkbutton .myFrame.myChecks.myXLibCheck -offvalue "false" -onvalue "true ttk::label .myFrame.myChecks.myXLibLbl -text "Use X11 for windows drawing" ttk::label .myFrame.myChecks.myVtkLbl -text "Use VTK" checkbutton .myFrame.myChecks.myVtkCheck -offvalue "false" -onvalue "true" -variable HAVE_VTK -command wokdep:gui:UpdateList +ttk::label .myFrame.myChecks.myBulletLbl -text "Use Bullet" +checkbutton .myFrame.myChecks.myBulletCheck -offvalue "false" -onvalue "true" -variable HAVE_BULLET -command wokdep:gui:UpdateList checkbutton .myFrame.myChecks.myZLibCheck -offvalue "false" -onvalue "true" -variable HAVE_ZLIB -command wokdep:gui:UpdateList ttk::label .myFrame.myChecks.myZLibLbl -text "Use zlib" @@ -655,8 +666,8 @@ grid .myFrame.myChecks.myJDKLbl -row $aCheckRowIter -column 13 -sticky w incr aCheckRowIter grid .myFrame.myChecks.myFFmpegCheck -row $aCheckRowIter -column 0 -sticky e grid .myFrame.myChecks.myFFmpegLbl -row $aCheckRowIter -column 1 -sticky w -grid .myFrame.myChecks.myVtkCheck -row $aCheckRowIter -column 2 -sticky e -grid .myFrame.myChecks.myVtkLbl -row $aCheckRowIter -column 3 -sticky w +grid .myFrame.myChecks.myBulletCheck -row $aCheckRowIter -column 2 -sticky e +grid .myFrame.myChecks.myBulletLbl -row $aCheckRowIter -column 3 -sticky w grid .myFrame.myChecks.myOpenVrCheck -row $aCheckRowIter -column 4 -sticky e grid .myFrame.myChecks.myOpenVrLbl -row $aCheckRowIter -column 5 -sticky w grid .myFrame.myChecks.myE57Check -row $aCheckRowIter -column 6 -sticky e @@ -669,6 +680,8 @@ if { "$::tcl_platform(platform)" == "windows" } { incr aCheckRowIter +grid .myFrame.myChecks.myVtkCheck -row $aCheckRowIter -column 2 -sticky e +grid .myFrame.myChecks.myVtkLbl -row $aCheckRowIter -column 3 -sticky w grid .myFrame.myChecks.myTbbCheck -row $aCheckRowIter -column 12 -sticky e grid .myFrame.myChecks.myTbbLbl -row $aCheckRowIter -column 13 -sticky w diff --git a/adm/genconfdeps.tcl b/adm/genconfdeps.tcl index 189613f170..c9e309865a 100644 --- a/adm/genconfdeps.tcl +++ b/adm/genconfdeps.tcl @@ -73,7 +73,7 @@ if { [info exists ::env(SHORTCUT_HEADERS)] } { # fetch environment variables (e.g. set by custom.sh or custom.bat) and set them as tcl variables with the same name set THE_ENV_VARIABLES { HAVE_TK HAVE_FREETYPE HAVE_FREEIMAGE HAVE_FFMPEG HAVE_TBB HAVE_GLES2 HAVE_D3D HAVE_VTK \ - HAVE_ZLIB HAVE_LIBLZMA HAVE_E57 HAVE_RAPIDJSON HAVE_DRACO HAVE_OPENVR HAVE_OPENCL \ + HAVE_ZLIB HAVE_LIBLZMA HAVE_E57 HAVE_RAPIDJSON HAVE_DRACO HAVE_OPENVR HAVE_BULLET HAVE_OPENCL \ CHECK_QT4 CHECK_JDK HAVE_XLIB \ HAVE_RelWithDebInfo BUILD_Inspector } foreach anEnvIter $THE_ENV_VARIABLES { set ${anEnvIter} "false" } diff --git a/adm/genproj.tcl b/adm/genproj.tcl index ce70ef7b4f..64b7f500e9 100644 --- a/adm/genproj.tcl +++ b/adm/genproj.tcl @@ -1415,6 +1415,9 @@ proc osutils:csfList { theOS theCsfLibsMap theCsfFrmsMap theRelease} { if { "$::HAVE_FREETYPE" == "true" } { set aLibsMap(CSF_FREETYPE) "freetype" } + if { "$::HAVE_BULLET" == "true" && "$theOS" != "wnt" } { + set aLibsMap(CSF_BULLET) "BulletDynamics BulletCollision LinearMath" + } set aLibsMap(CSF_TclLibs) "tcl8.6" if { "$::HAVE_TK" == "true" } { set aLibsMap(CSF_TclTkLibs) "tk8.6" diff --git a/adm/templates/env.bat b/adm/templates/env.bat index a02bea526a..289caaa0b2 100644 --- a/adm/templates/env.bat +++ b/adm/templates/env.bat @@ -29,6 +29,7 @@ set "HAVE_LIBLZMA=false" set "HAVE_RAPIDJSON=false" set "HAVE_DRACO=false" set "HAVE_OPENVR=false" +set "HAVE_BULLET=false" set "HAVE_E57=false" set "CSF_OPT_INC=" set "CSF_OPT_LIB32=" @@ -205,6 +206,7 @@ if ["%HAVE_LIBLZMA%"] == ["true"] set "PRODUCTS_DEFINES=%PRODUCTS_DEFINES% -DH if ["%HAVE_RAPIDJSON%"] == ["true"] set "PRODUCTS_DEFINES=%PRODUCTS_DEFINES% -DHAVE_RAPIDJSON" & set "CSF_DEFINES=HAVE_RAPIDJSON;%CSF_DEFINES%" if ["%HAVE_DRACO%"] == ["true"] set "PRODUCTS_DEFINES=%PRODUCTS_DEFINES% -DHAVE_DRACO" & set "CSF_DEFINES=HAVE_DRACO;%CSF_DEFINES%" if ["%HAVE_OPENVR%"] == ["true"] set "PRODUCTS_DEFINES=%PRODUCTS_DEFINES% -DHAVE_OPENVR" & set "CSF_DEFINES=HAVE_OPENVR;%CSF_DEFINES%" +if ["%HAVE_BULLET%"] == ["true"] set "PRODUCTS_DEFINES=%PRODUCTS_DEFINES% -DHAVE_BULLET" & set "CSF_DEFINES=HAVE_BULLET;%CSF_DEFINES%" if ["%HAVE_E57%"] == ["true"] set "PRODUCTS_DEFINES=%PRODUCTS_DEFINES% -DHAVE_E57" & set "CSF_DEFINES=HAVE_E57;%CSF_DEFINES%" rem Eliminate VS warning diff --git a/adm/templates/env.sh b/adm/templates/env.sh index 4ab363e153..a55b313cc7 100644 --- a/adm/templates/env.sh +++ b/adm/templates/env.sh @@ -22,6 +22,7 @@ export HAVE_LIBLZMA="false"; export HAVE_RAPIDJSON="false"; export HAVE_DRACO="false"; export HAVE_OPENVR="false"; +export HAVE_BULLET="false"; export HAVE_E57="false"; export HAVE_XLIB="true"; if [ "$aSystem" == "Darwin" ]; then @@ -104,7 +105,7 @@ fi export CSF_OPT_CMPL="" -# Optiona 3rd-parties should be enabled by HAVE macros +# Optional 3rd-parties should be enabled by HAVE macros if [ "$HAVE_TBB" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_TBB"; fi if [ "$HAVE_OPENCL" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_OPENCL"; fi if [ "$HAVE_TK" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_TK"; fi @@ -118,6 +119,7 @@ if [ "$HAVE_LIBLZMA" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -D if [ "$HAVE_RAPIDJSON" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_RAPIDJSON"; fi if [ "$HAVE_DRACO" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_DRACO"; fi if [ "$HAVE_OPENVR" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_OPENVR"; fi +if [ "$HAVE_BULLET" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_BULLET"; fi if [ "$HAVE_E57" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_E57"; fi if [ "$HAVE_XLIB" == "true" ]; then export CSF_OPT_CMPL="${CSF_OPT_CMPL} -DHAVE_XLIB"; fi diff --git a/src/DRAWEXE/EXTERNLIB_STATIC b/src/DRAWEXE/EXTERNLIB_STATIC index f99e4d4449..dc6b959600 100644 --- a/src/DRAWEXE/EXTERNLIB_STATIC +++ b/src/DRAWEXE/EXTERNLIB_STATIC @@ -60,6 +60,7 @@ CSF_OpenGlesLibs CSF_FFmpeg CSF_FreeImagePlus CSF_FREETYPE +CSF_BULLET CSF_Draco CSF_user32 CSF_advapi32 diff --git a/src/Draw/Draw_BasicCommands.cxx b/src/Draw/Draw_BasicCommands.cxx index 0f8dd40da0..cea00608a6 100644 --- a/src/Draw/Draw_BasicCommands.cxx +++ b/src/Draw/Draw_BasicCommands.cxx @@ -394,6 +394,11 @@ static Standard_Integer dversion(Draw_Interpretor& di, Standard_Integer, const c #else di << "Draco disabled\n"; #endif +#ifdef HAVE_BULLET + di << "Bullet Physics enabled (HAVE_BULLET)\n"; +#else + di << "Bullet Physics disabled\n"; +#endif #ifdef HAVE_VTK di << "VTK enabled (HAVE_VTK)\n"; #else diff --git a/src/Dynamics/Dynamics_Character.cxx b/src/Dynamics/Dynamics_Character.cxx new file mode 100644 index 0000000000..3893f65214 --- /dev/null +++ b/src/Dynamics/Dynamics_Character.cxx @@ -0,0 +1,389 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include +#include +#include + +#include +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_Character, Dynamics_CollisionBody) + +namespace +{ + static const double THE_GRAVITY_G = 9.8; + +#ifdef HAVE_BULLET + static const btScalar THE_TIME_PRECISION = 0.001f; + static const double THE_DIST_PRECISION = 0.0001; + + //! Character controller. + ATTRIBUTE_ALIGNED16(class) Dynamics_CharacterController : public btKinematicCharacterController + { + public: + + enum StableState + { + StableState_Reset, + StableState_Stable, + StableState_Stuck, + }; + + public: + + //! Main constructor. + Dynamics_CharacterController (btPairCachingGhostObject* theGhostObject, + btConvexShape* theConvexShape, + btScalar theStepHeight, + const btVector3& theUp) + : btKinematicCharacterController (theGhostObject, theConvexShape, theStepHeight, theUp), + myOldDirection (0.0f, 0.0f, 0.0f), + myStableState (StableState_Reset) + { + // + } + + //! Setup distance tolerance. + void setDistancePrecision (btScalar theDistancePrec) + { + myDistancePrec = theDistancePrec; + } + + //! Action interface. + virtual void updateAction (btCollisionWorld* theCollisionWorld, + btScalar theDeltaTimeStep) Standard_OVERRIDE + { + const bool isUpdateNeeded = !m_useWalkDirection && m_velocityTimeInterval > THE_TIME_PRECISION && m_walkDirection.length() > myDistancePrec; + if (myStableState != StableState_Reset) + { + return; + } + + const btVector3 aLocBefore = m_ghostObject->getWorldTransform().getOrigin(); + btKinematicCharacterController::updateAction (theCollisionWorld, theDeltaTimeStep); + + const btVector3 aLocAfter = m_ghostObject->getWorldTransform().getOrigin(); + const btVector3 aDelta = aLocAfter - aLocBefore; + if (theDeltaTimeStep > THE_TIME_PRECISION) + { + if (aDelta.length() < myDistancePrec) + { + myStableState = isUpdateNeeded ? StableState_Stuck : StableState_Stable; + } + } + } + + //! Setup walking. + virtual void setVelocityForTimeInterval (const btVector3& theVelocity, btScalar theTimeInterval) Standard_OVERRIDE + { + if (theVelocity.length() > myDistancePrec) + { + const btVector3 aNewDirection = theVelocity.normalized(); + const btScalar aDelta = (myOldDirection - aNewDirection).length(); + myOldDirection = aNewDirection; + if (aDelta > myDistancePrec + || myStableState == StableState_Stable) + { + myStableState = StableState_Reset; + } + } + + btKinematicCharacterController::setVelocityForTimeInterval (btVector3 (0.0f, 0.0f, 0.0f), 0.0f); + btKinematicCharacterController::setVelocityForTimeInterval (theVelocity, theTimeInterval); + } + + //! Reset stable state. + void ResetStable() + { + myStableState = StableState_Reset; + btKinematicCharacterController::setVelocityForTimeInterval (btVector3 (0.0f, 0.0f, 0.0f), 0.0f); + } + + private: + + btVector3 myOldDirection; + btScalar myDistancePrec; + StableState myStableState; + + }; +#endif +} + +// ======================================================================= +// function : Dynamics_Character +// purpose : +// ======================================================================= +Dynamics_Character::Dynamics_Character (double theHumanHeight) +: myDefFallSpeed(0.0), + myFallSpeed (0.0), + myHumanHeight(1.0), + myHumanWidth (1.0), + myStepHeight (1.0), + myRunRatio (3.0), + myCrouchRatio(0.5), + myCapsHalfHeight(1.0), + myCapsMaxPenetr (0.1), // 0.2 is default + myState (Dynamics_CharacterState_Stand) +{ + Init (theHumanHeight); +} + +// ======================================================================= +// function : ~Dynamics_Character +// purpose : +// ======================================================================= +Dynamics_Character::~Dynamics_Character() +{ + // +} + +// ======================================================================= +// function : Init +// purpose : +// ======================================================================= +void Dynamics_Character::Init (double theHumanHeight, + double theHumanWidth, + double theStepHeight) +{ + myHumanHeight = theHumanHeight; + myHumanWidth = theHumanWidth; + myStepHeight = theStepHeight; + init(); +} + +// ======================================================================= +// function : init +// purpose : +// ======================================================================= +void Dynamics_Character::init() +{ + Dynamics_World* aWorld = myWorld; + const double aLenScale = aWorld != nullptr ? aWorld->LengthUnitScale() : 1.0; + + myVelocity.SetCoord (0.0, 0.0, 0.0); + myGravity = myDefGravity = gp_Vec (0.0, 0.0, -THE_GRAVITY_G * 3.0 * aLenScale); // 3G acceleration + myFallSpeed = myDefFallSpeed = 55.0 * aLenScale; +#ifdef HAVE_BULLET + double aHumanHeight = myHumanHeight; + double aHumanWidth = myHumanWidth; + double aStepHeight = myStepHeight; + if (myState == Dynamics_CharacterState_Crouch) + { + aHumanHeight = myHumanHeight * 0.5; + aStepHeight = myStepHeight * 0.5; + } + myCapsHalfHeight = aHumanHeight * 0.5; + + btTransform aWorldLoc; + aWorldLoc.setIdentity(); + if (myBody.get() != NULL) + { + aWorldLoc = myBody->getWorldTransform(); + } + + myCapsMaxPenetr = 0.1 * aLenScale; + if (aWorld != NULL) + { + removeFromWorld(); + } + + if (!myCustomShape.IsNull()) + { + const btConvexShape* aConvexShape = dynamic_cast (myCustomShape->CollisionShape()); + if (aConvexShape == NULL) + { + throw Standard_ProgramError ("Dynamics_Character::init() - non-convex shape is set"); + } + myShape = myCustomShape; + } + else + { + const double aCapsuleRadius = aHumanWidth * 0.5; + const double aCapsuleHeight = aHumanHeight - 2.0 * aCapsuleRadius; + + btCapsuleShapeZ* aCapsule = new btCapsuleShapeZ (btScalar(aCapsuleRadius), btScalar(aCapsuleHeight)); + myShape = new Dynamics_CollisionShape (aCapsule); + } + + btPairCachingGhostObject* aGhostObject = new btPairCachingGhostObject(); + aGhostObject->setWorldTransform (aWorldLoc); + aGhostObject->setCollisionShape (myShape->CollisionShape()); + aGhostObject->setCollisionFlags (btCollisionObject::CF_CHARACTER_OBJECT); + myBody.reset (aGhostObject); + btConvexShape* aConvexShape = dynamic_cast (myShape->CollisionShape()); + Dynamics_CharacterController* aCharacter = new Dynamics_CharacterController (aGhostObject, aConvexShape, btScalar (aStepHeight), btVector3 (0.0f, 0.0f, 1.0f)); + aCharacter->setDistancePrecision (btScalar(THE_DIST_PRECISION * aLenScale)); + aCharacter->setMaxPenetrationDepth (btScalar(myCapsMaxPenetr)); + aCharacter->setFallSpeed (btScalar(55.0 * aLenScale)); // terminal velocity of a sky diver in m/s + aCharacter->setJumpSpeed (btScalar(10.0 * aLenScale)); + myAction.reset (aCharacter); + SetGravity (myDefGravity); + SetFallSpeed (myDefFallSpeed); + if (aWorld != NULL) + { + addToWorld (aWorld); + } +#endif +} + +// ======================================================================= +// function : addToWorld +// purpose : +// ======================================================================= +void Dynamics_Character::SetCustomConvexShape (const Handle(Dynamics_CollisionShape)& theShape) +{ + myCustomShape = theShape; + if (!myCustomShape.IsNull()) + { + #ifdef HAVE_BULLET + const btConvexShape* aConvexShape = dynamic_cast (myCustomShape->CollisionShape()); + if (aConvexShape == NULL) + { + throw Standard_ProgramError ("Dynamics_Character::SetCustomConvexShape() - non-convex shape is set"); + } + #endif + } + init(); +} + +// ======================================================================= +// function : addToWorld +// purpose : +// ======================================================================= +void Dynamics_Character::addToWorld (Dynamics_World* theWorld) +{ + removeFromWorld(); +#ifdef HAVE_BULLET + myWorld = theWorld; + if (myWorld != NULL && myBody.get() != NULL) + { + btDiscreteDynamicsWorld* aWorld = myWorld->myDynamicsWorld.get(); + aWorld->addCollisionObject (myBody.get(), btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); + aWorld->addAction (myAction.get()); + } +#else + (void )theWorld; +#endif +} + +// ======================================================================= +// function : SetEyePosition +// purpose : +// ======================================================================= +void Dynamics_Character::SetEyePosition (const gp_Pnt& theEyePosition, + const Bnd_Box& theSceneBndBox) +{ + myVelocity.SetCoord (0.0, 0.0, 0.0); + const double aShift = myCapsHalfHeight + myCapsMaxPenetr * 0.5 - myHumanHeight * ProportionalHumanEyeShift(); + gp_XYZ aPos (theEyePosition.XYZ()); + aPos.SetZ (theEyePosition.Z() - aShift); + if (!theSceneBndBox.IsVoid()) + { + aPos.SetZ (Max (aPos.Z(), theSceneBndBox.CornerMin().Z())); + } + SetWorldPosition (aPos); +} + +// ======================================================================= +// function : SetWorldPosition +// purpose : +// ======================================================================= +void Dynamics_Character::SetWorldPosition (const gp_Pnt& thePosition) +{ + myVelocity.SetCoord (0.0, 0.0, 0.0); +#ifdef HAVE_BULLET + Dynamics_CharacterController* aCharacter = dynamic_cast(myAction.get()); + aCharacter->ResetStable(); + + btPairCachingGhostObject* aGhostObject = dynamic_cast(myBody.get()); + btTransform aStartTrsf; + aStartTrsf.setIdentity(); + aStartTrsf.setOrigin (btVector3 (btScalar (thePosition.X()), btScalar(thePosition.Y()), btScalar(thePosition.Z()))); + aGhostObject->setWorldTransform (aStartTrsf); +#else + (void )thePosition; +#endif +} + +// ======================================================================= +// function : SetGravity +// purpose : +// ======================================================================= +void Dynamics_Character::SetGravity (const gp_Vec& theGravity) +{ + myGravity = theGravity; +#ifdef HAVE_BULLET + Dynamics_CharacterController* aCharacter = dynamic_cast(myAction.get()); + aCharacter->setGravity (btVector3 (btScalar(theGravity.X()), btScalar(theGravity.Y()), btScalar(theGravity.Z()))); +#endif +} + +// ======================================================================= +// function : SetFallSpeed +// purpose : +// ======================================================================= +void Dynamics_Character::SetFallSpeed (double theSpeed) +{ + myFallSpeed = theSpeed; +#ifdef HAVE_BULLET + Dynamics_CharacterController* aCharacter = dynamic_cast(myAction.get()); + aCharacter->setFallSpeed (btScalar(theSpeed)); +#endif +} + +// ======================================================================= +// function : Walk +// purpose : +// ======================================================================= +void Dynamics_Character::Walk (const gp_Vec& theMoveVec) +{ + myVelocity.SetCoord (0.0, 0.0, 0.0); +#ifdef HAVE_BULLET + Dynamics_CharacterController* aCharacter = dynamic_cast(myAction.get()); + aCharacter->setWalkDirection (btVector3 (btScalar(theMoveVec.X()), btScalar(theMoveVec.Y()), btScalar(theMoveVec.Z()))); +#else + (void )theMoveVec; +#endif +} + +// ======================================================================= +// function : Walk +// purpose : +// ======================================================================= +void Dynamics_Character::Walk (const gp_Vec& theSpeedVec, + const double theDurationSec) +{ + myVelocity = theSpeedVec; +#ifdef HAVE_BULLET + Dynamics_CharacterController* aCharacter = dynamic_cast(myAction.get()); + aCharacter->setVelocityForTimeInterval (btVector3 (btScalar(theSpeedVec.X()), btScalar(theSpeedVec.Y()), btScalar(theSpeedVec.Z())), btScalar(theDurationSec)); +#else + (void )theDurationSec; +#endif +} + +// ======================================================================= +// function : Jump +// purpose : +// ======================================================================= +void Dynamics_Character::Jump() +{ +#ifdef HAVE_BULLET + Dynamics_CharacterController* aCharacter = dynamic_cast(myAction.get()); + aCharacter->ResetStable(); + aCharacter->jump (btVector3 (0, 0, 0)); +#endif +} diff --git a/src/Dynamics/Dynamics_Character.hxx b/src/Dynamics/Dynamics_Character.hxx new file mode 100644 index 0000000000..83ed6b8e52 --- /dev/null +++ b/src/Dynamics/Dynamics_Character.hxx @@ -0,0 +1,218 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_Character_HeaderFile +#define _Dynamics_Character_HeaderFile + +#include +#include + +class Bnd_Box; +class gp_Vec; + +//! Class defining character object. +class Dynamics_Character : public Dynamics_CollisionBody +{ + friend class Dynamics_World; + DEFINE_STANDARD_RTTIEXT(Dynamics_Character, Dynamics_CollisionBody) +public: + + //! Return the average human height. + static double AverageHumanHeight() { return 1.75; } + + //! Compute human width from the human height using average proportions. + static double ProportionalHumanWidth (double theHumanHeight) { return theHumanHeight * 0.265; } + + //! Compute human step height from the human height using average proportions. + static double ProportionalHumanStepHeight (double theHumanHeight) { return theHumanHeight * 0.2; } + + //! Return the average human eye level shift (EyeLevel = Height - EyeShift), ~1/16. + static double ProportionalHumanEyeShift() { return 0.0625; } + +public: + + //! Main constructor. + Standard_EXPORT Dynamics_Character (double theHumanHeight = 1.75); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_Character(); + + //! Initialize the character using average proportions for specified human height. + void Init (double theHumanHeight) + { + Init (theHumanHeight, ProportionalHumanWidth (theHumanHeight), ProportionalHumanStepHeight (theHumanHeight)); + } + + //! Initialize the character. + Standard_EXPORT void Init (double theHumanHeight, + double theHumanWidth, + double theStepHeight); + + //! Setup custom convex shape instead of capsule. + Standard_EXPORT void SetCustomConvexShape (const Handle(Dynamics_CollisionShape)& theShape); + + //! Wrap to specified position. + Standard_EXPORT void SetEyePosition (const gp_Pnt& theEyePosition, + const Bnd_Box& theSceneBndBox); + + //! Return the eye location. + gp_Pnt EyePosition() const + { + const double aShift = myCapsHalfHeight + myCapsMaxPenetr * 0.5 - myHumanHeight * ProportionalHumanEyeShift(); + const gp_Pnt aPos = WorldPosition(); + return gp_Pnt (aPos.X(), aPos.Y(), aPos.Z() + aShift); + } + + //! Wrap to specified position. + Standard_EXPORT void SetWorldPosition (const gp_Pnt& thePosition); + + //! Return the ground location. + gp_Pnt GroundPosition() const + { + const double aShift = -myCapsHalfHeight + myCapsMaxPenetr * 0.5; + const gp_Pnt aPos = WorldPosition(); + return gp_Pnt (aPos.X(), aPos.Y(), aPos.Z() + aShift); + } + + //! Return the default gravity vector. + const gp_Vec& DefaultGravity() const { return myDefGravity; } + + //! Return the gravity vector. + const gp_Vec& Gravity() const { return myGravity; } + + //! Setup new gravity. + Standard_EXPORT void SetGravity (const gp_Vec& theGravity); + + //! Return the default fall speed limit. + double DefaultFallSpeed() const { return myDefFallSpeed; } + + //! Return the fall speed limit. + double FallSpeed() const { return myFallSpeed; } + + //! Setup fall speed limit. + Standard_EXPORT void SetFallSpeed (double theSpeed); + + //! Walk in specified direction and delta. + Standard_EXPORT void Walk (const gp_Vec& theMoveVec); + + //! Walk in specified direction and delta. + Standard_EXPORT void Walk (const gp_Vec& theSpeedVec, + const double theDurationSec); + + //! Jump impulse. + Standard_EXPORT void Jump(); + + //! Return current state. + Dynamics_CharacterState State() const { return myState; } + + //! Return current state. + void SetState (Dynamics_CharacterState theState) + { + if (myState == theState) + { + return; + } + myState = theState; + init(); + } + + //! Return character height. + double EyeLevel() const + { + double aHumanHeight = myHumanHeight; + if (myState == Dynamics_CharacterState_Crouch) + { + aHumanHeight = myHumanHeight * 0.5; + } + return aHumanHeight - myHumanHeight * ProportionalHumanEyeShift(); + } + + //! Return character height. + double Height() const { return myHumanHeight; } + + //! Return character width. + double Width() const { return myHumanWidth; } + + //! Return character step height. + double StepHeight() const { return myStepHeight; } + + //! Running speed ratio (relative to normal walk), 3 by default. + double RunRatio() const { return myRunRatio; } + + //! Setup running speed ratio. + void SetRunRatio (double theRatio) { myRunRatio = theRatio; } + + //! Crouching speed ratio (relative to normal walk), 0.5 by default. + double CrouchRatio() const { return myCrouchRatio; } + + //! Crouching running speed ratio. + void SetCrouchRatio (double theRatio) { myCrouchRatio = theRatio; } + + //! Return walking velocity. + const gp_Vec& Velocity() const { return myVelocity; } + + //! Return capsule half height (offset from the local CS). + double CapsuleHalfHeight() const { return myCapsHalfHeight; } + + //! Return allowed penetration. + double CapsuleMaxPenetration() const { return myCapsMaxPenetr; } + + //! Return TRUE if character is added to the World. + bool IsActive() const { return myWorld != nullptr; } + + //! Add/remove character to/from the World. + void SetActive (Dynamics_World* theWorld) + { + if (myWorld == theWorld) + { + return; + } + if (theWorld != nullptr) + { + addToWorld (theWorld); + init(); + } + else + { + removeFromWorld(); + } + } + +protected: + + //! Add body to the world. + Standard_EXPORT virtual void addToWorld (Dynamics_World* theWorld) Standard_OVERRIDE; + + //! Initialize the character. + Standard_EXPORT void init(); + +protected: + + Handle(Dynamics_CollisionShape) myCustomShape; + gp_Vec myVelocity; + gp_Vec myDefGravity; //!< default gravity + gp_Vec myGravity; //!< current gravity + Standard_Real myDefFallSpeed; //!< default maximum fall speed + Standard_Real myFallSpeed; //!< maximum fall speed + Standard_Real myHumanHeight; //!< character height + Standard_Real myHumanWidth; //!< character width + Standard_Real myStepHeight; //!< step height + Standard_Real myRunRatio; //!< running speed ratio (relative to normal walk) + Standard_Real myCrouchRatio; //!< crouching speed ratio (relative to normal walk) + Standard_Real myCapsHalfHeight; //!< capsule half height (offset from the local CS) + Standard_Real myCapsMaxPenetr; //!< allowed penetration + Dynamics_CharacterState myState; //!< current character state + +}; + +#endif // _Dynamics_Character_HeaderFile diff --git a/src/Dynamics/Dynamics_CharacterState.hxx b/src/Dynamics/Dynamics_CharacterState.hxx new file mode 100644 index 0000000000..b9c99bb7d9 --- /dev/null +++ b/src/Dynamics/Dynamics_CharacterState.hxx @@ -0,0 +1,24 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CharacterState_HeaderFile +#define _Dynamics_CharacterState_HeaderFile + +//! Character state. +enum Dynamics_CharacterState +{ + Dynamics_CharacterState_Stand, + Dynamics_CharacterState_Crouch, +}; + +#endif // _Dynamics_CharacterState_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionBRepFlags.hxx b/src/Dynamics/Dynamics_CollisionBRepFlags.hxx new file mode 100644 index 0000000000..7443ac6fc2 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBRepFlags.hxx @@ -0,0 +1,30 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionBRepFlags_HeaderFile +#define _Dynamics_CollisionBRepFlags_HeaderFile + +//! Dynamics_CollisionBRepShape initialization parameters. +enum Dynamics_CollisionBRepFlags +{ + Dynamics_CollisionBRepFlags_NONE = 0x00, + Dynamics_CollisionBRepFlags_Convex = 0x02, + Dynamics_CollisionBRepFlags_Single = 0x04, + Dynamics_CollisionBRepFlags_Compressed = 0x08, + Dynamics_CollisionBRepFlags_BuildBVH = 0x10, + + Dynamics_CollisionBRepFlags_DEFAULTS = Dynamics_CollisionBRepFlags_Compressed | Dynamics_CollisionBRepFlags_BuildBVH, + Dynamics_CollisionBRepFlags_CONVEX_DEFAULTS = Dynamics_CollisionBRepFlags_DEFAULTS | Dynamics_CollisionBRepFlags_Single | Dynamics_CollisionBRepFlags_Convex, +}; + +#endif // _Dynamics_CollisionBRepFlags_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionBRepShape.cxx b/src/Dynamics/Dynamics_CollisionBRepShape.cxx new file mode 100644 index 0000000000..3437096fd3 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBRepShape.cxx @@ -0,0 +1,237 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include +#include + +#ifdef HAVE_BULLET + #include + #include + #include +#endif + +#include +#include +#include +#include +#include +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionBRepShape, Dynamics_CollisionShape) +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionBRepShapeConvex, Dynamics_CollisionBRepShape) + +namespace +{ +#ifdef HAVE_BULLET + //! Wrapper over btCompoundShape with destructor deleting child shapes. + ATTRIBUTE_ALIGNED16(class) Dynamics_CompoundShape : public btCompoundShape + { + public: + //! Constructor. + Dynamics_CompoundShape (bool theToEnableDynamicAabbTree = true, const int theInitialChildCapacity = 0) + : btCompoundShape (theToEnableDynamicAabbTree, theInitialChildCapacity) {} + + //! Destructor. + virtual ~Dynamics_CompoundShape() + { + for (int aChildIter = 0; aChildIter < getNumChildShapes(); ++aChildIter) + { + btCollisionShape* aShape = getChildShape (aChildIter); + delete aShape; + } + } + }; + + //! Wrapper over btBvhTriangleMeshShape with destructor deleting Mesh Interface. + ATTRIBUTE_ALIGNED16(class) Dynamics_BvhTriangleMeshShape : public btBvhTriangleMeshShape + { + public: + //! Constructor. + Dynamics_BvhTriangleMeshShape (btStridingMeshInterface* theMeshInterface, + bool theToUseQuantizedAabbCompression, + bool theToBuildBvh = true) + : btBvhTriangleMeshShape (theMeshInterface, theToUseQuantizedAabbCompression, theToBuildBvh) {} + + //! Destructor. + virtual ~Dynamics_BvhTriangleMeshShape() + { + delete m_meshInterface; + m_meshInterface = NULL; + } + }; + + //! Wrapper over btConvexTriangleMeshShape with destructor deleting Mesh Interface. + ATTRIBUTE_ALIGNED16(class) Dynamics_ConvexTriangleMeshShape : public btConvexTriangleMeshShape + { + public: + //! Constructor. + Dynamics_ConvexTriangleMeshShape (btStridingMeshInterface* theMeshInterface, + bool theToBuildBvh = true) + : btConvexTriangleMeshShape (theMeshInterface, theToBuildBvh) {} + + //! Destructor. + virtual ~Dynamics_ConvexTriangleMeshShape() + { + btStridingMeshInterface* aMeshInterface = getMeshInterface(); + delete aMeshInterface; + } + }; + + //! Wrap triangulation into mesh interface. + btStridingMeshInterface* wrapTriangulation (const Handle(Poly_Triangulation)& thePolyTri) + { + if (!thePolyTri.IsNull()) + { + return new Dynamics_PolyTriangulation (thePolyTri); + } + return NULL; + } +#endif +} + +// ======================================================================= +// function : Init +// purpose : +// ======================================================================= +bool Dynamics_CollisionBRepShape::Init (const TopoDS_Shape& theShape, + Dynamics_CollisionBRepFlags theFlags) +{ + releaseShape(); + if (theShape.IsNull()) + { + return false; + } + +#ifdef HAVE_BULLET + const bool toUseCompression = (theFlags & Dynamics_CollisionBRepFlags_Compressed) != 0; + const bool toBuildBvh = (theFlags & Dynamics_CollisionBRepFlags_BuildBVH) != 0; + const bool isConvex = (theFlags & Dynamics_CollisionBRepFlags_Convex) != 0; + + TopLoc_Location aLoc; + btTransform aTrsfBtLocal; + aTrsfBtLocal.setIdentity(); + if ((theFlags & Dynamics_CollisionBRepFlags_Single) != 0) + { + int aNbNodes = 0, aNbTris = 0; + for (TopExp_Explorer aFaceIter (theShape, TopAbs_FACE); aFaceIter.More(); aFaceIter.Next()) + { + const TopoDS_Face& aFace = TopoDS::Face (aFaceIter.Current()); + const Handle(Poly_Triangulation)& aPolyTri = BRep_Tool::Triangulation (aFace, aLoc); + if (!aPolyTri.IsNull()) + { + aNbNodes += aPolyTri->NbNodes(); + aNbTris += aPolyTri->NbTriangles(); + } + } + if (aNbNodes < 3 + || aNbTris < 1) + { + return false; + } + + btConvexHullShape aConvexHullShape; + aConvexHullShape.setMargin (0); // this is to compensate for a bug in bullet + + Handle(Graphic3d_ArrayOfTriangles) aTriangles; + if (!isConvex) + { + aTriangles = new Graphic3d_ArrayOfTriangles (aNbNodes, aNbTris * 3, false, false, false); + } + for (TopExp_Explorer aFaceIter (theShape, TopAbs_FACE); aFaceIter.More(); aFaceIter.Next()) + { + const TopoDS_Face& aFace = TopoDS::Face (aFaceIter.Current()); + const Handle(Poly_Triangulation)& aPolyTri = BRep_Tool::Triangulation (aFace, aLoc); + const gp_Trsf aTrsf = aLoc; + if (!aPolyTri.IsNull()) + { + const int aFirstNode = !aTriangles.IsNull() ? aTriangles->VertexNumber() : 0; + for (int aNodeIter = 1; aNodeIter <= aPolyTri->NbNodes(); ++aNodeIter) + { + gp_Pnt aNode = aPolyTri->Node (aNodeIter); + aNode.Transform (aTrsf); + if (!aTriangles.IsNull()) + { + aTriangles->AddVertex (aNode); + } + else + { + aConvexHullShape.addPoint (btVector3 (btScalar (aNode.X()), btScalar (aNode.Y()), btScalar (aNode.Z())), false); + } + } + if (!aTriangles.IsNull()) + { + Graphic3d_Vec4i anElem (-1, -1, -1, -1); + for (int aTriIter = 1; aTriIter <= aPolyTri->NbTriangles(); ++aTriIter) + { + aPolyTri->Triangle (aTriIter).Get (anElem[0], anElem[1], anElem[2]); + aTriangles->AddEdges (aFirstNode + anElem[0], aFirstNode + anElem[1], aFirstNode + anElem[2]); + } + } + } + } + + if (isConvex) + { + // create a hull approximation + //btConvexHullShape aConvexHullShape ((const btScalar* )aTriangles->Attributes()->Data(), aNbNodes, aTriangles->Attributes()->Stride); + aConvexHullShape.recalcLocalAabb(); + + btShapeHull aHull (&aConvexHullShape); + aHull.buildHull (0); + if (aHull.numVertices() == 0) + { + Message::SendTrace ("Dynamics_CollisionBRepShape, btShapeHull returned an empty result"); + return false; + } + myShape = new btConvexHullShape ((const btScalar* )aHull.getVertexPointer(), aHull.numVertices(), sizeof(btVector3)); + } + else + { + btStridingMeshInterface* aMeshInterface = new Dynamics_Graphic3dTriangulation (aTriangles); + myShape = new Dynamics_BvhTriangleMeshShape (aMeshInterface, toUseCompression, toBuildBvh); + } + return true; + } + + Dynamics_CompoundShape* aCompShape = new Dynamics_CompoundShape(); + myShape = aCompShape; + for (TopExp_Explorer aFaceIter (theShape, TopAbs_FACE); aFaceIter.More(); aFaceIter.Next()) + { + const TopoDS_Face& aFace = TopoDS::Face (aFaceIter.Current()); + const Handle(Poly_Triangulation)& aPolyTri = BRep_Tool::Triangulation (aFace, aLoc); + if (btStridingMeshInterface* aChild = wrapTriangulation (aPolyTri)) + { + Dynamics_MotionState::TrsfBtFromGp (aTrsfBtLocal, aLoc.Transformation()); + if (isConvex) + { + aCompShape->addChildShape (aTrsfBtLocal, new Dynamics_ConvexTriangleMeshShape (aChild, toBuildBvh)); + } + else + { + aCompShape->addChildShape (aTrsfBtLocal, new Dynamics_BvhTriangleMeshShape (aChild, toUseCompression, toBuildBvh)); + } + } + } + if (aCompShape->getNumChildShapes() == 0) + { + releaseShape(); + return false; + } + return true; +#else + (void )theFlags; + return false; +#endif +} diff --git a/src/Dynamics/Dynamics_CollisionBRepShape.hxx b/src/Dynamics/Dynamics_CollisionBRepShape.hxx new file mode 100644 index 0000000000..b117cb77e9 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBRepShape.hxx @@ -0,0 +1,41 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionBRepShape_HeaderFile +#define _Dynamics_CollisionBRepShape_HeaderFile + +#include +#include + +class TopoDS_Shape; + +//! Define the collision body from triangulation data. +class Dynamics_CollisionBRepShape : public Dynamics_CollisionShape +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionBRepShape, Dynamics_CollisionShape) +public: + + //! Constructor. + Dynamics_CollisionBRepShape (const TopoDS_Shape& theShape, + Dynamics_CollisionBRepFlags theFlags = Dynamics_CollisionBRepFlags_DEFAULTS) + { + Init (theShape, theFlags); + } + + //! Initialize from triangulation. + Standard_EXPORT bool Init (const TopoDS_Shape& theShape, + Dynamics_CollisionBRepFlags theFlags); + +}; + +#endif // _Dynamics_CollisionBRepShape_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionBRepShapeConvex.hxx b/src/Dynamics/Dynamics_CollisionBRepShapeConvex.hxx new file mode 100644 index 0000000000..b5bd75f1ab --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBRepShapeConvex.hxx @@ -0,0 +1,38 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionBRepShapeConvex_HeaderFile +#define _Dynamics_CollisionBRepShapeConvex_HeaderFile + +#include + +//! Define the collision body from triangulation data (convex). +class Dynamics_CollisionBRepShapeConvex : public Dynamics_CollisionBRepShape +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionBRepShapeConvex, Dynamics_CollisionBRepShape) +public: + + //! Main constructor. + Dynamics_CollisionBRepShapeConvex (const TopoDS_Shape& theShape, + const gp_Trsf& theTrsf, + Dynamics_CollisionBRepFlags theFlags = Dynamics_CollisionBRepFlags_CONVEX_DEFAULTS) + : Dynamics_CollisionBRepShape (theShape, theFlags), myLocation (theTrsf) {} + + //! Return location. + virtual const gp_Trsf& Location() const Standard_OVERRIDE { return myLocation; } + +protected: + gp_Trsf myLocation; +}; + +#endif // _Dynamics_CollisionBRepShapeConvex_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionBody.cxx b/src/Dynamics/Dynamics_CollisionBody.cxx new file mode 100644 index 0000000000..d90b6e2cbc --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBody.cxx @@ -0,0 +1,453 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include +#include +#include +#include +#include + +namespace +{ +#ifdef HAVE_BULLET + static btVector3 gpXyz2Vec3Bt (const gp_XYZ& theXYZ) + { + return btVector3 (btScalar(theXYZ.X()), btScalar(theXYZ.Y()), btScalar(theXYZ.Z())); + } +#endif +} + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionBody, Standard_Transient) + +// ======================================================================= +// function : Dynamics_CollisionBody +// purpose : +// ======================================================================= +Dynamics_CollisionBody::Dynamics_CollisionBody() +: myWorld (NULL) +{ + // +} + +// ======================================================================= +// function : Dynamics_CollisionBody +// purpose : +// ======================================================================= +Dynamics_CollisionBody::Dynamics_CollisionBody (const Handle(Dynamics_CollisionShape)& theShape) +: myWorld(NULL) +{ + SetShape (theShape); +} + +// ======================================================================= +// function : ~Dynamics_CollisionBody +// purpose : +// ======================================================================= +Dynamics_CollisionBody::~Dynamics_CollisionBody() +{ + clearMain(); +} + +// ======================================================================= +// function : SetShape +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::SetShape (const Handle(Dynamics_CollisionShape)& theShape, + const gp_Trsf& theTrsf) +{ + myShape = theShape; + myMotionState.Nullify(); +#ifdef HAVE_BULLET + + // btCollisionObject cannot hold NULL collision shape (no NULL check in Bullet), + // so remove object from the world, by keep reference for another ::SetShape() call + bool toRemoveOld = myShape.IsNull(), toAdd = false; + if (btRigidBody* aRigidBody = myBody.get() != NULL ? btRigidBody::upcast (myBody.get()) : NULL) + { + (void )aRigidBody; + toRemoveOld = true; + } + if (toRemoveOld) + { + if (!myConstraint.IsNull()) + { + if (myConstraint->DynamicsWorld() != nullptr) + { + myConstraint->DynamicsWorld()->RemoveConstraint (myConstraint); + } + myConstraint.Nullify(); + } + + Dynamics_World* aWorld = myWorld; + removeFromWorld(); + myWorld = aWorld; + myBody.reset(); + } + if (myShape.IsNull()) + { + return; + } + + if (myBody.get() == NULL) + { + toAdd = true; + myBody.reset (new btCollisionObject()); + myBody->setCollisionFlags (myBody->getCollisionFlags() | btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT); + } + + btTransform aTrsfBt; + Dynamics_MotionState::TrsfBtFromGp (aTrsfBt, theTrsf); + + myBody->setCollisionShape (myShape->CollisionShape()); + myBody->setWorldTransform (aTrsfBt); + if (toAdd && myWorld != NULL) + { + addToWorld (myWorld); + } +#else + (void )theTrsf; +#endif +} + +// ======================================================================= +// function : SetShape +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::SetShape (const Handle(Dynamics_CollisionShape)& theShape, + const Handle(Dynamics_MotionState)& theMotionState, + const float theMass) +{ + myShape = theShape; + myMotionState = theMotionState; +#ifdef HAVE_BULLET + bool toAdd = false, toRemoveOld = true; + if (toRemoveOld) + { + if (!myConstraint.IsNull()) + { + if (myConstraint->DynamicsWorld() != nullptr) + { + myConstraint->DynamicsWorld()->RemoveConstraint (myConstraint); + } + myConstraint.Nullify(); + } + + Dynamics_World* aWorld = myWorld; + removeFromWorld(); + myWorld = aWorld; + myBody.reset(); + } + + if (myBody.get() == NULL) + { + toAdd = true; + btVector3 aLocalInertia (0.0f, 0.0f, 0.0f); + myShape->CollisionShape()->calculateLocalInertia (theMass, aLocalInertia); + + btRigidBody::btRigidBodyConstructionInfo aBodyInfo (theMass, myMotionState->MotionState(), myShape->CollisionShape(), aLocalInertia); + myBody.reset (new btRigidBody (aBodyInfo)); + + + myBody->setFriction (1.0f); + myBody->setRollingFriction (0.1f); + myBody->setSpinningFriction (0.1f); + myBody->setAnisotropicFriction (myShape->CollisionShape()->getAnisotropicRollingFrictionDirection(), + btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + } + + if (toAdd && myWorld != NULL) + { + addToWorld (myWorld); + } +#else + (void )theMass; +#endif +} + +// ======================================================================= +// function : addToWorld +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::addToWorld (Dynamics_World* theWorld) +{ + removeFromWorld(); +#ifdef HAVE_BULLET + myWorld = theWorld; + if (myBody.get() == NULL || myWorld == NULL || myShape.IsNull()) + { + return; + } + + btDiscreteDynamicsWorld* aWorld = myWorld->myDynamicsWorld.get(); + if (btRigidBody* aRigidBody = btRigidBody::upcast (myBody.get())) + { + ++myWorld->myNbRigidBodies; + aWorld->addRigidBody (aRigidBody); + } + else + { + aWorld->addCollisionObject (myBody.get()); + } + if (myAction.get() != NULL) + { + aWorld->addAction (myAction.get()); + } +#else + (void )theWorld; +#endif +} + +// ======================================================================= +// function : removeFromWorld +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::removeFromWorld() +{ +#ifdef HAVE_BULLET + if (myWorld != NULL && myBody.get() != NULL) + { + btDiscreteDynamicsWorld* aWorld = myWorld->myDynamicsWorld.get(); + if (myAction.get() != NULL) + { + aWorld->removeAction (myAction.get()); + } + if (btRigidBody* aRigidBody = btRigidBody::upcast (myBody.get())) + { + aWorld->removeRigidBody (aRigidBody); + --myWorld->myNbRigidBodies; + } + else + { + aWorld->removeCollisionObject (myBody.get()); + } + } + myWorld = NULL; +#endif +} + +// ======================================================================= +// function : clearMain +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::clearMain() +{ + removeFromWorld(); +#ifdef HAVE_BULLET + myAction.reset(); + myBody.reset(); + myMotionState.Nullify(); +#endif +} + +// ======================================================================= +// function : WorldPosition +// purpose : +// ======================================================================= +gp_Pnt Dynamics_CollisionBody::WorldPosition() const +{ +#ifdef HAVE_BULLET + if (myBody.get() == NULL) + { + return gp_Pnt (0.0, 0.0, 0.0); + } + + const btTransform aTrsfBt = myBody->getWorldTransform(); + return gp_Pnt (aTrsfBt.getOrigin().x(), aTrsfBt.getOrigin().y(), aTrsfBt.getOrigin().z()); +#else + return gp_Pnt (0.0, 0.0, 0.0); +#endif +} + +// ======================================================================= +// function : WorldLocation +// purpose : +// ======================================================================= +gp_Trsf Dynamics_CollisionBody::WorldLocation() const +{ + gp_Trsf aTrsf; +#ifdef HAVE_BULLET + if (myBody.get() == NULL) + { + return aTrsf; + } + + const btTransform aTrsfBt = myBody->getWorldTransform(); + Dynamics_MotionState::TrsfGpFromBt (aTrsf, aTrsfBt); +#endif + return aTrsf; +} + +// ======================================================================= +// function : SetWorldLocation +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::SetWorldLocation (const gp_Trsf& theLoc) +{ +#ifdef HAVE_BULLET + if (myBody.get() != NULL) + { + btTransform aTrsfBt; + Dynamics_MotionState::TrsfBtFromGp (aTrsfBt, theLoc); + myBody->setWorldTransform (aTrsfBt); + } +#else + (void )theLoc; +#endif +} + +// ======================================================================= +// function : WorldMotionLocation +// purpose : +// ======================================================================= +gp_Trsf Dynamics_CollisionBody::WorldMotionLocation() const +{ + gp_Trsf aTrsf; +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return WorldLocation(); + } + + btTransform aTrsfBt; + aBody->getMotionState()->getWorldTransform (aTrsfBt); + Dynamics_MotionState::TrsfGpFromBt (aTrsf, aTrsfBt); +#endif + return aTrsf; +} + +// ======================================================================= +// function : ApplyCentralForce +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::ApplyCentralForce (const gp_XYZ& theForce) +{ +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return; + } + + aBody->activate (true); + aBody->applyCentralForce (gpXyz2Vec3Bt (theForce)); +#else + (void )theForce; +#endif +} + +// ======================================================================= +// function : ApplyCentralImpulse +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::ApplyCentralImpulse (const gp_XYZ& theImpulse) +{ +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return; + } + + aBody->activate (true); + aBody->applyCentralImpulse (gpXyz2Vec3Bt (theImpulse)); +#else + (void )theImpulse; +#endif +} + +// ======================================================================= +// function : ApplyTorque +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::ApplyTorque (const gp_XYZ& theTorque) +{ +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return; + } + + aBody->activate (true); + aBody->applyTorque (gpXyz2Vec3Bt (theTorque)); +#else + (void )theTorque; +#endif +} + +// ======================================================================= +// function : ApplyForce +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::ApplyForce (const gp_XYZ& theForce, + const gp_Pnt& theRelPos) +{ +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return; + } + + aBody->activate (true); + aBody->applyForce (gpXyz2Vec3Bt (theForce), gpXyz2Vec3Bt (theRelPos.XYZ())); +#else + (void )theForce; + (void )theRelPos; +#endif +} + +// ======================================================================= +// function : ApplyImpulse +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::ApplyImpulse (const gp_XYZ& theImpulse, + const gp_Pnt& theRelPos) +{ +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return; + } + + aBody->activate (true); + aBody->applyImpulse (gpXyz2Vec3Bt (theImpulse), gpXyz2Vec3Bt (theRelPos.XYZ())); +#else + (void )theImpulse; + (void )theRelPos; +#endif +} + +// ======================================================================= +// function : ApplyTorqueImpulse +// purpose : +// ======================================================================= +void Dynamics_CollisionBody::ApplyTorqueImpulse (const gp_XYZ& theTorque) +{ +#ifdef HAVE_BULLET + btRigidBody* aBody = btRigidBody::upcast (myBody.get()); + if (aBody == NULL || aBody->getMotionState() == NULL) + { + return; + } + + aBody->activate (true); + aBody->applyTorqueImpulse (gpXyz2Vec3Bt (theTorque)); +#else + (void )theTorque; +#endif +} diff --git a/src/Dynamics/Dynamics_CollisionBody.hxx b/src/Dynamics/Dynamics_CollisionBody.hxx new file mode 100644 index 0000000000..de6723cc9f --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBody.hxx @@ -0,0 +1,128 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionBody_HeaderFile +#define _Dynamics_CollisionBody_HeaderFile + +#include +#include +#include + +#include + +class Dynamics_CollisionShape; +class Dynamics_Constraint; +class Dynamics_MotionState; +class Dynamics_World; + +class btActionInterface; +class btCollisionObject; +class btCollisionShape; +class btTransform; + +//! Class defining collision body. +class Dynamics_CollisionBody : public Standard_Transient +{ + friend class Dynamics_Constraint; + friend class Dynamics_World; + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionBody, Standard_Transient) +public: + + //! Empty constructor. + Standard_EXPORT Dynamics_CollisionBody(); + + //! Constructor. + Standard_EXPORT Dynamics_CollisionBody (const Handle(Dynamics_CollisionShape)& theShape); + + //! Destructor, automatically removes the body from the world. + Standard_EXPORT virtual ~Dynamics_CollisionBody(); + + //! Return the world. + Dynamics_World* DynamicsWorld() { return myWorld; } + + //! Return the shape. + const Handle(Dynamics_CollisionShape)& Shape() const { return myShape; } + + //! Assign new shape. + Standard_EXPORT void SetShape (const Handle(Dynamics_CollisionShape)& theShape, + const gp_Trsf& theTrsf = gp_Trsf()); + + //! Assign new shape. + Standard_EXPORT void SetShape (const Handle(Dynamics_CollisionShape)& theShape, + const Handle(Dynamics_MotionState)& theMotionState, + const float theMass); + + //! Return motion state. + const Handle(Dynamics_MotionState)& MotionState() const { return myMotionState; } + + //! Return constraint. + const Handle(Dynamics_Constraint)& Constraint() const { return myConstraint; } + + //! Return the position (origin) in the world. + Standard_EXPORT gp_Pnt WorldPosition() const; + + //! Return the location in the world. + Standard_EXPORT gp_Trsf WorldLocation() const; + + //! Return the location in the world. + Standard_EXPORT void SetWorldLocation (const gp_Trsf& theLoc); + + //! Return the location in the world. + Standard_EXPORT gp_Trsf WorldMotionLocation() const; + + //! Alias for btRigidBody::applyCentralImpulse(). + Standard_EXPORT void ApplyCentralForce (const gp_XYZ& theForce); + + //! Alias for btRigidBody::applyCentralImpulse(). + Standard_EXPORT void ApplyCentralImpulse(const gp_XYZ& theImpulse); + + //! Alias for btRigidBody::applyForce(). + Standard_EXPORT void ApplyForce (const gp_XYZ& theForce, const gp_Pnt& theRelPos); + + //! Alias for btRigidBody::applyTorque(). + Standard_EXPORT void ApplyTorque (const gp_XYZ& theTorque); + + //! Alias for btRigidBody::applyImpulse(). + Standard_EXPORT void ApplyImpulse(const gp_XYZ& theImpulse, const gp_Pnt& theRelPos); + + //! Alias for btRigidBody::applyTorqueImpulse(). + Standard_EXPORT void ApplyTorqueImpulse (const gp_XYZ& theTorque); + +public: + + //! Return collision body. + btCollisionObject* collisionBody() { return myBody.get(); } + +protected: + + //! Add body to the world. + Standard_EXPORT virtual void addToWorld (Dynamics_World* theWorld); + + //! Remove body from the world. + Standard_EXPORT virtual void removeFromWorld(); + + //! Release the memory. + Standard_EXPORT void clearMain(); + +protected: + + Handle(Dynamics_CollisionShape) myShape; + Handle(Dynamics_MotionState) myMotionState; + Handle(Dynamics_Constraint) myConstraint; + std::shared_ptr myAction; + std::shared_ptr myBody; + Dynamics_World* myWorld; + +}; + +#endif // _Dynamics_CollisionBody_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionBoxShape.cxx b/src/Dynamics/Dynamics_CollisionBoxShape.cxx new file mode 100644 index 0000000000..52b4a47e3a --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBoxShape.cxx @@ -0,0 +1,51 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionBoxShape, Dynamics_CollisionShape) + +// ======================================================================= +// function : Dynamics_CollisionBoxShape +// purpose : +// ======================================================================= +Dynamics_CollisionBoxShape::Dynamics_CollisionBoxShape (const Bnd_OBB& theOBB) +{ + if (!theOBB.IsVoid()) + { + #ifdef HAVE_BULLET + myShape = new btBoxShape (btVector3 (btScalar(theOBB.XHSize()), btScalar(theOBB.YHSize()), btScalar(theOBB.ZHSize()))); + myLocation.SetTransformation (gp_Ax3 (theOBB.Center(), theOBB.ZDirection(), theOBB.XDirection()), gp::XOY()); + #endif + } +} + +// ======================================================================= +// function : Dynamics_CollisionBoxShape +// purpose : +// ======================================================================= +Dynamics_CollisionBoxShape::Dynamics_CollisionBoxShape (const Bnd_Box& theAABB) +{ + if (!theAABB.IsVoid()) + { + #ifdef HAVE_BULLET + const gp_XYZ aSize = theAABB.CornerMax().XYZ() - theAABB.CornerMin().XYZ(); + myShape = new btBoxShape (btVector3 (btScalar(aSize.X() * 0.5), btScalar(aSize.Y() * 0.5), btScalar(aSize.Z() * 0.5))); + myLocation.SetTranslation ((theAABB.CornerMin().XYZ() + theAABB.CornerMax().XYZ()) * 0.5); + #endif + } +} diff --git a/src/Dynamics/Dynamics_CollisionBoxShape.hxx b/src/Dynamics/Dynamics_CollisionBoxShape.hxx new file mode 100644 index 0000000000..f5b2581b79 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionBoxShape.hxx @@ -0,0 +1,40 @@ +// Copyright (c) 2018-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionBoxShape_HeaderFile +#define _Dynamics_CollisionBoxShape_HeaderFile + +#include + +#include + +//! Define the collision shape as OBB. +class Dynamics_CollisionBoxShape : public Dynamics_CollisionShape +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionBoxShape, Dynamics_CollisionShape) +public: + + //! Constructor from OBB. + Standard_EXPORT Dynamics_CollisionBoxShape (const Bnd_OBB& theOBB); + + //! Constructor from AABB. + Standard_EXPORT Dynamics_CollisionBoxShape (const Bnd_Box& theAABB); + + //! Return shape location. + virtual const gp_Trsf& Location() const Standard_OVERRIDE { return myLocation; } + +protected: + gp_Trsf myLocation; +}; + +#endif // _Dynamics_CollisionBoxShape_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionCompoundShape.cxx b/src/Dynamics/Dynamics_CollisionCompoundShape.cxx new file mode 100644 index 0000000000..cd941a0e01 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionCompoundShape.cxx @@ -0,0 +1,77 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionCompoundShape, Dynamics_CollisionShape) + +// ======================================================================= +// function : Dynamics_CollisionCompoundShape +// purpose : +// ======================================================================= +Dynamics_CollisionCompoundShape::Dynamics_CollisionCompoundShape() +{ + // +} + +// ======================================================================= +// function : ~Dynamics_CollisionCompoundShape +// purpose : +// ======================================================================= +Dynamics_CollisionCompoundShape::~Dynamics_CollisionCompoundShape() +{ + // +} + +// ======================================================================= +// function : Init +// purpose : +// ======================================================================= +bool Dynamics_CollisionCompoundShape::Init() +{ + releaseShape(); + if (mySubShapes.IsEmpty()) + { + return false; + } + +#ifdef HAVE_BULLET + btTransform aTrsfBt; + aTrsfBt.setIdentity(); + + btCompoundShape* aCompShape = new btCompoundShape(); + myShape = aCompShape; + for (int aChildIter = mySubShapes.Lower(); aChildIter <= mySubShapes.Upper(); ++aChildIter) + { + const Handle(Dynamics_CollisionShape)& aShape = mySubShapes.Value (aChildIter); + if (!aShape.IsNull() + && aShape->IsValid()) + { + const gp_Trsf& aTrsf = aShape->Location(); + Dynamics_MotionState::TrsfBtFromGp (aTrsfBt, aTrsf); + aCompShape->addChildShape (aTrsfBt, aShape->myShape); + } + } + if (aCompShape->getNumChildShapes() == 0) + { + releaseShape(); + return false; + } + return true; +#else + return false; +#endif +} diff --git a/src/Dynamics/Dynamics_CollisionCompoundShape.hxx b/src/Dynamics/Dynamics_CollisionCompoundShape.hxx new file mode 100644 index 0000000000..cbc9496bb9 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionCompoundShape.hxx @@ -0,0 +1,45 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionCompoundShape_HeaderFile +#define _Dynamics_CollisionCompoundShape_HeaderFile + +#include + +#include + +//! Define the collision body from triangulation data. +class Dynamics_CollisionCompoundShape : public Dynamics_CollisionShape +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionCompoundShape, Dynamics_CollisionShape) +public: + + //! Constructor. + Standard_EXPORT Dynamics_CollisionCompoundShape(); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_CollisionCompoundShape(); + + //! Modify sub-shapes. + NCollection_Array1& ChangeSubShapes() { return mySubShapes; } + + //! Initialize the collision shape after modification of sub-shape + Standard_EXPORT bool Init(); + +protected: + + NCollection_Array1 mySubShapes; + +}; + +#endif // _Dynamics_CollisionCompoundShape_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionPlane.cxx b/src/Dynamics/Dynamics_CollisionPlane.cxx new file mode 100644 index 0000000000..6dec00b2c4 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionPlane.cxx @@ -0,0 +1,41 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionPlane, Dynamics_CollisionShape) + +// ======================================================================= +// function : InitPlane +// purpose : +// ======================================================================= +void Dynamics_CollisionPlane::InitPlane (const gp_Pln& thePlane) +{ + releaseShape(); +#ifdef HAVE_BULLET + // ABCD does not work as expected for unknown reason + //NCollection_Vec4 aCoef; + //thePlane.Coefficients (aCoef.x(), aCoef.y(), aCoef.z(), aCoef.w()); + //myShape = new btStaticPlaneShape (btVector3 (btScalar(aCoef.x()), btScalar(aCoef.y()), btScalar(aCoef.z())), btScalar(aCoef.w())); + myShape = new btStaticPlaneShape (btVector3 (btScalar(thePlane.Axis().Direction().X()), + btScalar(thePlane.Axis().Direction().Y()), + btScalar(thePlane.Axis().Direction().Z())), + btScalar(0.0)); +#else + (void )thePlane; +#endif +} diff --git a/src/Dynamics/Dynamics_CollisionPlane.hxx b/src/Dynamics/Dynamics_CollisionPlane.hxx new file mode 100644 index 0000000000..ad89aaaa88 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionPlane.hxx @@ -0,0 +1,38 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionPlane_HeaderFile +#define _Dynamics_CollisionPlane_HeaderFile + +#include + +class gp_Pln; + +//! Infinite collision plane. +class Dynamics_CollisionPlane : public Dynamics_CollisionShape +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionPlane, Dynamics_CollisionShape) +public: + + //! Main constructor. + Dynamics_CollisionPlane (const gp_Pln& thePlane) + { + InitPlane (thePlane); + } + + //! Initialize the collision plane. + Standard_EXPORT void InitPlane (const gp_Pln& thePlane); + +}; + +#endif // _Dynamics_CollisionPlane_HeaderFile diff --git a/src/Dynamics/Dynamics_CollisionShape.cxx b/src/Dynamics/Dynamics_CollisionShape.cxx new file mode 100644 index 0000000000..650714683a --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionShape.cxx @@ -0,0 +1,59 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_CollisionShape, Standard_Transient) + +// ======================================================================= +// function : Dynamics_CollisionShape +// purpose : +// ======================================================================= +Dynamics_CollisionShape::Dynamics_CollisionShape() +: myShape (NULL) +{ + // +} + +// ======================================================================= +// function : Dynamics_CollisionShape +// purpose : +// ======================================================================= +Dynamics_CollisionShape::Dynamics_CollisionShape (btCollisionShape* theShape) +: myShape (theShape) +{ + // +} + +// ======================================================================= +// function : ~Dynamics_CollisionShape +// purpose : +// ======================================================================= +Dynamics_CollisionShape::~Dynamics_CollisionShape() +{ + releaseShape(); +} + +// ======================================================================= +// function : releaseShape +// purpose : +// ======================================================================= +void Dynamics_CollisionShape::releaseShape() +{ +#ifdef HAVE_BULLET + delete myShape; +#endif + myShape = NULL; +} diff --git a/src/Dynamics/Dynamics_CollisionShape.hxx b/src/Dynamics/Dynamics_CollisionShape.hxx new file mode 100644 index 0000000000..04aefbf959 --- /dev/null +++ b/src/Dynamics/Dynamics_CollisionShape.hxx @@ -0,0 +1,65 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_CollisionShape_HeaderFile +#define _Dynamics_CollisionShape_HeaderFile + +#include +#include +#include + +class btCollisionShape; + +//! Define the collision shape. +class Dynamics_CollisionShape : public Standard_Transient +{ + friend class Dynamics_CollisionBody; + friend class Dynamics_CollisionCompoundShape; + DEFINE_STANDARD_RTTIEXT(Dynamics_CollisionShape, Standard_Transient) +public: + + //! Empty constructor. + Standard_EXPORT Dynamics_CollisionShape(); + + //! Constructor. + Standard_EXPORT Dynamics_CollisionShape (btCollisionShape* theShape); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_CollisionShape(); + + //! Return true if shape object is defined. + bool IsValid() const { return myShape != NULL; } + + //! Return collision shape pointer. + btCollisionShape* CollisionShape() const { return myShape; } + + //! Return shape location. + //! Default implementation always returns an identity location, + //! because it is expected that location is put into Motion State instead. + //! This method can be overridden for storing an initial location. + virtual const gp_Trsf& Location() const + { + static const gp_Trsf THE_IDENT_LOC; + return THE_IDENT_LOC; + } + +protected: + + //! Release memory. + Standard_EXPORT void releaseShape(); + +protected: + btCollisionShape* myShape; +}; + +#endif // _Dynamics_CollisionShape_HeaderFile diff --git a/src/Dynamics/Dynamics_Constraint.cxx b/src/Dynamics/Dynamics_Constraint.cxx new file mode 100644 index 0000000000..2061ddc70c --- /dev/null +++ b/src/Dynamics/Dynamics_Constraint.cxx @@ -0,0 +1,221 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include +#include +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_Constraint, Standard_Transient) + +// ======================================================================= +// function : Dynamics_Constraint +// purpose : +// ======================================================================= +Dynamics_Constraint::Dynamics_Constraint() +: myConstraint (nullptr) +{ + // +} + +// ======================================================================= +// function : ~Dynamics_Constraint +// purpose : +// ======================================================================= +Dynamics_Constraint::~Dynamics_Constraint() +{ + releaseConstraint(); +} + +// ======================================================================= +// function : releaseConstraint +// purpose : +// ======================================================================= +void Dynamics_Constraint::releaseConstraint() +{ + removeFromWorld(); +#ifdef HAVE_BULLET + delete myConstraint; +#endif + myConstraint = nullptr; +} + +// ======================================================================= +// function : addToWorld +// purpose : +// ======================================================================= +void Dynamics_Constraint::addToWorld (Dynamics_World* theWorld) +{ + removeFromWorld(); +#ifdef HAVE_BULLET + myWorld = theWorld; + if (myConstraint == nullptr || theWorld == nullptr) + { + return; + } + + btDiscreteDynamicsWorld* aWorld = theWorld->myDynamicsWorld.get(); + aWorld->addConstraint (myConstraint); +#else + (void )theWorld; +#endif +} + +// ======================================================================= +// function : removeFromWorld +// purpose : +// ======================================================================= +void Dynamics_Constraint::removeFromWorld() +{ +#ifdef HAVE_BULLET + if (myWorld != nullptr && myConstraint != nullptr) + { + btDiscreteDynamicsWorld* aWorld = myWorld->myDynamicsWorld.get(); + aWorld->removeConstraint (myConstraint); + } + myWorld = nullptr; +#endif +} + +// ======================================================================= +// function : CreateGeneric6Dof +// purpose : +// ======================================================================= +void Dynamics_Constraint::CreateGeneric6Dof (const Handle(Dynamics_CollisionBody)& theBody, + const gp_XYZ& theLinearLower, + const gp_XYZ& theLinearUpper, + const gp_XYZ& theAngularLower, + const gp_XYZ& theAngularUpper) +{ + Dynamics_World* aWorld = myWorld; + releaseConstraint(); + if (theBody.IsNull()) + { + throw Standard_ProgramError ("Dynamics_Constraint, Internal Error - NULL Rigid Body"); + } + +#ifdef HAVE_BULLET + btRigidBody* aRigidBody = btRigidBody::upcast (theBody->collisionBody()); + if (aRigidBody == nullptr) + { + throw Standard_ProgramError ("Dynamics_Constraint, Internal Error - no Rigid Body"); + } + + const btTransform aFrameInBody = btTransform::getIdentity(); + myConstraint = new btGeneric6DofConstraint (*aRigidBody, aFrameInBody, false); + myConstraint->setDbgDrawSize (5.0f); + SetLinearLimits (theLinearLower, theLinearUpper); + SetAngularLimits(theAngularLower, theAngularUpper); + theBody->myConstraint = this; + if (aWorld != nullptr) + { + addToWorld (aWorld); + } + else if (theBody->myWorld != nullptr) + { + theBody->myWorld->AddConstraint (this); + //addToWorld (theBody->myWorld); + } +#else + (void )aWorld; + (void )theLinearLower; + (void )theLinearUpper; + (void )theAngularLower; + (void )theAngularUpper; +#endif +} + +// ======================================================================= +// function : LinearLimits +// purpose : +// ======================================================================= +bool Dynamics_Constraint::LinearLimits (gp_XYZ& theLower, gp_XYZ& theUpper) const +{ +#ifdef HAVE_BULLET + if (const btGeneric6DofConstraint* aConstr = dynamic_cast(myConstraint)) + { + btVector3 aLower, anUpper; + aConstr->getLinearLowerLimit (aLower); + aConstr->getLinearUpperLimit (anUpper); + theLower.SetCoord (aLower.x(), aLower.y(), aLower.z()); + theUpper.SetCoord (anUpper.x(), anUpper.y(), anUpper.z()); + return true; + } +#endif + theLower.SetCoord ( 1.0, 1.0, 1.0); + theUpper.SetCoord (-1.0, -1.0, -1.0); + return false; +} + +// ======================================================================= +// function : SetLinearLimits +// purpose : +// ======================================================================= +void Dynamics_Constraint::SetLinearLimits (const gp_XYZ& theLower, const gp_XYZ& theUpper) +{ +#ifdef HAVE_BULLET + if (btGeneric6DofConstraint* aConstr = dynamic_cast(myConstraint)) + { + aConstr->setLinearLowerLimit (btVector3 ((btScalar )theLower.X(), (btScalar )theLower.Y(), (btScalar )theLower.Z())); + aConstr->setLinearUpperLimit (btVector3 ((btScalar )theUpper.X(), (btScalar )theUpper.Y(), (btScalar )theUpper.Z())); + return; + } +#else + (void )theLower; + (void )theUpper; +#endif + throw Standard_ProgramError ("Dynamics_Constraint::SetLinearLimits(), invalid constraint type"); +} + +// ======================================================================= +// function : AngularLimits +// purpose : +// ======================================================================= +bool Dynamics_Constraint::AngularLimits (gp_XYZ& theLower, gp_XYZ& theUpper) const +{ +#ifdef HAVE_BULLET + if (const btGeneric6DofConstraint* aConstr = dynamic_cast(myConstraint)) + { + btVector3 aLower, anUpper; + aConstr->getAngularLowerLimit (aLower); + aConstr->getAngularUpperLimit (anUpper); + theLower.SetCoord (aLower.x(), aLower.y(), aLower.z()); + theUpper.SetCoord (anUpper.x(), anUpper.y(), anUpper.z()); + return true; + } +#endif + theLower.SetCoord ( 1.0, 1.0, 1.0); + theUpper.SetCoord (-1.0, -1.0, -1.0); + return false; +} + +// ======================================================================= +// function : SetAngularLimits +// purpose : +// ======================================================================= +void Dynamics_Constraint::SetAngularLimits (const gp_XYZ& theLower, const gp_XYZ& theUpper) +{ +#ifdef HAVE_BULLET + if (btGeneric6DofConstraint* aConstr = dynamic_cast(myConstraint)) + { + aConstr->setAngularLowerLimit (btVector3 ((btScalar )theLower.X(), (btScalar )theLower.Y(), (btScalar )theLower.Z())); + aConstr->setAngularUpperLimit (btVector3 ((btScalar )theUpper.X(), (btScalar )theUpper.Y(), (btScalar )theUpper.Z())); + return; + } +#else + (void )theLower; + (void )theUpper; +#endif + throw Standard_ProgramError ("Dynamics_Constraint::SetAngularLimits(), invalid constraint type"); +} diff --git a/src/Dynamics/Dynamics_Constraint.hxx b/src/Dynamics/Dynamics_Constraint.hxx new file mode 100644 index 0000000000..4fc92770e5 --- /dev/null +++ b/src/Dynamics/Dynamics_Constraint.hxx @@ -0,0 +1,85 @@ +// Copyright (c) 2018-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_Constraint_HeaderFile +#define _Dynamics_Constraint_HeaderFile + +#include +#include +#include + +class btTypedConstraint; +class Dynamics_CollisionBody; +class Dynamics_World; + +//! Define the motion state. +class Dynamics_Constraint : public Standard_Transient +{ + friend class Dynamics_World; + DEFINE_STANDARD_RTTIEXT(Dynamics_Constraint, Standard_Transient) +public: + + //! Empty constructor. + Standard_EXPORT Dynamics_Constraint(); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_Constraint(); + + //! Return true if Constraint is defined. + bool IsValid() const { return myConstraint != NULL; } + + //! Return Constraint pointer. + btTypedConstraint* TypedConstraint() const { return myConstraint; } + + Standard_EXPORT void CreateGeneric6Dof (const Handle(Dynamics_CollisionBody)& theBody, + const gp_XYZ& theLinearLower, + const gp_XYZ& theLinearUpper, + const gp_XYZ& theAngularLower, + const gp_XYZ& theAngularUpper); + + //! Return the world. + Dynamics_World* DynamicsWorld() { return myWorld; } + + //! Return linear limits per axis. + //! Upper value below Lower means that there is NO limit. + Standard_EXPORT bool LinearLimits (gp_XYZ& theLower, gp_XYZ& theUpper) const; + + //! Set linear limits per axis (in radians). + Standard_EXPORT void SetLinearLimits (const gp_XYZ& theLower, const gp_XYZ& theUpper); + + //! Return angular limits per axis (in radians). + //! Upper value below Lower means that there is NO limit. + Standard_EXPORT bool AngularLimits(gp_XYZ& theLower, gp_XYZ& theUpper) const; + + //! Set angular limits per axis (in radians). + Standard_EXPORT void SetAngularLimits (const gp_XYZ& theLower, const gp_XYZ& theUpper); + +protected: + + //! Release memory. + Standard_EXPORT void releaseConstraint(); + + //! Add body to the world. + Standard_EXPORT void addToWorld (Dynamics_World* theWorld); + + //! Remove body from the world. + Standard_EXPORT void removeFromWorld(); + +protected: + + Dynamics_World* myWorld; + btTypedConstraint* myConstraint; + +}; + +#endif // _Dynamics_Constraint_HeaderFile diff --git a/src/Dynamics/Dynamics_DebugDrawer.cxx b/src/Dynamics/Dynamics_DebugDrawer.cxx new file mode 100644 index 0000000000..0bd2796fee --- /dev/null +++ b/src/Dynamics/Dynamics_DebugDrawer.cxx @@ -0,0 +1,345 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifdef HAVE_BULLET + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + //! Convert vec3 into vec4ub color. + Graphic3d_Vec4ub colorBt2vec4ub (const btVector3& theColor) + { + return Graphic3d_Vec4ub ((unsigned char)(int)(theColor.x() * 255.0f), + (unsigned char)(int)(theColor.y() * 255.0f), + (unsigned char)(int)(theColor.z() * 255.0f), + 255); + } +} + +//! Auxiliary presentation of line segments and points. +class Dynamics_DebugDrawer::Dynamics_DebugPrs : public AIS_InteractiveObject +{ + DEFINE_STANDARD_RTTI_INLINE(Dynamics_DebugPrs, AIS_InteractiveObject) +public: + //! Empty constructor. + Dynamics_DebugPrs() + { + SetMutable (true); + SetInfiniteState (true); + mySegments = new ColoredNodeBuffer(); + myPoints = new ColoredNodeBuffer(); + myDrawer->SetLineAspect (new Prs3d_LineAspect (Quantity_NOC_RED, Aspect_TOL_SOLID, 1.0f)); + myDrawer->SetPointAspect(new Prs3d_PointAspect(Aspect_TOM_PLUS, Quantity_NOC_RED, 1.0f)); + } + + //! Colored node. + struct ColoredNode + { + Graphic3d_Vec3 Position; + Graphic3d_Vec4ub Color; + }; + + //! Return vector of colored segments. + std::vector& ChangeSegments() { return mySegments->Nodes; } + + //! Return vector of colored points. + std::vector& ChangePoints() { return myPoints->Nodes; } + + //! Clear previous data. + void Clear() + { + if (!mySegments->Nodes.empty() + || !myPoints->Nodes.empty()) + { + mySegments->Nodes.clear(); + myPoints ->Nodes.clear(); + //SetToUpdate(); + } + } + + //! Setup view bounding box to be used instead of min/max range of points in this presentation. + //! This would allow avoiding issues with infinite presentations passed from Bullet (e.g. for btStaticPlaneShape). + void SetViewBox (const Bnd_Box& theBox) { myViewBox = theBox; } + +protected: + + //! Return TRUE for supported display modes. + virtual Standard_Boolean AcceptDisplayMode (const Standard_Integer theMode) const Standard_OVERRIDE { return theMode == 0; } + + //! Compute presentation. + virtual void Compute (const Handle(PrsMgr_PresentationManager)& , + const Handle(Prs3d_Presentation)& thePrs, + const Standard_Integer theMode) Standard_OVERRIDE + { + thePrs->SetInfiniteState (true); + if (theMode != 0) + { + return; + } + + Graphic3d_Vec3d aMinMax[2]; + bool toCalcMinMax = myViewBox.IsVoid(); + if (!toCalcMinMax) + { + aMinMax[0].SetValues (myViewBox.CornerMin().X(), myViewBox.CornerMin().Y(), myViewBox.CornerMin().Z()); + aMinMax[1].SetValues (myViewBox.CornerMax().X(), myViewBox.CornerMax().Y(), myViewBox.CornerMax().Z()); + } + + if (mySegments->Init()) + { + const Handle(Graphic3d_Group)& aGroup = thePrs->NewGroup(); + aGroup->SetGroupPrimitivesAspect (myDrawer->LineAspect()->Aspect()); + aGroup->AddPrimitiveArray (Graphic3d_TOPA_SEGMENTS, Handle(Graphic3d_IndexBuffer)(), mySegments, Handle(Graphic3d_BoundBuffer)(), toCalcMinMax); + if (!toCalcMinMax) + { + aGroup->SetMinMaxValues (aMinMax[0].x(), aMinMax[0].y(), aMinMax[0].z(), + aMinMax[1].x(), aMinMax[1].y(), aMinMax[1].z()); + } + } + if (myPoints->Init()) + { + const Handle(Graphic3d_Group)& aGroup = thePrs->NewGroup(); + aGroup->SetGroupPrimitivesAspect (myDrawer->PointAspect()->Aspect()); + aGroup->AddPrimitiveArray (Graphic3d_TOPA_POINTS, Handle(Graphic3d_IndexBuffer)(), myPoints, Handle(Graphic3d_BoundBuffer)(), toCalcMinMax); + if (!toCalcMinMax) + { + aGroup->SetMinMaxValues (aMinMax[0].x(), aMinMax[0].y(), aMinMax[0].z(), + aMinMax[1].x(), aMinMax[1].y(), aMinMax[1].z()); + } + } + } + + //! Empty selection builder. + virtual void ComputeSelection (const Handle(SelectMgr_Selection)& theSel, const Standard_Integer theMode) Standard_OVERRIDE + { + (void )theSel; + (void )theMode; + } + +protected: + + //! Buffer holding colored nodes. + //! This implementation wraps std::vector array, so that NCollection_Buffer wraps external memory and initialized with NULL allocator. + class ColoredNodeBuffer : public Graphic3d_Buffer + { + public: + //! Data array. + std::vector Nodes; + + //! Empty constructor; passes a NULL allocator. + ColoredNodeBuffer() : Graphic3d_Buffer (Handle(NCollection_BaseAllocator)()) {} + + //! Wraps buffer from std::vector. + bool Init() + { + release(); + if (Nodes.empty()) + { + return false; + } + + // Graphic3d_Buffer defines attributes definition at the end of memory buffer, + // so that we are extending std::vector capacity to cover this memory block. + const size_t aReserve = (sizeof(Nodes[0]) + sizeof(Graphic3d_Attribute) - 1) / sizeof(Graphic3d_Attribute); + Nodes.reserve (Nodes.size() + aReserve); + + static const Graphic3d_Attribute anAttribs[2] = + { + { Graphic3d_TOA_POS, Graphic3d_TOD_VEC3 }, + { Graphic3d_TOA_COLOR, Graphic3d_TOD_VEC4UB } + }; + + NbAttributes = 2; + Stride = anAttribs[0].Stride() + anAttribs[1].Stride(); + NbElements = (int )Nodes.size(); + myData = (Standard_Byte* )&Nodes[0]; + mySize = size_t(Stride) * size_t(NbElements); + ChangeAttribute (0) = anAttribs[0]; + ChangeAttribute (1) = anAttribs[1]; + return true; + } + }; + +private: + + Handle(ColoredNodeBuffer) mySegments; + Handle(ColoredNodeBuffer) myPoints; + Bnd_Box myViewBox; + +}; + +// ======================================================================= +// function : Dynamics_DebugDrawer +// purpose : +// ======================================================================= +Dynamics_DebugDrawer::Dynamics_DebugDrawer() +: myZLayer (Graphic3d_ZLayerId_Topmost), + myDebugMode (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawContactPoints | btIDebugDraw::DBG_DrawText | btIDebugDraw::DBG_DrawFeaturesText) +{ + //myDebugMode = -1; +} + +// ======================================================================= +// function : ~Dynamics_DebugDrawer +// purpose : +// ======================================================================= +Dynamics_DebugDrawer::~Dynamics_DebugDrawer() +{ + FrameClear (nullptr); +} + +// ======================================================================= +// function : FrameClear +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::FrameClear (AIS_InteractiveContext* theCtx) +{ + for (NCollection_Sequence::Iterator aPrsIter (myTmpPrsList); aPrsIter.More(); aPrsIter.Next()) + { + if (aPrsIter.Value()->HasInteractiveContext()) + { + if (theCtx == nullptr) + { + throw Standard_ProgramError ("Internal Error: Dynamics_DebugDrawer::FrameClear() is called with NULL context"); + } + theCtx->Remove (aPrsIter.Value(), false); + } + } + myTmpPrsList.Clear(); + if (!myTmpPrs.IsNull()) + { + myTmpPrs->Clear(); + } +} + +// ======================================================================= +// function : FrameBegin +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::FrameBegin (const Handle(AIS_InteractiveContext)& theCtx) +{ + FrameClear (theCtx.get()); + if (myTmpPrs.IsNull()) + { + myTmpPrs = new Dynamics_DebugPrs(); + myTmpPrs->SetZLayer (myZLayer); + } + { + Handle(V3d_View) aView = theCtx->CurrentViewer()->ActiveViewIterator().Value(); + const Bnd_Box aViewBox = aView->View()->MinMaxValues(); + myTmpPrs->SetViewBox (aViewBox); + } + myTmpPrsList.Append (myTmpPrs); +} + +// ======================================================================= +// function : FrameEnd +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::FrameEnd (const Handle(AIS_InteractiveContext)& theCtx) +{ + theCtx->RecomputePrsOnly (myTmpPrs, false); + for (NCollection_Sequence::Iterator aPrsIter (myTmpPrsList); aPrsIter.More(); aPrsIter.Next()) + { + theCtx->Display (aPrsIter.Value(), 0, -1, false); + } +} + +// ======================================================================= +// function : drawLine +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::drawLine (const btVector3& theFrom, + const btVector3& theTo, + const btVector3& theColor) +{ + Dynamics_DebugPrs::ColoredNode aNode; + aNode.Color = colorBt2vec4ub (theColor); + aNode.Position.SetValues (theFrom.x(), theFrom.y(), theFrom.z()); + myTmpPrs->ChangeSegments().push_back (aNode); + aNode.Position.SetValues (theTo.x(), theTo.y(), theTo.z()); + myTmpPrs->ChangeSegments().push_back (aNode); +} + +// ======================================================================= +// function : drawLine +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::drawLine (const btVector3& theFrom, + const btVector3& theTo, + const btVector3& theFromColor, + const btVector3& theToColor) +{ + Dynamics_DebugPrs::ColoredNode aNode; + aNode.Color = colorBt2vec4ub (theFromColor); + aNode.Position.SetValues (theFrom.x(), theFrom.y(), theFrom.z()); + myTmpPrs->ChangeSegments().push_back (aNode); + aNode.Color = colorBt2vec4ub (theToColor); + aNode.Position.SetValues (theTo.x(), theTo.y(), theTo.z()); + myTmpPrs->ChangeSegments().push_back (aNode); +} + +// ======================================================================= +// function : drawContactPoint +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::drawContactPoint (const btVector3& thePointOnB, + const btVector3& theNormalOnB, + btScalar theDistance, + int theLifeTime, + const btVector3& theColor) +{ + Dynamics_DebugPrs::ColoredNode aNode; + aNode.Color = colorBt2vec4ub (theColor); + aNode.Position.SetValues (thePointOnB.x(), thePointOnB.y(), thePointOnB.z()); + myTmpPrs->ChangePoints().push_back (aNode); + (void )theNormalOnB; + (void )theDistance; + (void )theLifeTime; +} + +// ======================================================================= +// function : reportErrorWarning +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::reportErrorWarning (const char* theWarningString) +{ + Message::SendWarning (theWarningString); +} + +// ======================================================================= +// function : draw3dText +// purpose : +// ======================================================================= +void Dynamics_DebugDrawer::draw3dText (const btVector3& theLocation, + const char* theTextString) +{ + Handle(AIS_TextLabel) aLabel = new AIS_TextLabel(); + aLabel->SetMutable(true); + aLabel->SetZLayer (myZLayer); + aLabel->SetInfiniteState (true); + aLabel->SetText (theTextString); + aLabel->SetPosition (gp_Pnt (theLocation.x(), theLocation.y(), theLocation.z())); + myTmpPrsList.Append (aLabel); +} + +#endif diff --git a/src/Dynamics/Dynamics_DebugDrawer.hxx b/src/Dynamics/Dynamics_DebugDrawer.hxx new file mode 100644 index 0000000000..c656001320 --- /dev/null +++ b/src/Dynamics/Dynamics_DebugDrawer.hxx @@ -0,0 +1,90 @@ +// Copyright (c) 2018-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_DebugDrawer_HeaderFile +#define _Dynamics_DebugDrawer_HeaderFile + +#include + +#include + +//! Define the debugging drawer. +class Dynamics_DebugDrawer : public btIDebugDraw +{ +public: + + //! Empty constructor. + Standard_EXPORT Dynamics_DebugDrawer(); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_DebugDrawer(); + + //! Return TRUE if temporary presentations have been displayed (and not cleared) in Interactive Context. + bool IsEmpty() const { return myTmpPrsList.IsEmpty(); } + + //! Remove temporary presentations from context. + Standard_EXPORT void FrameClear (AIS_InteractiveContext* theCtx); + + //! Begin frame rendering. + Standard_EXPORT void FrameBegin (const Handle(AIS_InteractiveContext)& theCtx); + + //! End frame rendering. + Standard_EXPORT void FrameEnd (const Handle(AIS_InteractiveContext)& theCtx); + +public: + + //! Draw the line segment. + Standard_EXPORT virtual void drawLine (const btVector3& theFrom, + const btVector3& theTo, + const btVector3& theColor) Standard_OVERRIDE; + + //! Draw the line segment. + Standard_EXPORT virtual void drawLine (const btVector3& theFrom, + const btVector3& theTo, + const btVector3& theFromColor, + const btVector3& theToColor) Standard_OVERRIDE; + + //! Draw contact point. + Standard_EXPORT virtual void drawContactPoint (const btVector3& thePointOnB, + const btVector3& theNormalOnB, + btScalar theDistance, + int theLifeTime, + const btVector3& theColor) Standard_OVERRIDE; + + //! Print message. + Standard_EXPORT virtual void reportErrorWarning (const char* theWarningString) Standard_OVERRIDE; + + //! Draw text. + Standard_EXPORT virtual void draw3dText (const btVector3& theLocation, + const char* theTextString) Standard_OVERRIDE; + + //! Return debug mode. + virtual int getDebugMode() const Standard_OVERRIDE { return myDebugMode; } + + //! Set debug mode. + virtual void setDebugMode (int theDebugMode) Standard_OVERRIDE { myDebugMode = theDebugMode; } + +private: + + class Dynamics_DebugPrs; + +private: + + Handle(Dynamics_DebugPrs) myTmpPrs; + NCollection_Sequence myTmpPrsList; + Graphic3d_ZLayerId myZLayer; + Standard_Integer myDebugMode; + +}; + +#endif // _Dynamics_DebugDrawer_HeaderFile diff --git a/src/Dynamics/Dynamics_Graphic3dTriangulation.cxx b/src/Dynamics/Dynamics_Graphic3dTriangulation.cxx new file mode 100644 index 0000000000..fe9934fff7 --- /dev/null +++ b/src/Dynamics/Dynamics_Graphic3dTriangulation.cxx @@ -0,0 +1,131 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#ifdef HAVE_BULLET + +// ======================================================================= +// function : Dynamics_Graphic3dTriangulation +// purpose : +// ======================================================================= +Dynamics_Graphic3dTriangulation::Dynamics_Graphic3dTriangulation (const Handle(Graphic3d_ArrayOfTriangles)& theTris) +: myPosOffset (0) +{ + if (!theTris.IsNull()) + { + myVerts = theTris->Attributes(); + myIndices = theTris->Indices(); + } + + if (myVerts.IsNull() + || myIndices.IsNull() + || !(myIndices->Stride == 2 || myIndices->Stride == 4)) + { + throw Standard_ProgramError ("Dynamics_Graphic3dTriangulation constructor called with NULL object!"); + } + + bool hasPos = false; + myPosOffset = 0; + for (Standard_Integer anAttribIter = 0; anAttribIter < myVerts->NbAttributes; ++anAttribIter) + { + const Graphic3d_Attribute& anAttrib = myVerts->Attribute (anAttribIter); + if (anAttrib.Id == Graphic3d_TOA_POS) + { + if (anAttrib.DataType != Graphic3d_TOD_VEC3 + && anAttrib.DataType != Graphic3d_TOD_VEC4) + { + throw Standard_ProgramError ("Dynamics_Graphic3dTriangulation constructor called with unsupported data!"); + } + hasPos = true; + break; + } + + myPosOffset += Graphic3d_Attribute::Stride (anAttrib.DataType); + } + if (!hasPos) + { + throw Standard_ProgramError ("Dynamics_Graphic3dTriangulation constructor called with NULL object!"); + } +} + +// ======================================================================= +// function : ~Dynamics_Graphic3dTriangulation +// purpose : +// ======================================================================= +Dynamics_Graphic3dTriangulation::~Dynamics_Graphic3dTriangulation() +{ + // +} + +// ======================================================================= +// function : getLockedVertexIndexBase +// purpose : +// ======================================================================= +void Dynamics_Graphic3dTriangulation::getLockedVertexIndexBase (unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) +{ + if (theSubPart != 0) + { + return; + } + + theType = PHY_FLOAT; + theNbVerts = myVerts->NbElements; + theStride = myVerts->Stride; + *theVertexBase = myVerts->changeValue (0) + myPosOffset; + + theIndicesType = myIndices->Stride == 2 ? PHY_SHORT : PHY_INTEGER; + *theIndexBase = myIndices->changeValue (0); + theNbFaces = myIndices->NbElements / 3; + theIndexStride = myIndices->Stride * 3; +} + +// ======================================================================= +// function : getLockedReadOnlyVertexIndexBase +// purpose : +// ======================================================================= +void Dynamics_Graphic3dTriangulation::getLockedReadOnlyVertexIndexBase (const unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + const unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) const +{ + if (theSubPart != 0) + { + return; + } + + theType = PHY_FLOAT; + theNbVerts = myVerts->NbElements; + theStride = myVerts->Stride; + *theVertexBase = myVerts->value (0) + myPosOffset; + + theIndicesType = myIndices->Stride == 2 ? PHY_SHORT : PHY_INTEGER; + *theIndexBase = myIndices->value (0); + theNbFaces = myIndices->NbElements / 3; + theIndexStride = myIndices->Stride * 3; +} + +#endif // HAVE_BULLET diff --git a/src/Dynamics/Dynamics_Graphic3dTriangulation.hxx b/src/Dynamics/Dynamics_Graphic3dTriangulation.hxx new file mode 100644 index 0000000000..3666d94951 --- /dev/null +++ b/src/Dynamics/Dynamics_Graphic3dTriangulation.hxx @@ -0,0 +1,82 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_Graphic3dTriangulation_HeaderFile +#define _Dynamics_Graphic3dTriangulation_HeaderFile + +#include + +#include + +#ifdef HAVE_BULLET + +//! Interface for Graphic3d_ArrayOfTriangles defining continuous triangulation data. +ATTRIBUTE_ALIGNED16(class) Dynamics_Graphic3dTriangulation : public btStridingMeshInterface +{ +public: + BT_DECLARE_ALIGNED_ALLOCATOR() + + //! Constructor. + Standard_EXPORT Dynamics_Graphic3dTriangulation (const Handle(Graphic3d_ArrayOfTriangles)& theTris); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_Graphic3dTriangulation(); + + //! Get read/write access to a subpart of a triangle mesh. + Standard_EXPORT virtual void getLockedVertexIndexBase (unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) Standard_OVERRIDE; + + //! Get read access to a subpart of a triangle mesh. + Standard_EXPORT virtual void getLockedReadOnlyVertexIndexBase (const unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + const unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) const Standard_OVERRIDE; + + //! Finishes the access to a subpart of the triangle mesh. + virtual void unLockVertexBase (int theSubPart) Standard_OVERRIDE { (void )theSubPart; } + virtual void unLockReadOnlyVertexBase (int theSubPart) const Standard_OVERRIDE { (void )theSubPart; } + + //! Returns the number of seperate subparts; each subpart has a continuous array of vertices and indices. + virtual int getNumSubParts() const Standard_OVERRIDE { return 1; } + + virtual void preallocateVertices (int theNbVerts) Standard_OVERRIDE { (void )theNbVerts; } + virtual void preallocateIndices (int theNbIndices) Standard_OVERRIDE { (void )theNbIndices; } + + //virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + //virtual bool hasPremadeAabb() const Standard_OVERRIDE { return false; } + //virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const Standard_OVERRIDE; + //virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const Standard_OVERRIDE + +protected: + + Handle(Graphic3d_Buffer) myVerts; + Handle(Graphic3d_IndexBuffer) myIndices; + size_t myPosOffset; + +}; + +#endif // HAVE_BULLET + +#endif // _Dynamics_Graphic3dTriangulation_HeaderFile diff --git a/src/Dynamics/Dynamics_Internal.hxx b/src/Dynamics/Dynamics_Internal.hxx new file mode 100644 index 0000000000..53c9c2151f --- /dev/null +++ b/src/Dynamics/Dynamics_Internal.hxx @@ -0,0 +1,31 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_Internal_HeaderFile +#define _Dynamics_Internal_HeaderFile + +#ifdef HAVE_BULLET + #include + + #include + //#include + #include + #include + #include + + #include +#else +class btTransform {}; +#endif + +#endif // _Dynamics_Internal_HeaderFile diff --git a/src/Dynamics/Dynamics_MotionState.cxx b/src/Dynamics/Dynamics_MotionState.cxx new file mode 100644 index 0000000000..1d6884bdde --- /dev/null +++ b/src/Dynamics/Dynamics_MotionState.cxx @@ -0,0 +1,169 @@ +// Copyright (c) 2018-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_MotionState, Standard_Transient) + +#ifdef HAVE_BULLET +ATTRIBUTE_ALIGNED16(struct) Dynamics_MotionState::Dynamics_MotionStateProxy : public btDefaultMotionState +{ + Dynamics_MotionStateProxy (Dynamics_MotionState* theHolder, + const btTransform& theStartTrans = btTransform::getIdentity(), + const btTransform& theCenterOfMassOffset = btTransform::getIdentity(), + const gp_Trsf& thePrsOffset = gp_Trsf()) + : btDefaultMotionState (theStartTrans, theCenterOfMassOffset), + myHolder (theHolder), + myPrsOffset (thePrsOffset) {} + + virtual void setWorldTransform (const btTransform& theCenterOfMassWorldTrans) Standard_OVERRIDE + { + btDefaultMotionState::setWorldTransform (theCenterOfMassWorldTrans); + + btTransform aTrsfBt; + getWorldTransform (aTrsfBt); + + gp_Trsf aTrsf; + Dynamics_MotionState::TrsfGpFromBt (aTrsf, aTrsfBt); + aTrsf *= myPrsOffset; + myHolder->setWorldTransform (aTrsf); + } +private: + Dynamics_MotionState* myHolder; + gp_Trsf myPrsOffset; +}; +#endif + +// ======================================================================= +// function : Dynamics_MotionState +// purpose : +// ======================================================================= +Dynamics_MotionState::Dynamics_MotionState() +: myMotionState (NULL), + myToDisplaceCenterOfMass (false) +{ + // +} + +// ======================================================================= +// function : ~Dynamics_MotionState +// purpose : +// ======================================================================= +Dynamics_MotionState::~Dynamics_MotionState() +{ + releaseMotionState(); +} + +// ======================================================================= +// function : InitProxy +// purpose : +// ======================================================================= +void Dynamics_MotionState::InitProxy (const gp_Trsf& theStartTrans, + const gp_Trsf& theCenterOfMassOffset, + const gp_Trsf& thePrsOffset) +{ + releaseMotionState(); +#ifdef HAVE_BULLET + btTransform aTrsfBt, aMassOffsetBt; + Dynamics_MotionState::TrsfBtFromGp (aTrsfBt, theStartTrans); + Dynamics_MotionState::TrsfBtFromGp (aMassOffsetBt, theCenterOfMassOffset); + myMotionState = new Dynamics_MotionStateProxy (this, aTrsfBt, aMassOffsetBt, thePrsOffset); +#else + (void )theStartTrans; + (void )theCenterOfMassOffset; + (void )thePrsOffset; +#endif +} + +// ======================================================================= +// function : InitDefault +// purpose : +// ======================================================================= +void Dynamics_MotionState::InitDefault (const btTransform& theStartTrans, + const btTransform& theCenterOfMassOffset) +{ + releaseMotionState(); +#ifdef HAVE_BULLET + myMotionState = new btDefaultMotionState (theStartTrans, theCenterOfMassOffset); +#else + (void )theStartTrans; + (void )theCenterOfMassOffset; +#endif +} + +// ======================================================================= +// function : setWorldTransform +// purpose : +// ======================================================================= +void Dynamics_MotionState::setWorldTransform (const gp_Trsf& theTrsf) +{ + (void )theTrsf; +} + +// ======================================================================= +// function : releaseMotionState +// purpose : +// ======================================================================= +void Dynamics_MotionState::releaseMotionState() +{ +#ifdef HAVE_BULLET + delete myMotionState; +#endif + myMotionState = NULL; +} + +// ======================================================================= +// function : TrsfGpFromBt +// purpose : +// ======================================================================= +void Dynamics_MotionState::TrsfGpFromBt (gp_Trsf& theLoc, const btTransform& theTrsf) +{ +#ifdef HAVE_BULLET + const btVector3& anOrigin = theTrsf.getOrigin(); + const btMatrix3x3& aMat3bt = theTrsf.getBasis(); + theLoc.SetValues (aMat3bt.getRow (0).x(), aMat3bt.getRow (0).y(), aMat3bt.getRow (0).z(), anOrigin.x(), + aMat3bt.getRow (1).x(), aMat3bt.getRow (1).y(), aMat3bt.getRow (1).z(), anOrigin.y(), + aMat3bt.getRow (2).x(), aMat3bt.getRow (2).y(), aMat3bt.getRow (2).z(), anOrigin.z()); +#else + (void )theTrsf; + (void )theLoc; +#endif +} + +// ======================================================================= +// function : TrsfBtFromGp +// purpose : +// ======================================================================= +void Dynamics_MotionState::TrsfBtFromGp (btTransform& theTrsf, const gp_Trsf& theLoc) +{ +#ifdef HAVE_BULLET + theTrsf.setIdentity(); + if (theLoc.Form() == gp_Identity) + { + return; + } + + const gp_Mat aMat3 = theLoc.VectorialPart(); + btVector3 anOrigin (btScalar (theLoc.TranslationPart().X()), btScalar (theLoc.TranslationPart().Y()), btScalar (theLoc.TranslationPart().Z())); + btMatrix3x3 aMat3bt (btScalar (aMat3.Value (1, 1)), btScalar (aMat3.Value (1, 2)), btScalar (aMat3.Value (1, 3)), + btScalar (aMat3.Value (2, 1)), btScalar (aMat3.Value (2, 2)), btScalar (aMat3.Value (2, 3)), + btScalar (aMat3.Value (3, 1)), btScalar (aMat3.Value (3, 2)), btScalar (aMat3.Value (3, 3))); + theTrsf.setOrigin(anOrigin); + theTrsf.setBasis (aMat3bt); +#else + (void )theTrsf; + (void )theLoc; +#endif +} diff --git a/src/Dynamics/Dynamics_MotionState.hxx b/src/Dynamics/Dynamics_MotionState.hxx new file mode 100644 index 0000000000..c0959a4459 --- /dev/null +++ b/src/Dynamics/Dynamics_MotionState.hxx @@ -0,0 +1,86 @@ +// Copyright (c) 2018-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_MotionState_HeaderFile +#define _Dynamics_MotionState_HeaderFile + +#include +#include +#include + +class btMotionState; +class btTransform; + +//! Define the motion state. +class Dynamics_MotionState : public Standard_Transient +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_MotionState, Standard_Transient) +public: + + //! Convert transformation definition from OCCT to Bullet structure. + Standard_EXPORT static void TrsfBtFromGp (btTransform& theTrsf, const gp_Trsf& theLoc); + + //! Convert transformation definition from Bullet to OCCT structure. + Standard_EXPORT static void TrsfGpFromBt (gp_Trsf& theLoc, const btTransform& theTrsf); +public: + + //! Empty constructor. + Standard_EXPORT Dynamics_MotionState(); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_MotionState(); + + //! Create a proxy motion state. + //! @param theStartTrans initial transformation + //! @param theCenterOfMassOffset offset to the center of mass + //! @param thePrsOffset optional transformation from collision shape to presentation shape + Standard_EXPORT void InitProxy (const gp_Trsf& theStartTrans, + const gp_Trsf& theCenterOfMassOffset, + const gp_Trsf& thePrsOffset = gp_Trsf()); + + //! Create a default motion state. + Standard_EXPORT void InitDefault (const btTransform& theStartTrans, + const btTransform& theCenterOfMassOffset); + + //! Return true if motion state is defined. + bool IsValid() const { return myMotionState != NULL; } + + //! Return motion state pointer. + btMotionState* MotionState() const { return myMotionState; } + + //! Option displaying center of mass; FALSE by default. + bool ToDisplaceCenterOfMass() const { return myToDisplaceCenterOfMass; } + + //! Set option displaying center of mass. + void SetDisplaceCenterOfMass (bool theToDisplace) { myToDisplaceCenterOfMass = theToDisplace; } + +protected: + + //! Virtual callback. + Standard_EXPORT virtual void setWorldTransform (const gp_Trsf& theTrsf); + + //! Release memory. + Standard_EXPORT void releaseMotionState(); + +protected: + //! Proxy btMotionState implementation. + struct Dynamics_MotionStateProxy; + +protected: + + btMotionState* myMotionState; + bool myToDisplaceCenterOfMass; + +}; + +#endif // _Dynamics_MotionState_HeaderFile diff --git a/src/Dynamics/Dynamics_Parameters.cxx b/src/Dynamics/Dynamics_Parameters.cxx new file mode 100644 index 0000000000..7280a49d4d --- /dev/null +++ b/src/Dynamics/Dynamics_Parameters.cxx @@ -0,0 +1,31 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_Parameters, Standard_Transient) + +// ======================================================================= +// function : Dynamics_Parameters +// purpose : +// ======================================================================= +Dynamics_Parameters::Dynamics_Parameters() +: myRigidBodyYaw (-1), + myRigidBodyPitch (15), + myRigidBodyRoll (15), + myFirstPersonHeight (1.75f), + myFirstPersonWidth (0.46f), + myFirstPersonStepHeight (0.35f) +{ + // +} diff --git a/src/Dynamics/Dynamics_Parameters.hxx b/src/Dynamics/Dynamics_Parameters.hxx new file mode 100644 index 0000000000..82c73fb7b9 --- /dev/null +++ b/src/Dynamics/Dynamics_Parameters.hxx @@ -0,0 +1,78 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_Parameters_HeaderFile +#define _Dynamics_Parameters_HeaderFile + +#include + +//! Class defining global dynamics parameters. +class Dynamics_Parameters : public Standard_Transient +{ + DEFINE_STANDARD_RTTIEXT(Dynamics_Parameters, Standard_Transient) +public: + + //! Empty constructor. + Standard_EXPORT Dynamics_Parameters(); + + //! Return rigid body yaw constraint in degrees; -1 by default. + //! -1 means no constraint. + int RigidBodyYawConstraint() const { return myRigidBodyYaw; } + + //! Set rigid body yaw constraint in degrees. + void SetRigidBodyYawConstraint (int theAngle) { myRigidBodyYaw = theAngle; } + + //! Return rigid body pitch constraint in degrees; 15 degrees by default. + //! -1 means no constraint. + int RigidBodyPitchConstraint() const { return myRigidBodyPitch; } + + //! Set rigid body pitch constraint in degrees. + void SetRigidBodyPitchConstraint (int theAngle) { myRigidBodyPitch = theAngle; } + + //! Return rigid body roll constraint in degrees; 15 degrees by default. + //! -1 means no constraint. + int RigidBodyRollConstraint() const { return myRigidBodyRoll; } + + //! Set rigid body roll constraint in degrees. + void SetRigidBodyRollConstraint (int theAngle) { myRigidBodyRoll = theAngle; } + + //! Return height of first person capsule, in meters; 1.75 by default. + float FirstPersonHeight() const { return myFirstPersonHeight; } + + //! Set height of first person capsule. + void SetFirstPersonHeight (float theHeight) { myFirstPersonHeight = theHeight; } + + //! Return width (diameter) of first person capsule, in meters; 0.46 by default. + float FirstPersonWidth() const { return myFirstPersonWidth; } + + //! Set width of first person capsule. + void SetFirstPersonWidth (float theWidth) { myFirstPersonWidth = theWidth; } + + //! Return step height of first person, in meters; 0.35 by default. + float FirstPersonStepHeight() const { return myFirstPersonStepHeight; } + + //! Set step height of first person. + void SetFirstPersonStepHeight (float theStepHeight) { myFirstPersonStepHeight = theStepHeight; } + +protected: + + int myRigidBodyYaw; //!< rigid body yaw constraint + int myRigidBodyPitch; //!< rigid body pitch constraint + int myRigidBodyRoll; //!< rigid body roll constraint + float myFirstPersonHeight; //!< height of first person capsule, in meters + float myFirstPersonWidth; //!< width (diameter) of first person capsule, in meters + float myFirstPersonStepHeight; //!< step height of first person, in meters + +}; + +#endif // _Dynamics_Parameters_HeaderFile diff --git a/src/Dynamics/Dynamics_PolyTriangulation.cxx b/src/Dynamics/Dynamics_PolyTriangulation.cxx new file mode 100644 index 0000000000..5b690a3865 --- /dev/null +++ b/src/Dynamics/Dynamics_PolyTriangulation.cxx @@ -0,0 +1,102 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#ifdef HAVE_BULLET + +// ======================================================================= +// function : Dynamics_PolyTriangulation +// purpose : +// ======================================================================= +Dynamics_PolyTriangulation::Dynamics_PolyTriangulation (const Handle(Poly_Triangulation)& thePolyTri) +: myPolyTri (thePolyTri) +{ + if (myPolyTri.IsNull()) + { + throw Standard_ProgramError ("Dynamics_PolyTriangulation constructor called with NULL object!"); + } +} + +// ======================================================================= +// function : ~Dynamics_PolyTriangulation +// purpose : +// ======================================================================= +Dynamics_PolyTriangulation::~Dynamics_PolyTriangulation() +{ + // +} + +// ======================================================================= +// function : getLockedVertexIndexBase +// purpose : +// ======================================================================= +void Dynamics_PolyTriangulation::getLockedVertexIndexBase (unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) +{ + if (theSubPart != 0) + { + return; + } + + theType = myPolyTri->InternalNodes().IsDoublePrecision() ? PHY_DOUBLE : PHY_FLOAT; + theNbVerts = myPolyTri->NbNodes(); + theStride = myPolyTri->InternalNodes().Stride(); + *theVertexBase = reinterpret_cast(myPolyTri->InternalNodes().changeValue (0)) + - theStride; // step back since indexation starts with 1 in Poly_Triangulation + + theIndicesType = PHY_INTEGER; + *theIndexBase = reinterpret_cast(&myPolyTri->InternalTriangles().ChangeFirst()); + theNbFaces = myPolyTri->NbTriangles(); + theIndexStride = sizeof(Poly_Triangle); +} + +// ======================================================================= +// function : getLockedReadOnlyVertexIndexBase +// purpose : +// ======================================================================= +void Dynamics_PolyTriangulation::getLockedReadOnlyVertexIndexBase (const unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + const unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) const +{ + if (theSubPart != 0) + { + return; + } + + theType = myPolyTri->InternalNodes().IsDoublePrecision() ? PHY_DOUBLE : PHY_FLOAT; + theNbVerts = myPolyTri->NbNodes(); + theStride = myPolyTri->InternalNodes().Stride(); + *theVertexBase = reinterpret_cast(myPolyTri->InternalNodes().changeValue (0)) + - theStride; // step back since indexation starts with 1 in Poly_Triangulation + + theIndicesType = PHY_INTEGER; + *theIndexBase = reinterpret_cast(&myPolyTri->InternalTriangles().First()); + theNbFaces = myPolyTri->NbTriangles(); + theIndexStride = sizeof(Poly_Triangle); +} + +#endif // HAVE_BULLET diff --git a/src/Dynamics/Dynamics_PolyTriangulation.hxx b/src/Dynamics/Dynamics_PolyTriangulation.hxx new file mode 100644 index 0000000000..0a41c883ad --- /dev/null +++ b/src/Dynamics/Dynamics_PolyTriangulation.hxx @@ -0,0 +1,80 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_PolyTriangulation_HeaderFile +#define _Dynamics_PolyTriangulation_HeaderFile + +#include + +#ifdef HAVE_BULLET + +#include + +//! Interface for Poly_Triangulation defining continuous triangulation data. +ATTRIBUTE_ALIGNED16(class) Dynamics_PolyTriangulation : public btStridingMeshInterface +{ +public: + BT_DECLARE_ALIGNED_ALLOCATOR() + + //! Constructor. + Standard_EXPORT Dynamics_PolyTriangulation (const Handle(Poly_Triangulation)& thePolyTri); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_PolyTriangulation(); + + //! Get read/write access to a subpart of a triangle mesh. + Standard_EXPORT virtual void getLockedVertexIndexBase (unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) Standard_OVERRIDE; + + //! Get read access to a subpart of a triangle mesh. + Standard_EXPORT virtual void getLockedReadOnlyVertexIndexBase (const unsigned char** theVertexBase, + int& theNbVerts, + PHY_ScalarType& theType, + int& theStride, + const unsigned char** theIndexBase, + int& theIndexStride, + int& theNbFaces, + PHY_ScalarType& theIndicesType, + int theSubPart) const Standard_OVERRIDE; + + //! Finishes the access to a subpart of the triangle mesh. + virtual void unLockVertexBase (int theSubPart) Standard_OVERRIDE { (void )theSubPart; } + virtual void unLockReadOnlyVertexBase (int theSubPart) const Standard_OVERRIDE { (void )theSubPart; } + + //! Returns the number of separate subparts; each subpart has a continuous array of vertices and indices. + virtual int getNumSubParts() const Standard_OVERRIDE { return 1; } + + virtual void preallocateVertices (int theNbVerts) Standard_OVERRIDE { (void )theNbVerts; } + virtual void preallocateIndices (int theNbIndices) Standard_OVERRIDE { (void )theNbIndices; } + + //virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + //virtual bool hasPremadeAabb() const Standard_OVERRIDE { return false; } + //virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const Standard_OVERRIDE; + //virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const Standard_OVERRIDE + +protected: + + Handle(Poly_Triangulation) myPolyTri; + +}; + +#endif // HAVE_BULLET + +#endif // _Dynamics_PolyTriangulation_HeaderFile diff --git a/src/Dynamics/Dynamics_World.cxx b/src/Dynamics/Dynamics_World.cxx new file mode 100644 index 0000000000..2c18021c03 --- /dev/null +++ b/src/Dynamics/Dynamics_World.cxx @@ -0,0 +1,312 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include +#include +#include +#include + +#include + +#ifdef HAVE_BULLET + #include + + #ifdef _MSC_VER + #ifdef NDEBUG + #pragma comment(lib, "BulletDynamics.lib") + #pragma comment(lib, "BulletCollision.lib") + #pragma comment(lib, "LinearMath.lib") + #else + #pragma comment(lib, "BulletDynamics_Debug.lib") + #pragma comment(lib, "BulletCollision_Debug.lib") + #pragma comment(lib, "LinearMath_Debug.lib") + #endif + #endif +#endif + +IMPLEMENT_STANDARD_RTTIEXT(Dynamics_World, Standard_Transient) + +// ======================================================================= +// function : Dynamics_World +// purpose : +// ======================================================================= +Dynamics_World::Dynamics_World (const Handle(Dynamics_Parameters)& theDefaultStyle) : +#ifdef HAVE_BULLET + myGhostPairCallback (new btGhostPairCallback()), + myCollisionConfiguration (new btDefaultCollisionConfiguration()), + myOverlappingPairCache (new btDbvtBroadphase()), + mySolver (new btSequentialImpulseConstraintSolver()), + myCharacter (new Dynamics_Character()), + myGround (new Dynamics_CollisionBody()), +#endif + myLengthUnitScale (1.0), + myLastWorldTick (0.0), + myFixedTimeStep (1.0 / 120.0), + myNbMaxSubTicks (20), + myNbRigidBodies (0) +{ + myDefaultDrawer = theDefaultStyle; + if (theDefaultStyle.IsNull()) + { + myDefaultDrawer = new Dynamics_Parameters(); + } +#ifdef HAVE_BULLET + myCharacter->Init (theDefaultStyle->FirstPersonHeight(), theDefaultStyle->FirstPersonWidth(), theDefaultStyle->FirstPersonStepHeight()); + myOverlappingPairCache->getOverlappingPairCache()->setInternalGhostPairCallback (myGhostPairCallback.get()); + myDispatcher.reset (new btCollisionDispatcher (myCollisionConfiguration.get())); + myDynamicsWorld.reset (new btDiscreteDynamicsWorld (myDispatcher.get(), myOverlappingPairCache.get(), mySolver.get(), myCollisionConfiguration.get())); + myDynamicsWorld->setGravity (btVector3 (0.0f, 0.0f, btScalar(-9.8 * myLengthUnitScale))); + + myGround->addToWorld (this); +#endif +} + +// ======================================================================= +// function : ~Dynamics_World +// purpose : +// ======================================================================= +Dynamics_World::~Dynamics_World() +{ + for (NCollection_IndexedMap::Iterator aConstrIter (myConstraints); aConstrIter.More(); aConstrIter.Next()) + { + aConstrIter.Value()->removeFromWorld(); + } + for (NCollection_IndexedMap::Iterator aBodyIter (myCollisionBodies); aBodyIter.More(); aBodyIter.Next()) + { + aBodyIter.Value()->removeFromWorld(); + } + if (!myCharacter.IsNull()) + { + myCharacter->removeFromWorld(); + } + if (!myGround.IsNull()) + { + myGround->removeFromWorld(); + } + myConstraints.Clear(); + myCollisionBodies.Clear(); + myCharacter.Nullify(); + +#ifdef HAVE_BULLET + myDynamicsWorld.reset(); + mySolver.reset(); + myOverlappingPairCache.reset(); + myDispatcher.reset(); + myCollisionConfiguration.reset(); + myGhostPairCallback.reset(); +#endif +} + +// ======================================================================= +// function : UpdateFirstPersonSize +// purpose : +// ======================================================================= +void Dynamics_World::UpdateFirstPersonSize() +{ + if (!myCharacter.IsNull()) + { + myCharacter->Init (myLengthUnitScale * myDefaultDrawer->FirstPersonHeight(), myLengthUnitScale * myDefaultDrawer->FirstPersonWidth(), myLengthUnitScale * myDefaultDrawer->FirstPersonStepHeight()); + } +} + +// ======================================================================= +// function : SetLengthUnitScale +// purpose : +// ======================================================================= +void Dynamics_World::SetLengthUnitScale (double theScale) +{ + myLengthUnitScale = theScale; +#ifdef HAVE_BULLET + myDynamicsWorld->setGravity (btVector3 (0.0f, 0.0f, btScalar(-9.8 * myLengthUnitScale))); +#endif + UpdateFirstPersonSize(); +} + +// ======================================================================= +// function : SetSceneBoundaries +// purpose : +// ======================================================================= +void Dynamics_World::SetSceneBoundaries (const Bnd_Box& theBndBox) +{ + mySceneBox = theBndBox; + if (mySceneBox.IsVoid()) + { + return; + } + +#ifdef HAVE_BULLET + const gp_XYZ aMin = mySceneBox.CornerMin().XYZ(); + const gp_XYZ aMax = mySceneBox.CornerMax().XYZ(); + gp_XYZ aCenter = (aMin + aMax) * 0.5; + aCenter.SetZ (aMin.Z()); + + gp_Pln aPlane (gp_Pnt (0.0, 0.0, 0.0), gp_Dir (0.0, 0.0, 1.0)); + Handle(Dynamics_CollisionPlane) aPlaneShape = new Dynamics_CollisionPlane (aPlane); + + gp_Trsf aTrsf; + aTrsf.SetTranslation (gp_Vec (aCenter)); + myGround->SetShape (aPlaneShape, aTrsf); +#endif +} + +// ======================================================================= +// function : AddCollisionBody +// purpose : +// ======================================================================= +void Dynamics_World::AddCollisionBody (const Handle(Dynamics_CollisionBody)& theBody) +{ + const int anExtendOld = myCollisionBodies.Extent(); + if (myCollisionBodies.Add (theBody) > anExtendOld) + { + #ifdef HAVE_BULLET + theBody->addToWorld (this); + #endif + } +} + +// ======================================================================= +// function : RemoveCollisionBody +// purpose : +// ======================================================================= +void Dynamics_World::RemoveCollisionBody (const Handle(Dynamics_CollisionBody)& theBody) +{ + Handle(Dynamics_CollisionBody) aTmpCopy = theBody; + if (myCollisionBodies.RemoveKey (theBody)) + { + #ifdef HAVE_BULLET + theBody->removeFromWorld(); + #endif + } +} + +// ======================================================================= +// function : AddConstraint +// purpose : +// ======================================================================= +void Dynamics_World::AddConstraint (const Handle(Dynamics_Constraint)& theConstraint) +{ + const int anExtendOld = myConstraints.Extent(); + if (myConstraints.Add (theConstraint) > anExtendOld) + { + #ifdef HAVE_BULLET + theConstraint->addToWorld (this); + #endif + } +} + +// ======================================================================= +// function : RemoveConstraint +// purpose : +// ======================================================================= +void Dynamics_World::RemoveConstraint (const Handle(Dynamics_Constraint)& theConstraint) +{ + Handle(Dynamics_Constraint) aTmpCopy = theConstraint; + if (myConstraints.RemoveKey (theConstraint)) + { + #ifdef HAVE_BULLET + theConstraint->removeFromWorld(); + #endif + } +} + +// ======================================================================= +// function : SetPaused +// purpose : +// ======================================================================= +void Dynamics_World::SetPaused (bool theToPause) +{ + if (IsPaused() == theToPause) + { + return; + } + + if (theToPause) + { + myWorldTimer.Stop(); + } + else + { + myWorldTimer.Start(); + } +} + +// ======================================================================= +// function : SetEnableDebugDrawer +// purpose : +// ======================================================================= +bool Dynamics_World::SetEnableDebugDrawer (bool theToDebug, + AIS_InteractiveContext* theCtx) +{ +#ifdef HAVE_BULLET + if (theToDebug) + { + if (myDebugDrawer.get() == nullptr) + { + myDebugDrawer.reset (new Dynamics_DebugDrawer()); + myDynamicsWorld->setDebugDrawer (myDebugDrawer.get()); + return true; + } + } + else if (myDebugDrawer.get() != nullptr) + { + const bool toUpdate = !myDebugDrawer->IsEmpty(); + myDynamicsWorld->setDebugDrawer (nullptr); + myDebugDrawer->FrameClear (theCtx); + myDebugDrawer.reset(); + return toUpdate; + } +#else + (void )theToDebug; + (void )theCtx; +#endif + return false; +} + +// ======================================================================= +// function : StepSimulation +// purpose : +// ======================================================================= +bool Dynamics_World::StepSimulation (const Handle(AIS_InteractiveContext)& theCtx) +{ + const double aTime = myWorldTimer.ElapsedTime(); + const double aTimeDiff = aTime - myLastWorldTick; + SetPaused (false); + if (Abs (aTimeDiff - myLastWorldTick) < gp::Resolution()) + { + return false; + } + + myLastWorldTick = aTime; +#ifdef HAVE_BULLET + if (myDynamicsWorld->stepSimulation (btScalar(aTimeDiff), myNbMaxSubTicks, btScalar(myFixedTimeStep)) <= 0) + { + return false; + } + + if (Dynamics_DebugDrawer* aDebugDraw = myDebugDrawer.get()) + { + if (!theCtx.IsNull()) + { + aDebugDraw->FrameBegin (theCtx); + myDynamicsWorld->debugDrawWorld(); + aDebugDraw->FrameEnd (theCtx); + } + } + return true; +#else + (void )theCtx; + return false; +#endif +} diff --git a/src/Dynamics/Dynamics_World.hxx b/src/Dynamics/Dynamics_World.hxx new file mode 100644 index 0000000000..8cbf27cccf --- /dev/null +++ b/src/Dynamics/Dynamics_World.hxx @@ -0,0 +1,143 @@ +// Copyright (c) 2017-2021 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _Dynamics_World_HeaderFile +#define _Dynamics_World_HeaderFile + +#include +#include + +#include +#include +#include + +class btBroadphaseInterface; +class btCollisionDispatcher; +class btDefaultCollisionConfiguration; +class btDiscreteDynamicsWorld; +class btGhostPairCallback; +class btSequentialImpulseConstraintSolver; + +class Dynamics_DebugDrawer; +class Dynamics_Parameters; +class AIS_InteractiveContext; + +//! Physics engine - world holder. +class Dynamics_World : public Standard_Transient +{ + friend class Dynamics_CollisionBody; + friend class Dynamics_Constraint; + friend class Dynamics_Character; + DEFINE_STANDARD_RTTIEXT(Dynamics_World, Standard_Transient) +public: + + //! Main constructor. + Standard_EXPORT Dynamics_World (const Handle(Dynamics_Parameters)& theDefaultStyle = Handle(Dynamics_Parameters)()); + + //! Destructor. + Standard_EXPORT virtual ~Dynamics_World(); + + //! Add collision body to the world. + Standard_EXPORT void AddCollisionBody (const Handle(Dynamics_CollisionBody)& theBody); + + //! Remove collision body from the world. + Standard_EXPORT void RemoveCollisionBody (const Handle(Dynamics_CollisionBody)& theBody); + + //! Add constraint to the world. + Standard_EXPORT void AddConstraint (const Handle(Dynamics_Constraint)& theConstraint); + + //! Remove constraint from the world. + Standard_EXPORT void RemoveConstraint (const Handle(Dynamics_Constraint)& theConstraint); + + //! Return the character object. + const Handle(Dynamics_Character)& Character() const { return myCharacter; } + + //! Return scene boundaries. + const Bnd_Box& SceneBoundaries() const { return mySceneBox; } + + //! Define scene boundaries. + Standard_EXPORT void SetSceneBoundaries (const Bnd_Box& theBndBox); + + //! Return length unit scale factor (as scale factor for meters); 1.0 by default. + double LengthUnitScale() const { return myLengthUnitScale; } + + //! Set length unit scale factor. + Standard_EXPORT void SetLengthUnitScale (double theScale); + + //! Update first-person capsule size basing on Drawer parameters. + Standard_EXPORT void UpdateFirstPersonSize(); + + //! Return time step in seconds for computing dynamics within discrete math; 1/120 by default. + double FixedTimeStep() const { return myFixedTimeStep; } + + //! Setup time step in seconds. + void SetFixedTimeStep (double theStepSeconds) { myFixedTimeStep = theStepSeconds; } + + //! Return maximum number of sub steps in case if time increment is greater than fixed time step. + //! 20 by default, which means that within default 1/120 fixed time step + //! the playback should not be lower than 6 FPS for proper computations. + int MaximumSubSteps() const { return myNbMaxSubTicks; } + + //! Setup maximum number of sub steps in case if time increment is greater than fixed time step. + void SetMaximumSubSteps (int theNbMaxSubTicks) { myNbMaxSubTicks = theNbMaxSubTicks; } + + //! Return TRUE if world timer is in paused state; TRUE by default. + bool IsPaused() const { return !myWorldTimer.IsStarted(); } + + //! Start/pause world timer. + //! Note that StepSimulation() will automatically resume timer! + Standard_EXPORT void SetPaused (bool theToPause); + + //! Proceed the simulation over Time step units. + //! Note that if world timer was in paused state, it will be automatically resumed! + //! @param theMaxSubSteps maximum number of sub-steps in case if time increment is greater then fixed time step + //! @param theCtx interactive context for drawing debug information + //! @return TRUE if simulation was updated + Standard_EXPORT bool StepSimulation (const Handle(AIS_InteractiveContext)& theCtx); + + //! Return TRUE if debug drawer is enabled. + bool ToEnableDebugDrawer() const { return myDebugDrawer.get() != NULL; } + + //! Set if debug drawer should be enabled/disabled. + Standard_EXPORT bool SetEnableDebugDrawer (bool theToDebug, + AIS_InteractiveContext* theCtx); + + //! Return the number of registered rigid bodies. + int NbRigidBodies() const { return myNbRigidBodies; } + +protected: + + std::shared_ptr myGhostPairCallback; + std::shared_ptr myCollisionConfiguration; + std::shared_ptr myDispatcher; + std::shared_ptr myOverlappingPairCache; + std::shared_ptr mySolver; + std::shared_ptr myDebugDrawer; + std::shared_ptr myDynamicsWorld; + + Handle(Dynamics_Parameters) myDefaultDrawer; //!< default attributes + Handle(Dynamics_Character) myCharacter; + Handle(Dynamics_CollisionBody) myGround; + NCollection_IndexedMap myCollisionBodies; + NCollection_IndexedMap myConstraints; + Bnd_Box mySceneBox; + OSD_Timer myWorldTimer; + Standard_Real myLengthUnitScale; + Standard_Real myLastWorldTick; + Standard_Real myFixedTimeStep; + Standard_Integer myNbMaxSubTicks; + Standard_Integer myNbRigidBodies; + +}; + +#endif // _Dynamics_World_HeaderFile diff --git a/src/Dynamics/FILES b/src/Dynamics/FILES new file mode 100644 index 0000000000..244c70da21 --- /dev/null +++ b/src/Dynamics/FILES @@ -0,0 +1,32 @@ +Dynamics_Character.cxx +Dynamics_Character.hxx +Dynamics_CharacterState.hxx +Dynamics_CollisionBody.cxx +Dynamics_CollisionBody.hxx +Dynamics_CollisionBoxShape.cxx +Dynamics_CollisionBoxShape.hxx +Dynamics_CollisionBRepFlags.hxx +Dynamics_CollisionBRepShape.cxx +Dynamics_CollisionBRepShape.hxx +Dynamics_CollisionBRepShapeConvex.hxx +Dynamics_CollisionCompoundShape.cxx +Dynamics_CollisionCompoundShape.hxx +Dynamics_CollisionPlane.cxx +Dynamics_CollisionPlane.hxx +Dynamics_CollisionShape.cxx +Dynamics_CollisionShape.hxx +Dynamics_Constraint.cxx +Dynamics_Constraint.hxx +Dynamics_DebugDrawer.cxx +Dynamics_DebugDrawer.hxx +Dynamics_Graphic3dTriangulation.cxx +Dynamics_Graphic3dTriangulation.hxx +Dynamics_Internal.hxx +Dynamics_MotionState.cxx +Dynamics_MotionState.hxx +Dynamics_Parameters.cxx +Dynamics_Parameters.hxx +Dynamics_PolyTriangulation.cxx +Dynamics_PolyTriangulation.hxx +Dynamics_World.cxx +Dynamics_World.hxx diff --git a/src/TKV3d/EXTERNLIB b/src/TKV3d/EXTERNLIB index 7828756181..f5f65b200a 100755 --- a/src/TKV3d/EXTERNLIB +++ b/src/TKV3d/EXTERNLIB @@ -15,4 +15,5 @@ CSF_gdi32 CSF_OpenGlLibs CSF_XwLibs CSF_FREETYPE +CSF_BULLET CSF_TBB diff --git a/src/TKV3d/PACKAGES b/src/TKV3d/PACKAGES index 904ab32610..d41c3a3019 100755 --- a/src/TKV3d/PACKAGES +++ b/src/TKV3d/PACKAGES @@ -9,3 +9,4 @@ AIS StdSelect DsgPrs PrsDim +Dynamics