diff --git a/include/sapien/physx/collision_shape.h b/include/sapien/physx/collision_shape.h index d0211e5a..5c678f9d 100644 --- a/include/sapien/physx/collision_shape.h +++ b/include/sapien/physx/collision_shape.h @@ -183,12 +183,12 @@ class PhysxCollisionShapeConvexMesh : public PhysxCollisionShape { class PhysxCollisionShapeTriangleMesh : public PhysxCollisionShape { public: // TODO: support rotation? - PhysxCollisionShapeTriangleMesh(std::string const &filename, Vec3 const &scale, + PhysxCollisionShapeTriangleMesh(std::string const &filename, Vec3 const &scale, bool sdf, std::shared_ptr material = nullptr); PhysxCollisionShapeTriangleMesh( Eigen::Matrix const &vertices, Eigen::Matrix const &triangles, - Vec3 const &scale, std::shared_ptr material = nullptr); + Vec3 const &scale, bool sdf, std::shared_ptr material = nullptr); // internal use only PhysxCollisionShapeTriangleMesh(std::shared_ptr mesh, Vec3 const &scale, diff --git a/include/sapien/physx/mesh.h b/include/sapien/physx/mesh.h index 3911d14c..7411d0a9 100644 --- a/include/sapien/physx/mesh.h +++ b/include/sapien/physx/mesh.h @@ -66,10 +66,10 @@ class PhysxConvexMesh { class PhysxTriangleMesh { public: - PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles); + PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles, bool generateSDF); PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles, - std::string const &filename); - PhysxTriangleMesh(std::string const &filename); + std::string const &filename, bool generateSDF); + PhysxTriangleMesh(std::string const &filename, bool generateSDF); ::physx::PxTriangleMesh *getPxMesh() const { return mMesh; } bool hasFilename() { return mFilename.has_value(); } @@ -91,7 +91,7 @@ class PhysxTriangleMesh { } private: - void loadMesh(Vertices const &vertices, Triangles const &triangles); + void loadMesh(Vertices const &vertices, Triangles const &triangles, bool generateSDF); std::shared_ptr mEngine; ::physx::PxTriangleMesh *mMesh{}; std::optional mFilename; diff --git a/include/sapien/physx/mesh_manager.h b/include/sapien/physx/mesh_manager.h index 615f947b..c8aa670b 100644 --- a/include/sapien/physx/mesh_manager.h +++ b/include/sapien/physx/mesh_manager.h @@ -21,6 +21,7 @@ class MeshManager { MeshManager(); std::shared_ptr loadTriangleMesh(const std::string &filename); + std::shared_ptr loadTriangleMeshWithSDF(const std::string &filename); std::shared_ptr loadConvexMesh(const std::string &filename); std::vector> loadConvexMeshGroup(const std::string &filename); diff --git a/include/sapien/physx/physx_default.h b/include/sapien/physx/physx_default.h index 121f8d41..eded5120 100644 --- a/include/sapien/physx/physx_default.h +++ b/include/sapien/physx/physx_default.h @@ -25,9 +25,9 @@ struct PhysxSceneConfig { }; struct PhysxBodyConfig { - uint32_t solverPositionIterations = 10; // solver position iterations, helps reduce jittering - uint32_t solverVelocityIterations = 1; // solver velocity iterations - float sleepThreshold = 0.005f; // put to sleep if (kinetic energy/(mass) falls below + uint32_t solverPositionIterations = 10; // solver position iterations, helps reduce jittering + uint32_t solverVelocityIterations = 1; // solver velocity iterations + float sleepThreshold = 0.005f; // put to sleep if (kinetic energy/(mass) falls below }; struct PhysxShapeConfig { @@ -35,6 +35,12 @@ struct PhysxShapeConfig { float restOffset = 0.f; }; +struct PhysxSDFShapeConfig { + float spacing = 0.01f; + uint32_t subgridSize = 6; + uint32_t numThreadsForConstruction = 4; +}; + class PhysxDefault { public: static std::shared_ptr GetDefaultMaterial(); @@ -61,6 +67,11 @@ class PhysxDefault { static void setShapeConfig(PhysxShapeConfig const &); static PhysxShapeConfig const &getShapeConfig(); + static void setSDFShapeConfig(float spacing, uint32_t subgridSize, + uint32_t numThreadsForConstruction); + static void setSDFShapeConfig(PhysxSDFShapeConfig const &); + static PhysxSDFShapeConfig getSDFShapeConfig(); + // enable GPU simulation, may not be disabled static void EnableGPU(); static bool GetGPUEnabled(); diff --git a/include/sapien/physx/physx_system.h b/include/sapien/physx/physx_system.h index d8f542c1..e3fac651 100644 --- a/include/sapien/physx/physx_system.h +++ b/include/sapien/physx/physx_system.h @@ -168,6 +168,19 @@ class PhysxSystemGpu : public PhysxSystem { CudaArrayHandle gpuGetRigidDynamicCudaHandle() const { return mCudaRigidDynamicHandle; } CudaArrayHandle gpuGetArticulationLinkCudaHandle() const { return mCudaLinkHandle; } + CudaArrayHandle gpuGetRigidBodyForceCudaHandle() const { + return mCudaRigidBodyForceBuffer.handle(); + } + CudaArrayHandle gpuGetRigidDynamicForceCudaHandle() const { + return mCudaRigidDynamicForceHandle; + } + CudaArrayHandle gpuGetRigidBodyTorqueCudaHandle() const { + return mCudaRigidBodyTorqueBuffer.handle(); + } + CudaArrayHandle gpuGetRigidDynamicTorqueCudaHandle() const { + return mCudaRigidDynamicTorqueHandle; + } + CudaArrayHandle gpuGetArticulationQposCudaHandle() const { return mCudaQposHandle; } CudaArrayHandle gpuGetArticulationQvelCudaHandle() const { return mCudaQvelHandle; } CudaArrayHandle gpuGetArticulationQaccCudaHandle() const { return mCudaQaccHandle; } @@ -195,6 +208,8 @@ class PhysxSystemGpu : public PhysxSystem { void gpuApplyArticulationQTargetVel(CudaArrayHandle const &indices); void gpuApplyRigidDynamicData(); + void gpuApplyRigidDynamicForce(); + void gpuApplyRigidDynamicTorque(); void gpuApplyArticulationRootPose(); void gpuApplyArticulationRootVel(); void gpuApplyArticulationQpos(); @@ -269,6 +284,12 @@ class PhysxSystemGpu : public PhysxSystem { CudaArrayHandle mCudaRigidDynamicHandle; CudaArrayHandle mCudaLinkHandle; + CudaArray mCudaRigidBodyForceBuffer; + CudaArrayHandle mCudaRigidDynamicForceHandle; + + CudaArray mCudaRigidBodyTorqueBuffer; + CudaArrayHandle mCudaRigidDynamicTorqueHandle; + CudaHostArray mCudaHostRigidBodyBuffer; CudaArray mCudaArticulationBuffer; diff --git a/manualtest/test.py b/manualtest/test.py deleted file mode 100644 index 1a1d7a9f..00000000 --- a/manualtest/test.py +++ /dev/null @@ -1,103 +0,0 @@ -import sapien -import numpy as np -from sapien.utils import Viewer -import os -import warnings - - -# sapien.physx.enable_gpu() - -# sapien.render.set_viewer_shader_dir("../vulkan_shader/rt") -# sapien.render.set_viewer_shader_dir("../vulkan_shader/trivial") -# sapien.render.set_viewer_shader_dir("rt") - - -def main(): - engine = sapien.Engine() - config = sapien.SceneConfig() - config.solver_iterations = 20 - scene = engine.create_scene(config) - - reinstall = False - - scene.load_widget_from_package("demo_arena", "DemoArena") - scene.set_timestep(1 / 200) - - # scene.create_urdf_loader().load("/home/fx/Downloads/1000/1000/mobility_cvx.urdf") - - xarm = scene.load_widget_from_package("xarm7", "XArm7") - franka = scene.load_widget_from_package("franka", "Franka") - - franka.robot.pose = sapien.Pose([0, 1, 0]) - - # filename = "/home/fx/Downloads/Spray_Bottle_5.glb" - # filename = "/home/fx/Downloads/untitled.obj" - # bottle = ( - # scene.create_actor_builder() - # .add_visual_from_file(filename) - # .add_multiple_convex_collisions_from_file(filename) - # .build() - # ) - - # entity = ( - # scene.create_actor_builder() - # .add_visual_from_file("./1ce2ee00bf9442cca7327cd58c88775d.glb") - # .build() - # ) - # comp = entity.find_component_by_type(sapien.render.RenderBodyComponent) - - viewer = Viewer() - viewer.set_scene(scene) - - # b = scene.create_actor_builder() - # size = [0.015, 0.015, 0.015] - # b.add_box_collision(half_size=size) - # b.add_box_visual(half_size=size, material=[1, 0, 0]) - # box = b.build_kinematic(name="red cube") - # box.set_pose(sapien.Pose([0.4, 0, 0.015])) - - # aabb = bottle.find_component_by_type( - # sapien.physx.PhysxRigidBaseComponent - # ).get_global_aabb_fast() - - # aabb = bottle.find_component_by_type( - # sapien.render.RenderBodyComponent - # ).compute_global_aabb_tight() - - # aabb = bottle.find_component_by_type( - # sapien.physx.PhysxRigidBaseComponent - # ).compute_global_aabb_tight() - - # aabb_handle = viewer.draw_aabb(aabb[0], aabb[1], [1, 0, 1]) - - scene.step() - - count = 0 - while not viewer.closed: - for _ in range(4): - scene.step() - count += 1 - # scene.update_render() - # viewer.render() - - xarm.set_gripper_target((np.sin(count / 100) + 1) * 0.4) - - franka.set_gripper_target((np.sin(count / 100) + 1) * 0.02) - - # aabb = bottle.find_component_by_type( - # sapien.render.RenderBodyComponent - # ).compute_global_aabb_tight() - - # aabb = bottle.find_component_by_type( - # sapien.physx.PhysxRigidBaseComponent - # ).get_global_aabb_fast() - - # aabb = bottle.find_component_by_type( - # sapien.physx.PhysxRigidBaseComponent - # ).compute_global_aabb_tight() - - # viewer.update_aabb(aabb_handle, aabb[0], aabb[1]) - # print(aabb) - - -main() diff --git a/python/py_package/__init__.pyi b/python/py_package/__init__.pyi index 0345d0e0..2c2bc607 100644 --- a/python/py_package/__init__.pyi +++ b/python/py_package/__init__.pyi @@ -34,5 +34,5 @@ from . import utils from . import version from . import wrapper __all__ = ['ActorBuilder', 'ArticulationBuilder', 'Component', 'CudaArray', 'Device', 'Engine', 'Entity', 'Path', 'PinocchioModel', 'Pose', 'SapienRenderer', 'Scene', 'SceneConfig', 'System', 'Widget', 'asset', 'internal_renderer', 'math', 'os', 'physx', 'pkg_resources', 'platform', 'profile', 'pysapien', 'pysapien_pinocchio', 'render', 'set_log_level', 'simsense', 'utils', 'version', 'warn', 'wrapper'] -__version__: str = '3.0.0.dev20240506+da1ab45a' +__version__: str = '3.0.0.dev20240514+4fcf746f' __warningregistry__: dict = {'version': 0} diff --git a/python/py_package/pysapien/physx.pyi b/python/py_package/pysapien/physx.pyi index 8eee6b41..2d420abe 100644 --- a/python/py_package/pysapien/physx.pyi +++ b/python/py_package/pysapien/physx.pyi @@ -4,7 +4,7 @@ import pybind11_stubgen.typing_ext import sapien.pysapien import sapien.pysapien_pinocchio import typing -__all__ = ['PhysxArticulation', 'PhysxArticulationJoint', 'PhysxArticulationLinkComponent', 'PhysxBaseComponent', 'PhysxBodyConfig', 'PhysxCollisionShape', 'PhysxCollisionShapeBox', 'PhysxCollisionShapeCapsule', 'PhysxCollisionShapeConvexMesh', 'PhysxCollisionShapeCylinder', 'PhysxCollisionShapePlane', 'PhysxCollisionShapeSphere', 'PhysxCollisionShapeTriangleMesh', 'PhysxContact', 'PhysxContactPoint', 'PhysxCpuSystem', 'PhysxDistanceJointComponent', 'PhysxDriveComponent', 'PhysxEngine', 'PhysxGearComponent', 'PhysxGpuContactBodyImpulseQuery', 'PhysxGpuContactPairImpulseQuery', 'PhysxGpuSystem', 'PhysxJointComponent', 'PhysxMaterial', 'PhysxRayHit', 'PhysxRigidBaseComponent', 'PhysxRigidBodyComponent', 'PhysxRigidDynamicComponent', 'PhysxRigidStaticComponent', 'PhysxSceneConfig', 'PhysxShapeConfig', 'PhysxSystem', 'get_body_config', 'get_default_material', 'get_scene_config', 'get_shape_config', 'is_gpu_enabled', 'set_body_config', 'set_default_material', 'set_gpu_memory_config', 'set_scene_config', 'set_shape_config', 'version'] +__all__ = ['PhysxArticulation', 'PhysxArticulationJoint', 'PhysxArticulationLinkComponent', 'PhysxBaseComponent', 'PhysxBodyConfig', 'PhysxCollisionShape', 'PhysxCollisionShapeBox', 'PhysxCollisionShapeCapsule', 'PhysxCollisionShapeConvexMesh', 'PhysxCollisionShapeCylinder', 'PhysxCollisionShapePlane', 'PhysxCollisionShapeSphere', 'PhysxCollisionShapeTriangleMesh', 'PhysxContact', 'PhysxContactPoint', 'PhysxCpuSystem', 'PhysxDistanceJointComponent', 'PhysxDriveComponent', 'PhysxEngine', 'PhysxGearComponent', 'PhysxGpuContactBodyImpulseQuery', 'PhysxGpuContactPairImpulseQuery', 'PhysxGpuSystem', 'PhysxJointComponent', 'PhysxMaterial', 'PhysxRayHit', 'PhysxRigidBaseComponent', 'PhysxRigidBodyComponent', 'PhysxRigidDynamicComponent', 'PhysxRigidStaticComponent', 'PhysxSDFConfig', 'PhysxSceneConfig', 'PhysxShapeConfig', 'PhysxSystem', 'get_body_config', 'get_default_material', 'get_scene_config', 'get_sdf_config', 'get_shape_config', 'is_gpu_enabled', 'set_body_config', 'set_default_material', 'set_gpu_memory_config', 'set_scene_config', 'set_sdf_config', 'set_shape_config', 'version'] M = typing.TypeVar("M", bound=int) class PhysxArticulation: name: str @@ -424,7 +424,7 @@ class PhysxCollisionShapeSphere(PhysxCollisionShape): def radius(self) -> float: ... class PhysxCollisionShapeTriangleMesh(PhysxCollisionShape): - def __init__(self, filename: str, scale: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float32]] | list[float] | tuple, material: PhysxMaterial) -> None: + def __init__(self, filename: str, scale: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float32]] | list[float] | tuple, sdf: bool, material: PhysxMaterial) -> None: ... def get_scale(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float32]]: ... @@ -630,6 +630,10 @@ class PhysxGpuSystem(PhysxSystem): @typing.overload def gpu_apply_rigid_dynamic_data(self, index_buffer: sapien.pysapien.CudaArray) -> None: ... + def gpu_apply_rigid_dynamic_force(self) -> None: + ... + def gpu_apply_rigid_dynamic_torque(self) -> None: + ... def gpu_create_contact_body_impulse_query(self, bodies: list[PhysxRigidBaseComponent]) -> PhysxGpuContactBodyImpulseQuery: ... def gpu_create_contact_pair_impulse_query(self, body_pairs: list[tuple[PhysxRigidBaseComponent, PhysxRigidBaseComponent]]) -> PhysxGpuContactPairImpulseQuery: @@ -722,9 +726,21 @@ class PhysxGpuSystem(PhysxSystem): def cuda_rigid_body_data(self) -> sapien.pysapien.CudaArray: ... @property + def cuda_rigid_body_force(self) -> sapien.pysapien.CudaArray: + ... + @property + def cuda_rigid_body_torque(self) -> sapien.pysapien.CudaArray: + ... + @property def cuda_rigid_dynamic_data(self) -> sapien.pysapien.CudaArray: ... @property + def cuda_rigid_dynamic_force(self) -> sapien.pysapien.CudaArray: + ... + @property + def cuda_rigid_dynamic_torque(self) -> sapien.pysapien.CudaArray: + ... + @property def device(self) -> sapien.pysapien.Device: ... class PhysxJointComponent(PhysxBaseComponent): @@ -936,6 +952,18 @@ class PhysxRigidDynamicComponent(PhysxRigidBodyComponent): class PhysxRigidStaticComponent(PhysxRigidBaseComponent): def __init__(self) -> None: ... +class PhysxSDFConfig: + num_threads_for_construction: int + spacing: float + subgridSize: int + def __getstate__(self) -> tuple: + ... + def __init__(self) -> None: + ... + def __repr__(self) -> str: + ... + def __setstate__(self, arg0: tuple) -> None: + ... class PhysxSceneConfig: bounce_threshold: float enable_ccd: bool @@ -1004,6 +1032,8 @@ def get_default_material() -> PhysxMaterial: ... def get_scene_config() -> PhysxSceneConfig: ... +def get_sdf_config() -> PhysxSDFConfig: + ... def get_shape_config() -> PhysxShapeConfig: ... def is_gpu_enabled() -> bool: @@ -1025,6 +1055,12 @@ def set_scene_config(gravity: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy def set_scene_config(config: PhysxSceneConfig) -> None: ... @typing.overload +def set_sdf_config(spacing: float = 0.009999999776482582, subgrid_size: int = 6, num_threads_for_construction: int = 4) -> None: + ... +@typing.overload +def set_sdf_config(config: PhysxSDFConfig) -> None: + ... +@typing.overload def set_shape_config(contact_offset: float = 0.009999999776482582, rest_offset: float = 0.0) -> None: ... @typing.overload diff --git a/python/py_package/wrapper/actor_builder.py b/python/py_package/wrapper/actor_builder.py index c53ad0ba..8740272d 100644 --- a/python/py_package/wrapper/actor_builder.py +++ b/python/py_package/wrapper/actor_builder.py @@ -72,9 +72,9 @@ class VisualShapeRecord: radius: float = 1 length: float = 1 - material: Union[ - sapien.render.RenderMaterial, None - ] = None # None is only used for mesh + material: Union[sapien.render.RenderMaterial, None] = ( + None # None is only used for mesh + ) pose: sapien.Pose = sapien.Pose() name: str = "" @@ -219,6 +219,7 @@ def build_physx_component(self, link_parent=None): shape = sapien.physx.PhysxCollisionShapeTriangleMesh( filename=preprocess_mesh_file(r.filename), scale=r.scale, + sdf=self.physx_body_type == "dynamic", material=r.material, ) shapes = [shape] @@ -492,6 +493,7 @@ def add_nonconvex_collision_from_file( pose: sapien.Pose = sapien.Pose(), scale: Vec3 = (1, 1, 1), material: Union[sapien.physx.PhysxMaterial, None] = None, + density: float = 1000, patch_radius: float = 0, min_patch_radius: float = 0, is_trigger: bool = False, @@ -506,7 +508,7 @@ def add_nonconvex_collision_from_file( pose=pose, scale=scale, material=material, - density=0, + density=density, patch_radius=patch_radius, min_patch_radius=min_patch_radius, is_trigger=is_trigger, diff --git a/python/pybind/physx.cpp b/python/pybind/physx.cpp index c452c508..772b0534 100644 --- a/python/pybind/physx.cpp +++ b/python/pybind/physx.cpp @@ -255,6 +255,30 @@ Generator init_physx(py::module &sapien) { return config; })); + auto PyPhysxSDFConfig = py::class_(m, "PhysxSDFConfig"); + PyPhysxSDFConfig.def(py::init<>()) + .def_readwrite("spacing", &PhysxSDFShapeConfig::spacing) + .def_readwrite("subgridSize", &PhysxSDFShapeConfig::subgridSize) + .def_readwrite("num_threads_for_construction", + &PhysxSDFShapeConfig::numThreadsForConstruction) + .def("__repr__", [](PhysxSDFShapeConfig &) { return "PhysxSDFConfig()"; }) + .def(py::pickle( + [](PhysxSDFShapeConfig &config) { + return py::make_tuple(config.spacing, config.subgridSize, + config.numThreadsForConstruction); + }, + [](py::tuple t) { + if (t.size() != 3) { + throw std::runtime_error("Invalid state!"); + } + PhysxSDFShapeConfig config; + config.spacing = t[0].cast(); + config.subgridSize = t[1].cast(); + config.numThreadsForConstruction = + t[2].cast(); + return config; + })); + auto PyPhysxEngine = py::class_(m, "PhysxEngine"); auto PyPhysxContactPoint = py::class_(m, "PhysxContactPoint"); auto PyPhysxContact = py::class_(m, "PhysxContact"); @@ -427,6 +451,16 @@ kernels will be launched to the provided stream. &PhysxSystemGpu::gpuGetRigidDynamicCudaHandle) .def_property_readonly("cuda_articulation_link_data", &PhysxSystemGpu::gpuGetArticulationLinkCudaHandle) + + .def_property_readonly("cuda_rigid_body_force", + &PhysxSystemGpu::gpuGetRigidBodyForceCudaHandle) + .def_property_readonly("cuda_rigid_dynamic_force", + &PhysxSystemGpu::gpuGetRigidDynamicForceCudaHandle) + .def_property_readonly("cuda_rigid_body_torque", + &PhysxSystemGpu::gpuGetRigidBodyTorqueCudaHandle) + .def_property_readonly("cuda_rigid_dynamic_torque", + &PhysxSystemGpu::gpuGetRigidDynamicTorqueCudaHandle) + .def_property_readonly("cuda_articulation_qpos", &PhysxSystemGpu::gpuGetArticulationQposCudaHandle) .def_property_readonly("cuda_articulation_qvel", @@ -472,6 +506,11 @@ kernels will be launched to the provided stream. // TODO apply force torque .def("gpu_apply_rigid_dynamic_data", py::overload_cast<>(&PhysxSystemGpu::gpuApplyRigidDynamicData)) + .def("gpu_apply_rigid_dynamic_force", + py::overload_cast<>(&PhysxSystemGpu::gpuApplyRigidDynamicForce)) + .def("gpu_apply_rigid_dynamic_torque", + py::overload_cast<>(&PhysxSystemGpu::gpuApplyRigidDynamicTorque)) + .def("gpu_apply_articulation_root_pose", py::overload_cast<>(&PhysxSystemGpu::gpuApplyArticulationRootPose)) .def("gpu_apply_articulation_root_velocity", @@ -644,8 +683,8 @@ If after testing g2 and g3, the objects may collide, g0 and g1 come into play. g .def("get_triangles", &PhysxCollisionShapeConvexMesh::getTriangles); PyPhysxCollisionShapeTriangleMesh - .def(py::init>(), - py::arg("filename"), py::arg("scale"), py::arg("material")) + .def(py::init>(), + py::arg("filename"), py::arg("scale"), py::arg("sdf"), py::arg("material")) .def_property_readonly("scale", &PhysxCollisionShapeTriangleMesh::getScale) .def("get_scale", &PhysxCollisionShapeTriangleMesh::getScale) .def_property_readonly("vertices", &PhysxCollisionShapeTriangleMesh::getVertices) @@ -1169,6 +1208,15 @@ set some motion axes of the dynamic rigid body to be locked py::arg("config")) .def("get_body_config", &PhysxDefault::getBodyConfig) + .def("set_sdf_config", + py::overload_cast(&PhysxDefault::setSDFShapeConfig), + py::arg("spacing") = 0.01f, py::arg("subgrid_size") = 6, + py::arg("num_threads_for_construction") = 4) + .def("set_sdf_config", + py::overload_cast(&PhysxDefault::setSDFShapeConfig), + py::arg("config")) + .def("get_sdf_config", &PhysxDefault::getSDFShapeConfig) + .def("version", []() { return PhysxDefault::getPhysxVersion(); }); ////////// end global ////////// diff --git a/src/physx/collision_shape.cpp b/src/physx/collision_shape.cpp index ee114971..beef9879 100644 --- a/src/physx/collision_shape.cpp +++ b/src/physx/collision_shape.cpp @@ -231,10 +231,16 @@ PhysxCollisionShapeConvexMesh::LoadMultiple(std::string const &filename, Vec3 sc } PhysxCollisionShapeTriangleMesh::PhysxCollisionShapeTriangleMesh( - std::string const &filename, Vec3 const &scale, std::shared_ptr material) { + std::string const &filename, Vec3 const &scale, bool sdf, + std::shared_ptr material) { mEngine = PhysxEngine::Get(); mPhysicalMaterial = material ? material : PhysxDefault::GetDefaultMaterial(); - mMesh = MeshManager::Get()->loadTriangleMesh(filename); + + if (sdf) { + mMesh = MeshManager::Get()->loadTriangleMeshWithSDF(filename); + } else { + mMesh = MeshManager::Get()->loadTriangleMesh(filename); + } mPxShape = mEngine->getPxPhysics()->createShape( PxTriangleMeshGeometry(mMesh->getPxMesh(), PxMeshScale(Vec3ToPxVec3(scale))), *getPhysicalMaterial()->getPxMaterial(), true); @@ -247,10 +253,10 @@ PhysxCollisionShapeTriangleMesh::PhysxCollisionShapeTriangleMesh( PhysxCollisionShapeTriangleMesh::PhysxCollisionShapeTriangleMesh( Eigen::Matrix const &vertices, Eigen::Matrix const &triangles, - Vec3 const &scale, std::shared_ptr material) { + Vec3 const &scale, bool sdf, std::shared_ptr material) { mEngine = PhysxEngine::Get(); mPhysicalMaterial = material ? material : PhysxDefault::GetDefaultMaterial(); - mMesh = std::make_shared(vertices, triangles); + mMesh = std::make_shared(vertices, triangles, sdf); mPxShape = mEngine->getPxPhysics()->createShape( PxTriangleMeshGeometry(mMesh->getPxMesh(), PxMeshScale(Vec3ToPxVec3(scale))), *getPhysicalMaterial()->getPxMaterial(), true); diff --git a/src/physx/mesh.cpp b/src/physx/mesh.cpp index 3d359afa..ab134ba1 100644 --- a/src/physx/mesh.cpp +++ b/src/physx/mesh.cpp @@ -383,7 +383,8 @@ Triangles PhysxConvexMesh::getTriangles() const { return Eigen::Map(indices.data(), indices.size() / 3, 3); } -void PhysxTriangleMesh::loadMesh(Vertices const &vertices, Triangles const &triangles) { +void PhysxTriangleMesh::loadMesh(Vertices const &vertices, Triangles const &triangles, + bool generateSDF) { mEngine = PhysxEngine::Get(); PxTriangleMeshDesc meshDesc; @@ -400,6 +401,17 @@ void PhysxTriangleMesh::loadMesh(Vertices const &vertices, Triangles const &tria params.buildGPUData = true; } + PxSDFDesc sdfDesc; + auto config = PhysxDefault::getSDFShapeConfig(); + if (generateSDF) { + sdfDesc.spacing = config.spacing; + sdfDesc.subgridSize = config.subgridSize; + sdfDesc.bitsPerSubgridPixel = PxSdfBitsPerSubgridPixel::e16_BIT_PER_PIXEL; + sdfDesc.numThreadsForSdfConstruction = config.subgridSize; + meshDesc.sdfDesc = &sdfDesc; + params.meshPreprocessParams |= PxMeshPreprocessingFlag::eENABLE_INERTIA; + } + PxDefaultMemoryOutputStream writeBuffer; if (!PxCookTriangleMesh(params, meshDesc, writeBuffer)) { throw std::runtime_error("Failed to cook non-convex mesh"); @@ -410,19 +422,20 @@ void PhysxTriangleMesh::loadMesh(Vertices const &vertices, Triangles const &tria mAABB = computeAABB(getVertices()); } -PhysxTriangleMesh::PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles) { - loadMesh(vertices, triangles); +PhysxTriangleMesh::PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles, + bool generateSDF) { + loadMesh(vertices, triangles, generateSDF); } PhysxTriangleMesh::PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles, - std::string const &filename) - : PhysxTriangleMesh(vertices, triangles) { + std::string const &filename, bool generateSDF) + : PhysxTriangleMesh(vertices, triangles, generateSDF) { mFilename = filename; } -PhysxTriangleMesh::PhysxTriangleMesh(std::string const &filename) { +PhysxTriangleMesh::PhysxTriangleMesh(std::string const &filename, bool generateSDF) { auto [vertices, triangles] = loadVerticesAndTrianglesFromMeshFile(filename); - loadMesh(vertices, triangles); + loadMesh(vertices, triangles, generateSDF); mFilename = filename; } @@ -478,126 +491,5 @@ std::shared_ptr PhysxConvexMesh::CreateCylinder() { return c; } -// static float computeMeshRadius(PxConvexMesh &mesh, PxVec3 cm) { -// float radius = FLT_MAX; -// for (uint32_t i = 0; i < mesh.getNbPolygons(); ++i) { -// PxHullPolygon polygon; -// mesh.getPolygonData(i, polygon); -// float dist = PxAbs(cm.dot(PxVec3(polygon.mPlane[0], polygon.mPlane[1], polygon.mPlane[2])) + -// polygon.mPlane[3]); -// radius = PxMin(radius, dist); -// assert(PxVec3(polygon.mPlane[0], polygon.mPlane[1], polygon.mPlane[2]).magnitudeSquared() > -// 0.999); -// assert(PxVec3(polygon.mPlane[0], polygon.mPlane[1], polygon.mPlane[2]).magnitudeSquared() < -// 1.001); -// } -// return radius; -// } - -// static PxVec3 ComputeInternalExtent(PxConvexMesh &mesh, float radius, PxVec3 cm) { -// auto bound = mesh.getLocalBounds(); -// PxVec3 e = bound.maximum - bound.minimum; -// float r = radius / sqrtf(3.0f); -// const float epsilon = 1e-7f; - -// const PxU32 largestExtent = PxLargestAxis(e); -// PxU32 e0 = PxGetNextIndex3(largestExtent); -// PxU32 e1 = PxGetNextIndex3(e0); -// if (e[e0] < e[e1]) -// PxSwap(e0, e1); - -// PxVec3 extents(FLT_MAX, FLT_MAX, FLT_MAX); - -// // find the largest box along the largest extent, with given internal radius -// for (PxU32 i = 0; i < mesh.getNbPolygons(); i++) { -// PxHullPolygon poly; -// mesh.getPolygonData(i, poly); -// PxPlane plane; -// PxVec3 N = PxVec3(poly.mPlane[0], poly.mPlane[1], poly.mPlane[2]); -// float D = poly.mPlane[3]; -// const float d = N[largestExtent]; -// if ((-epsilon < d && d < epsilon)) -// continue; - -// const float numBase = -D - N.dot(cm); -// const float denBase = 1.0f / N[largestExtent]; -// const float numn0 = r * N[e0]; -// const float numn1 = r * N[e1]; - -// float num = numBase - numn0 - numn1; -// float ext = PxMax(fabsf(num * denBase), r); -// if (ext < extents[largestExtent]) -// extents[largestExtent] = ext; - -// num = numBase - numn0 + numn1; -// ext = PxMax(fabsf(num * denBase), r); -// if (ext < extents[largestExtent]) -// extents[largestExtent] = ext; - -// num = numBase + numn0 + numn1; -// ext = PxMax(fabsf(num * denBase), r); -// if (ext < extents[largestExtent]) -// extents[largestExtent] = ext; - -// num = numBase + numn0 - numn1; -// ext = PxMax(fabsf(num * denBase), r); -// if (ext < extents[largestExtent]) -// extents[largestExtent] = ext; -// } - -// // Refine the box along e0,e1 -// for (PxU32 i = 0; i < mesh.getNbPolygons(); i++) { -// PxHullPolygon poly; -// mesh.getPolygonData(i, poly); -// PxVec3 N = PxVec3(poly.mPlane[0], poly.mPlane[1], poly.mPlane[2]); -// float D = poly.mPlane[3]; - -// const float denumAdd = N[e0] + N[e1]; -// const float denumSub = N[e0] - N[e1]; - -// const float numBase = -D - N.dot(cm); -// const float numn0 = extents[largestExtent] * N[largestExtent]; - -// if (!(-epsilon < denumAdd && denumAdd < epsilon)) { -// float num = numBase - numn0; -// float ext = PxMax(fabsf(num / denumAdd), r); -// if (ext < extents[e0]) -// extents[e0] = ext; - -// num = numBase + numn0; -// ext = PxMax(fabsf(num / denumAdd), r); -// if (ext < extents[e0]) -// extents[e0] = ext; -// } - -// if (!(-epsilon < denumSub && denumSub < epsilon)) { -// float num = numBase - numn0; -// float ext = PxMax(fabsf(num / denumSub), r); -// if (ext < extents[e0]) -// extents[e0] = ext; - -// num = numBase + numn0; -// ext = PxMax(fabsf(num / denumSub), r); -// if (ext < extents[e0]) -// extents[e0] = ext; -// } -// } -// extents[e1] = extents[e0]; -// return extents; -// } - -// std::vector> PhysxConvexMesh::chopUp() { -// if (mMesh->isGpuCompatible()) { -// throw std::runtime_error("mesh is already gpu compatible"); -// } - -// float mass; -// PxMat33 inertia; -// PxVec3 cm; -// mMesh->getMassInformation(mass, inertia, cm); - -// float radius = computeMeshRadius(*mMesh, cm); -// } - } // namespace physx } // namespace sapien diff --git a/src/physx/mesh_manager.cpp b/src/physx/mesh_manager.cpp index a176e5f8..0df56501 100644 --- a/src/physx/mesh_manager.cpp +++ b/src/physx/mesh_manager.cpp @@ -49,12 +49,19 @@ std::shared_ptr MeshManager::loadTriangleMesh(const std::stri return it->second; } - auto mesh = std::make_shared(fullPath); + auto mesh = std::make_shared(fullPath, false); mTriangleMeshRegistry[fullPath] = mesh; return mesh; } +std::shared_ptr +MeshManager::loadTriangleMeshWithSDF(const std::string &filename) { + std::string fullPath = getFullPath(filename); + auto mesh = std::make_shared(fullPath, true); + return mesh; +} + std::vector> MeshManager::loadConvexMeshGroup(const std::string &filename) { std::string fullPath = getFullPath(filename); diff --git a/src/physx/physx_default.cpp b/src/physx/physx_default.cpp index 3a646403..36985807 100644 --- a/src/physx/physx_default.cpp +++ b/src/physx/physx_default.cpp @@ -13,6 +13,7 @@ static bool gGPUEnabled{false}; static PhysxSceneConfig gSceneConfig{}; static PhysxBodyConfig gBodyConfig{}; static PhysxShapeConfig gShapeConfig{}; +static PhysxSDFShapeConfig gSDFConfig{}; static ::physx::PxgDynamicsMemoryConfig gGpuMemoryConfig{}; @@ -91,8 +92,16 @@ void PhysxDefault::EnableGPU() { gGPUEnabled = true; } -bool PhysxDefault::GetGPUEnabled() { return gGPUEnabled; } +void PhysxDefault::setSDFShapeConfig(float spacing, uint32_t subgridSize, + uint32_t numThreadsForConstruction) { + gSDFConfig.spacing = spacing; + gSDFConfig.subgridSize = subgridSize; + gSDFConfig.numThreadsForConstruction = numThreadsForConstruction; +} +void PhysxDefault::setSDFShapeConfig(PhysxSDFShapeConfig const &c) { gSDFConfig = c; } +PhysxSDFShapeConfig PhysxDefault::getSDFShapeConfig() { return gSDFConfig; } +bool PhysxDefault::GetGPUEnabled() { return gGPUEnabled; } std::string PhysxDefault::getPhysxVersion() { return PHYSX_VERSION; } } // namespace physx diff --git a/src/physx/physx_system.cpp b/src/physx/physx_system.cpp index 1dbee08a..6b650db5 100644 --- a/src/physx/physx_system.cpp +++ b/src/physx/physx_system.cpp @@ -715,6 +715,34 @@ void PhysxSystemGpu::gpuApplyRigidDynamicData(CudaArrayHandle const &indices) { mCudaEventWait.wait(mCudaStream); } +void PhysxSystemGpu::gpuApplyRigidDynamicForce() { + SAPIEN_PROFILE_FUNCTION; + checkGpuInitialized(); + if (mRigidDynamicComponents.empty()) { + return; + } + mCudaEventRecord.record(mCudaStream); + mPxScene->applyActorData(mCudaRigidDynamicForceHandle.ptr, + (PxGpuActorPair *)mCudaRigidDynamicIndexBuffer.ptr, + PxActorCacheFlag::eFORCE, mCudaRigidDynamicIndexBuffer.shape.at(0), + mCudaEventRecord.event, mCudaEventWait.event); + mCudaEventWait.wait(mCudaStream); +} + +void PhysxSystemGpu::gpuApplyRigidDynamicTorque() { + SAPIEN_PROFILE_FUNCTION; + checkGpuInitialized(); + if (mRigidDynamicComponents.empty()) { + return; + } + mCudaEventRecord.record(mCudaStream); + mPxScene->applyActorData(mCudaRigidDynamicTorqueHandle.ptr, + (PxGpuActorPair *)mCudaRigidDynamicIndexBuffer.ptr, + PxActorCacheFlag::eTORQUE, mCudaRigidDynamicIndexBuffer.shape.at(0), + mCudaEventRecord.event, mCudaEventWait.event); + mCudaEventWait.wait(mCudaStream); +} + void PhysxSystemGpu::gpuApplyArticulationRootPose() { gpuApplyArticulationRootPose(mCudaArticulationIndexBuffer.handle()); } @@ -957,14 +985,14 @@ void PhysxSystemGpu::allocateCudaBuffers() { int rigidBodyCount = rigidDynamicCount + mGpuArticulationCount * mGpuArticulationMaxLinkCount; ensureCudaDevice(); - mCudaRigidBodyBuffer = CudaArray({rigidBodyCount, 13}, "f4"); + // rigid body data buffer + mCudaRigidBodyBuffer = CudaArray({rigidBodyCount, 13}, "f4"); mCudaRigidDynamicHandle = CudaArrayHandle{.shape = {rigidDynamicCount, 13}, .strides = {52, 4}, .type = "f4", .cudaId = mCudaRigidBodyBuffer.cudaId, .ptr = (float *)mCudaRigidBodyBuffer.ptr}; - mCudaLinkHandle = CudaArrayHandle{.shape = {mGpuArticulationCount, mGpuArticulationMaxLinkCount, 13}, .strides = {mGpuArticulationMaxLinkCount * 52, 52, 4}, @@ -972,6 +1000,22 @@ void PhysxSystemGpu::allocateCudaBuffers() { .cudaId = mCudaRigidBodyBuffer.cudaId, .ptr = (float *)mCudaRigidBodyBuffer.ptr + 13 * rigidDynamicCount}; + // rigid body force torque buffer + mCudaRigidBodyForceBuffer = CudaArray({rigidBodyCount, 4}, "f4"); + mCudaRigidDynamicForceHandle = CudaArrayHandle{.shape = {rigidDynamicCount, 4}, + .strides = {16, 4}, + .type = "f4", + .cudaId = mCudaRigidBodyForceBuffer.cudaId, + .ptr = (float *)mCudaRigidBodyForceBuffer.ptr}; + // TODO: articulation link handle + mCudaRigidBodyTorqueBuffer = CudaArray({rigidBodyCount, 4}, "f4"); + mCudaRigidDynamicTorqueHandle = CudaArrayHandle{.shape = {rigidDynamicCount, 4}, + .strides = {16, 4}, + .type = "f4", + .cudaId = mCudaRigidBodyTorqueBuffer.cudaId, + .ptr = (float *)mCudaRigidBodyTorqueBuffer.ptr}; + // TODO: articulation link handle + mCudaArticulationBuffer = CudaArray({mGpuArticulationCount * mGpuArticulationMaxDof * 6}, "f4"); mCudaQposHandle = CudaArrayHandle{.shape = {mGpuArticulationCount, mGpuArticulationMaxDof}, diff --git a/src/physx/rigid_component.cpp b/src/physx/rigid_component.cpp index d11b3bf6..21eef1fa 100644 --- a/src/physx/rigid_component.cpp +++ b/src/physx/rigid_component.cpp @@ -283,8 +283,7 @@ PhysxRigidBodyComponent::attachCollision(std::shared_ptr sh mCollisionShapes.push_back(shape); shape->internalSetParent(this); - if (std::dynamic_pointer_cast(shape) || - std::dynamic_pointer_cast(shape)) { + if (std::dynamic_pointer_cast(shape)) { setAutoComputeMass(false); } if (getAutoComputeMass()) { @@ -319,8 +318,7 @@ void PhysxRigidBodyComponent::internalUpdateMass() { bool PhysxRigidBodyComponent::canAutoComputeMass() { for (auto c : mCollisionShapes) { auto type = c->getPxShape()->getGeometry().getType(); - if (type == PxGeometryType::eTRIANGLEMESH || type == PxGeometryType::eHEIGHTFIELD || - type == PxGeometryType::ePLANE) { + if (type == PxGeometryType::eHEIGHTFIELD || type == PxGeometryType::ePLANE) { return false; } }