From 5f341c7761f0bf552c4f94a0d91a5201c7ec4c18 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sun, 20 Mar 2022 20:22:56 -0400 Subject: [PATCH 01/23] Fix: python print command --- ambf_ros_modules/ambf_client/python/tests/ambf_throttle.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ambf_ros_modules/ambf_client/python/tests/ambf_throttle.py b/ambf_ros_modules/ambf_client/python/tests/ambf_throttle.py index a2f917f11..8e1c1bfc9 100644 --- a/ambf_ros_modules/ambf_client/python/tests/ambf_throttle.py +++ b/ambf_ros_modules/ambf_client/python/tests/ambf_throttle.py @@ -53,7 +53,7 @@ cmd.n_skip_steps = 1 if len(sys.argv) > 1: - print 'Clock Rate Specified as {}'.format(sys.argv[1]) + print('Clock Rate Specified as {}'.format(sys.argv[1])) clock_rate = float(sys.argv[1]) else: clock_rate = 50 From 9088d3eb39c99dc9f4b5cace8b961ae1bc18c5a0 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sun, 20 Mar 2022 20:23:16 -0400 Subject: [PATCH 02/23] Fix: enable comm in worldComm plugin --- ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp index 97c78aa1f..d29f14f04 100644 --- a/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp +++ b/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp @@ -20,6 +20,7 @@ int afWorldCommunicationPlugin::init(const afWorldPtr a_afWorld, const afWorldAt bool success = false; m_afWorldCommPtr.reset(new ambf_comm::World(objName, objNamespace, minFreq, maxFreq, timeOut)); + m_afWorldCommPtr->enableComm(); success = true; return success; From a1c30f65a24675c7817b41641f0638f95e36c5eb Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sun, 20 Mar 2022 20:25:16 -0400 Subject: [PATCH 03/23] Fix: only update volumes from graphics loop --- ambf_framework/afFramework.cpp | 16 +++++++++++++++- ambf_framework/afFramework.h | 11 +++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index e5fb5ac5b..9c3345e31 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -8849,7 +8849,7 @@ void afNoiseModel::createFromAttribs(afNoiseModelAttribs *a_attribs) /// afVolume::afVolume(afWorldPtr a_afWorld, afModelPtr a_modelPtr): afBaseObject(afType::VOLUME, a_afWorld, a_modelPtr) { - + clearResetFlag(); } @@ -8962,15 +8962,29 @@ void afVolume::update(double dt) { } +void afVolume::updateSceneObjects() +{ + if (m_resetFlag){ + resetTextures(); + } + afBaseObject::updateSceneObjects(); +} + /// /// \brief afVolume::reset /// void afVolume::reset() +{ + setResetFlag(); +} + +void afVolume::resetTextures() { cTexture3dPtr tex = copy3DTexture(m_originalTextureCopy); m_voxelObject->setTexture(tex); tex->markForUpdate(); afBaseObject::reset(); + clearResetFlag(); } /// diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index 67cf571a6..72a25c819 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -2521,8 +2521,12 @@ class afVolume: public afBaseObject{ virtual void update(double dt); + virtual void updateSceneObjects(); + virtual void reset(); + void resetTextures(); + virtual cShaderProgramPtr getShaderProgram(); virtual void setShaderProgram(cShaderProgramPtr a_program); @@ -2557,6 +2561,10 @@ class afVolume: public afBaseObject{ static cTexture3dPtr copy3DTexture(cTexture1dPtr tex3D); + void setResetFlag(){m_resetFlag = true;} + + void clearResetFlag(){m_resetFlag = false;} + protected: afVolumeAttributes m_attribs; cVoxelObject* m_voxelObject; @@ -2574,6 +2582,9 @@ class afVolume: public afBaseObject{ // a sub block of the volume. cVector3d m_minCornerInitial; cVector3d m_maxCornerInitial; + + // Should not reset the volume from physics thread, only from graphics thread. This flag is for that purpose. + bool m_resetFlag; }; From 5b04d11e3195f88887a4f2077744385eb3d1193a Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sun, 20 Mar 2022 20:44:33 -0400 Subject: [PATCH 04/23] Fix: reset didn't always work on all bodies. --- ambf_framework/afFramework.cpp | 34 +++++++++++++++++---------- ambf_framework/afFramework.h | 14 ++++++++++- ambf_simulator/src/ambf_simulator.cpp | 25 ++++++++++++++------ 3 files changed, 53 insertions(+), 20 deletions(-) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index 9c3345e31..212563e72 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -5850,6 +5850,7 @@ void afWorld::setGlobalNamespace(string a_global_namespace){ /// \brief afWorld::resetCameras /// void afWorld::resetCameras(){ + cerr << "INFO! RESETTING ALL CAMERAS IN THE WORLD " << endl; afBaseObjectMap::iterator camIt; for (camIt = getCameraMap()->begin() ; camIt != getCameraMap()->end() ; ++camIt){ camIt->second->reset(); @@ -5857,28 +5858,26 @@ void afWorld::resetCameras(){ } + /// /// \brief afWorld::resetWorld /// \param reset_time /// -void afWorld::resetDynamicBodies(bool reset_time){ - pausePhysics(true); - +void afWorld::resetDynamicBodies(){ + cerr << "INFO! RESETTING ALL BODIES IN THE WORLD " << endl; afBaseObjectMap::iterator rbIt; - for (rbIt = getRigidBodyMap()->begin() ; rbIt != getRigidBodyMap()->end() ; ++rbIt){ rbIt->second->reset(); } - - if (reset_time){ - // s_bulletWorld->setSimulationTime(0.0); - } - - pausePhysics(false); + clearResetBodiesFlag(); } -void afWorld::reset() -{ + +/// +/// \brief afWorld::reset +/// +void afWorld::reset(){ + cerr << "INFO! RESETTING WORLD" << endl; pausePhysics(true); for (afModelMap::iterator mIt = m_modelsMap.begin() ; mIt != m_modelsMap.end() ; ++mIt){ (mIt->second)->reset(); @@ -5886,6 +5885,7 @@ void afWorld::reset() // Call the reset for all plugins pluginsReset(); + clearResetFlag(); pausePhysics(false); } @@ -5926,6 +5926,16 @@ void afWorld::updateDynamics(double a_interval, double a_wallClock, double a_loo // sanity check if (a_interval <= 0) { return; } + if (m_resetFlag){ + reset(); + return; + } + + if (m_resetBodiesFlag){ + resetDynamicBodies(); + return; + } + if (m_pausePhx){ if (m_manualStepPhx > 0){ m_manualStepPhx--; diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index 72a25c819..780ddb5ae 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -2148,7 +2148,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ void resetCameras(); - void resetDynamicBodies(bool reset_time=false); + void resetDynamicBodies(); void reset(); @@ -2230,6 +2230,14 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ int getNumDevices(){return m_numDevices;} + void setResetFlag(){m_resetFlag = true;} + + void clearResetFlag(){m_resetFlag = false;} + + void setResetBodiesFlag(){m_resetBodiesFlag = true;} + + void clearResetBodiesFlag(){m_resetBodiesFlag = false;} + public: GLFWwindow* m_mainWindow; @@ -2377,6 +2385,10 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ cWorld* m_chaiWorld = nullptr; bool m_headless = false; + + bool m_resetFlag = false; + + bool m_resetBodiesFlag = false; }; diff --git a/ambf_simulator/src/ambf_simulator.cpp b/ambf_simulator/src/ambf_simulator.cpp index cd254982e..1448149c8 100644 --- a/ambf_simulator/src/ambf_simulator.cpp +++ b/ambf_simulator/src/ambf_simulator.cpp @@ -132,6 +132,10 @@ bool g_enableGrippingAssist = true; bool g_enableNormalMapping = true; +bool g_resetFlag = false; + +bool g_bodiesResetFlag = false; + // haptic thread std::vector g_hapticsThreads; @@ -605,6 +609,16 @@ void updatePhysics(){ torque_prev.set(0, 0, 0); while(g_simulationRunning) { + if (g_resetFlag){ + g_afWorld->reset(); + g_pluginManager.reset(); + g_resetFlag = false; + } + + if (g_bodiesResetFlag){ + g_afWorld->resetDynamicBodies(); + g_bodiesResetFlag = false; + } g_afWorld->m_freqCounterHaptics.signal(1); // Take care of any picked body by mouse @@ -1061,8 +1075,8 @@ void keyCallback(GLFWwindow* a_window, int a_key, int a_scancode, int a_action, // MODS IF THE CTRL KEY IS PRESSED // option - If CTRL R is pressed, reset the simulation if (a_key == GLFW_KEY_R){ - printf("Resetting Rigid Bodies\n"); - g_afWorld->resetDynamicBodies(); + printf("Setting bodies reset flag\n"); + g_bodiesResetFlag = true; // Reset the clutched position of all Physical devices to their // simulated dynamic end-effectors @@ -1142,11 +1156,8 @@ void keyCallback(GLFWwindow* a_window, int a_key, int a_scancode, int a_action, else if (a_mods == GLFW_MOD_ALT){ // option - Toogle visibility of body frames and softbody skeleton if (a_key == GLFW_KEY_R){ - printf("Resetting The World\n"); - g_afWorld->reset(); - g_afWorld->pausePhysics(true); - g_pluginManager.reset(); - g_afWorld->pausePhysics(false); + printf("Setting world reset flag \n"); + g_resetFlag = true; } } else{ From 2f3df82191ac98d06f87bc21be146fadb65e10d7 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sun, 20 Mar 2022 20:45:08 -0400 Subject: [PATCH 05/23] Adding capability to reset world and bodies from world comm obj --- .../core/ros_comm_plugin/WorldCommPlugin.cpp | 12 ++++++++++++ .../include/ambf_server/WorldRosCom.h | 17 +++++++++++++++++ .../ambf_server/src/WorldRosCom.cpp | 19 +++++++++++++++++++ 3 files changed, 48 insertions(+) diff --git a/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp index d29f14f04..f369d3a19 100644 --- a/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp +++ b/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp @@ -88,6 +88,18 @@ void afWorldCommunicationPlugin::worldFetchCommand(afWorldPtr worldPtr, double) m_read_count = 0; } + if (m_afWorldCommPtr->get_reset_flag()){ + std::cerr << "INFO! RESET CALLED FROM WORLD COMM" << std::endl; + m_worldPtr->setResetFlag(); + m_afWorldCommPtr->clear_reset_flag(); + } + + if (m_afWorldCommPtr->get_reset_bodies_flag()){ + std::cerr << "INFO! RESET BODIES CALLED FROM WORLD COMM" << std::endl; + m_worldPtr->setResetBodiesFlag(); + m_afWorldCommPtr->clear_reset_bodies_flag(); + } + } void afWorldCommunicationPlugin::worldUpdateState(afWorldPtr worldPtr, double dt) diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/WorldRosCom.h b/ambf_ros_modules/ambf_server/include/ambf_server/WorldRosCom.h index f508af309..4994059e5 100644 --- a/ambf_ros_modules/ambf_server/include/ambf_server/WorldRosCom.h +++ b/ambf_ros_modules/ambf_server/include/ambf_server/WorldRosCom.h @@ -46,12 +46,19 @@ #include "ambf_server/RosComBase.h" #include "ambf_msgs/WorldState.h" #include "ambf_msgs/WorldCmd.h" +#include class WorldRosCom: public RosComBase{ public: WorldRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); virtual void init(); + bool get_reset_flag(){return m_resetFlag;} + void clear_reset_flag(){m_resetFlag = false;} + + bool get_reset_bodies_flag(){return m_resetBodiesFlag;} + void clear_reset_bodies_flag(){m_resetBodiesFlag = false;} + protected: bool m_enableSimThrottle; bool m_stepSim; @@ -59,6 +66,16 @@ class WorldRosCom: public RosComBase int m_skip_steps_ctr; virtual void reset_cmd(); void sub_cb(ambf_msgs::WorldCmdConstPtr msg); + +private: + bool m_resetFlag; + bool m_resetBodiesFlag; + + ros::Subscriber m_resetBodiesSub; + ros::Subscriber m_resetSub; + + void reset_cb(std_msgs::EmptyConstPtr); + void reset_bodies_cb(std_msgs::EmptyConstPtr); }; diff --git a/ambf_ros_modules/ambf_server/src/WorldRosCom.cpp b/ambf_ros_modules/ambf_server/src/WorldRosCom.cpp index 8d740b960..5fb5515ad 100644 --- a/ambf_ros_modules/ambf_server/src/WorldRosCom.cpp +++ b/ambf_ros_modules/ambf_server/src/WorldRosCom.cpp @@ -62,9 +62,13 @@ void WorldRosCom::init(){ m_State.sim_step = 0; m_enableSimThrottle = false; m_stepSim = true; + m_resetFlag = false; + m_resetBodiesFlag = false; m_pub = nodePtr->advertise("/" + m_namespace + "/" + m_name + "/State", 10); m_sub = nodePtr->subscribe("/" + m_namespace + "/" + m_name + "/Command", 10, &WorldRosCom::sub_cb, this); + m_resetSub = nodePtr->subscribe("/" + m_namespace + "/" + m_name + "/Command/Reset", 1, &WorldRosCom::reset_cb, this); + m_resetBodiesSub = nodePtr->subscribe("/" + m_namespace + "/" + m_name + "/Command/Reset/Bodies", 1, &WorldRosCom::reset_bodies_cb, this); m_thread = boost::thread(boost::bind(&WorldRosCom::run_publishers, this)); std::cerr << "INFO! Thread Joined: " << m_name << std::endl; @@ -99,3 +103,18 @@ void WorldRosCom::sub_cb(ambf_msgs::WorldCmdConstPtr msg){ } m_watchDogPtr->acknowledge_wd(); } + + +/// +/// \brief WorldRosCom::reset_cb +/// +void WorldRosCom::reset_cb(std_msgs::EmptyConstPtr){ + m_resetFlag = true; +} + +/// +/// \brief WorldRosCom::reset_bodies_cb +/// +void WorldRosCom::reset_bodies_cb(std_msgs::EmptyConstPtr){ + m_resetBodiesFlag = true; +} From 735e4e5c95b1892e48bec95bd6b4ec30e5e4c6c0 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sun, 20 Mar 2022 21:45:30 -0400 Subject: [PATCH 06/23] Adding the ability to reset the world or the bodies --- ambf_ros_modules/ambf_client/python/ambf_client.py | 5 +++++ ambf_ros_modules/ambf_client/python/ambf_world.py | 10 +++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/ambf_ros_modules/ambf_client/python/ambf_client.py b/ambf_ros_modules/ambf_client/python/ambf_client.py index ff73ac746..99d566b23 100755 --- a/ambf_ros_modules/ambf_client/python/ambf_client.py +++ b/ambf_ros_modules/ambf_client/python/ambf_client.py @@ -51,6 +51,7 @@ from ambf_msgs.msg import WorldState, WorldCmd from ambf_msgs.msg import SensorState, SensorCmd from ambf_msgs.msg import VehicleState, VehicleCmd +from std_msgs.msg import Empty import threading from geometry_msgs.msg import WrenchStamped from ambf_actuator import Actuator @@ -125,6 +126,10 @@ def create_objs_from_rostopics(self): world_obj._sub = rospy.Subscriber(topic_name, WorldState, world_obj.ros_cb) world_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=WorldCmd, queue_size=10) + world_obj._reset_pub = rospy.Publisher(name=topic_name.replace('/State', '/Command/Reset'), + data_class=Empty, queue_size=1) + world_obj._reset_bodies_pub = rospy.Publisher( + name=topic_name.replace('/State', '/Command/Reset/Bodies'), data_class=Empty, queue_size=1) self._world_handle = world_obj self._objects_dict[world_obj.get_name()] = world_obj elif msg_type == 'ambf_msgs/ActuatorState': diff --git a/ambf_ros_modules/ambf_client/python/ambf_world.py b/ambf_ros_modules/ambf_client/python/ambf_world.py index db6add052..3203dd407 100644 --- a/ambf_ros_modules/ambf_client/python/ambf_world.py +++ b/ambf_ros_modules/ambf_client/python/ambf_world.py @@ -44,7 +44,7 @@ from ambf_msgs.msg import WorldState, WorldCmd from watch_dog import WatchDog - +from std_msgs.msg import Empty class World(WatchDog): def __init__(self, a_name): @@ -54,6 +54,8 @@ def __init__(self, a_name): self._cmd = WorldCmd() self._cmd.enable_step_throttling = False self._pub = None + self._reset_pub = None + self._reset_bodies_pub = None self._sub = None self._pub_flag = True self._active = False @@ -96,3 +98,9 @@ def run_publisher(self): self.console_print('World') self.clear_cmd() self._pub.publish(self._cmd) + + def reset(self): + self._reset_pub.publish(Empty()) + + def reset_bodies(self): + self._reset_bodies_pub.publish(Empty()) From 8602d48395e579ea55fca63216e1e3ca6c440af3 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Wed, 23 Mar 2022 19:20:58 -0400 Subject: [PATCH 07/23] FIX: Resetting the GLFW_VISIBLE flag to show subsequent visible windows --- ambf_framework/afFramework.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index 212563e72..e4ed17ec4 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -7073,6 +7073,9 @@ bool afCamera::createWindow() " VIA ITS IMAGE / DEPTH TOPICS IF THOSE ARE SET TO TRUE" << endl; glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE); } + else{ + glfwWindowHint(GLFW_VISIBLE, GLFW_TRUE); + } m_window = glfwCreateWindow(w, h, window_name.c_str(), nullptr, s_mainWindow); if (s_windowIdx == 0){ From 11557759fe2c5079dd957d9670a832f219d55121 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sat, 9 Apr 2022 16:06:57 -0400 Subject: [PATCH 08/23] Issue #163. Setting activeControllerType to Force. --- ambf_framework/afInputDevices.cpp | 6 +++++- ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ambf_framework/afInputDevices.cpp b/ambf_framework/afInputDevices.cpp index 942e3fa1e..0a5197ee6 100644 --- a/ambf_framework/afInputDevices.cpp +++ b/ambf_framework/afInputDevices.cpp @@ -640,9 +640,13 @@ bool afSimulatedDevice::createFromAttribs(afSimulatedDeviceAttribs *a_attribs) // Since an existing root body is bound to the physical device whose afComm should already be // running if(attribs.m_sdeDefined){ - std::string simDevName = "simulated_device_" + std::to_string(m_phyDev->m_CCU_Manager->s_inputDeviceCount) + modelName; + m_rootLink->setPassive(false); + m_rootLink->setNamespace(m_rootLink->getNamespace() + "/simulated_device/"); m_rootLink->loadCommunicationPlugin(m_rootLink, a_attribs); } + + // Initialize the default controller to be the force controller + m_rootLink->m_activeControllerType = afControlType::FORCE; } else{ cerr << "ERROR! FAILED TO LOAD ROOT LINK FOR MODEL " << attribs.m_modelAttribs.m_filePath.c_str() << endl; diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp index bab625786..454dce8d5 100644 --- a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp +++ b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp @@ -555,7 +555,7 @@ void afObjectCommunicationPlugin::rigidBodyFetchCommand(afRigidBodyPtr afRBPtr, // IF THE COMMAND IS OF TYPE FORCE switch (afCommand.cartesian_cmd_type) { case ambf_msgs::RigidBodyCmd::TYPE_FORCE:{ - afRBPtr->m_activeControllerType = afControlType::FORCE; + afRBPtr->m_activeControllerType = afControlType::FORCE; if (afRBPtr->m_bulletRigidBody){ force.setValue(afCommand.wrench.force.x, afCommand.wrench.force.y, From 1d72174a4dd904b20efc71fd812a8e989c8f24d0 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Mon, 11 Apr 2022 12:56:13 -0400 Subject: [PATCH 09/23] Fix: For rigid body's Position command, use the COM Transform. --- ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp index 454dce8d5..e2267fb11 100644 --- a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp +++ b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp @@ -604,7 +604,7 @@ void afObjectCommunicationPlugin::rigidBodyFetchCommand(afRigidBodyPtr afRBPtr, btMatrix3x3 cur_rot, cmd_rot; btTransform b_trans; - btRBPtr->getMotionState()->getWorldTransform(b_trans); + b_trans = afRBPtr->getCOMTransform(); cur_pos = b_trans.getOrigin(); cur_rot.setRotation(b_trans.getRotation()); From 044b3cc253d930d019f726256eec57a28bccaa49 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sat, 16 Apr 2022 18:14:28 -0400 Subject: [PATCH 10/23] Reset sensors communication message --- ambf_ros_modules/ambf_server/src/ActuatorRosCom.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ambf_ros_modules/ambf_server/src/ActuatorRosCom.cpp b/ambf_ros_modules/ambf_server/src/ActuatorRosCom.cpp index ac589f777..4317c991c 100644 --- a/ambf_ros_modules/ambf_server/src/ActuatorRosCom.cpp +++ b/ambf_ros_modules/ambf_server/src/ActuatorRosCom.cpp @@ -58,6 +58,8 @@ void ActuatorRosCom::init(){ } void ActuatorRosCom::reset_cmd(){ + m_Cmd.actuate = false; + m_Cmd.body_name.data = ""; } void ActuatorRosCom::sub_cb(ambf_msgs::ActuatorCmdConstPtr msg){ From 9020905d4edfae22857991ccf10130fa1e649e3b Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Thu, 16 Jun 2022 05:03:44 +0500 Subject: [PATCH 11/23] Should address #178 --- ambf_framework/afFramework.cpp | 52 +++++++------------ ambf_framework/afFramework.h | 18 +++---- .../include/ambf_server/RosComBase.h | 2 +- ambf_simulator/src/ambf_simulator.cpp | 13 +++-- 4 files changed, 36 insertions(+), 49 deletions(-) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index e4ed17ec4..05a5a2dbc 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -82,6 +82,7 @@ int afCamera::s_windowIdx = 0; bool afComm::s_globalOverride = false; int afComm::s_maxFreq = 1000; int afComm::s_minFreq = 50; +string afComm::s_global_namespace_prefix = ""; btGhostPairCallback* afGhostObject::m_bulletGhostPairCallback = nullptr; //------------------------------------------------------------------------------ @@ -628,6 +629,18 @@ void afComm::overrideMinPublishingFrequency(int freq) } +/// +/// \brief afComm::setGlobalNamespacePrefix +/// \param a_global_namespace_prefix +/// +void afComm::setGlobalNamespacePrefix(string a_global_namespace_prefix){ + s_global_namespace_prefix = a_global_namespace_prefix; + if (!s_global_namespace_prefix.empty()){ + cerr << " INFO! FORCE PREPENDING GLOBAL NAMESPACE PREFIX \"" << s_global_namespace_prefix << "\" \n" ; + } +} + + /// /// \brief afCartesianController::afCartesianController @@ -898,15 +911,15 @@ string afIdentification::getTypeAsStr(){ string afIdentification::getName(){return m_name;} -string afIdentification::getNamespace(){return m_namespace;} +string afIdentification::getNamespace(){return afComm::getGlobalNamespacePrefix() + m_namespace;} -string afIdentification::getQualifiedName(){return m_namespace + m_name;} +string afIdentification::getQualifiedName(){return getNamespace() + m_name;} void afIdentification::setName(string a_name){m_name = a_name;} void afIdentification::setNamespace(string a_namespace){m_namespace = a_namespace; } -string afIdentification::getQualifiedIdentifier(){return m_namespace + m_identifier;} +string afIdentification::getQualifiedIdentifier(){return getNamespace() + m_identifier;} /// @@ -3829,10 +3842,10 @@ bool afJoint::createFromAttribs(afJointAttributes *a_attribs) m_damping = attribs.m_damping; // Initialize damping to 0 // First we should search in the local Model space and if we don't find the body. - // On then we find the world space + // Only then we find the world space - string body1Name = m_namespace + m_parentName; - string body2Name = m_namespace + m_childName; + string body1Name = getNamespace() + m_parentName; + string body2Name = getNamespace() + m_childName; m_afParentBody = m_modelPtr->getRigidBody(body1Name, true); m_afChildBody = m_modelPtr->getRigidBody(body2Name, true); @@ -5645,7 +5658,7 @@ void afModelManager::addChildsSceneObjectsToWorld(afBaseObjectPtr a_object) /// \brief afWorld::afWorld /// \param a_global_namespace /// -afWorld::afWorld(string a_global_namespace): afIdentification(afType::WORLD), afModelManager(this){ +afWorld::afWorld(): afIdentification(afType::WORLD), afModelManager(this){ m_maxIterations = 10; // reset simulation time @@ -5714,7 +5727,6 @@ afWorld::afWorld(string a_global_namespace): afIdentification(afType::WORLD), af m_pickColor.setOrangeTomato(); m_pickColor.setTransparencyLevel(0.3); m_namespace = ""; - setGlobalNamespace(a_global_namespace); } afWorld::~afWorld() @@ -5822,30 +5834,6 @@ bool afWorld::loadCommunicationPlugin(afWorldPtr a_worldPtr, afWorldAttribsPtr a } -/// -/// \brief afWorld::getFullyQualifiedName -/// \param a_name -/// \return -/// -string afWorld::resolveGlobalNamespace(string a_name){ - string fully_qualified_name = getGlobalNamespace() + a_name; - fully_qualified_name = afUtils::removeAdjacentBackSlashes(fully_qualified_name); - return fully_qualified_name; -} - - -/// -/// \brief afWorld::setGlobalNamespace -/// \param a_global_namespace -/// -void afWorld::setGlobalNamespace(string a_global_namespace){ - m_global_namespace = a_global_namespace; - if (!m_global_namespace.empty()){ - cerr << " INFO! FORCE PREPENDING GLOBAL NAMESPACE \"" << m_global_namespace << "\" \n" ; - } -} - - /// /// \brief afWorld::resetCameras /// diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index 780ddb5ae..e87bc89d5 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -280,6 +280,10 @@ class afComm{ // Override the Min Freq static void overrideMinPublishingFrequency(int freq); + static string getGlobalNamespacePrefix(){return s_global_namespace_prefix;} + + static void setGlobalNamespacePrefix(string a_namespace_prefix); + private: // Min publishing frequency uint m_minPubFreq=50; @@ -294,6 +298,8 @@ class afComm{ static int s_maxFreq; static int s_minFreq; + static string s_global_namespace_prefix; + double m_timeStamp = 0.0; }; @@ -2105,7 +2111,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ friend class afModel; public: - afWorld(string a_global_namespace); + afWorld(); virtual ~afWorld(); @@ -2206,12 +2212,6 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ // defined in the bodies void buildCollisionGroups(); - string resolveGlobalNamespace(string a_name); - - string getGlobalNamespace(){return m_global_namespace;} - - void setGlobalNamespace(string a_namespace); - bool pickBody(const cVector3d& rayFromWorld, const cVector3d& rayToWorld); bool movePickedBody(const cVector3d& rayFromWorld, const cVector3d& rayToWorld); @@ -2340,10 +2340,6 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ protected: - // If this string is set, it will force itself to preeced all nampespaces - // regardless of whether any namespace starts with a '/' or not. - string m_global_namespace; - // Current time of simulation. double m_simulationTime; diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h b/ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h index 3f2aac2fc..994990725 100644 --- a/ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h +++ b/ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h @@ -64,7 +64,7 @@ class afROSNode{ if (s_initialized == false){ int argc = 0; char **argv = 0; - ros::init(argc, argv, "ambf_comm_node"); + ros::init(argc, argv, "ambf_comm_node", ros::init_options::AnonymousName); s_nodePtr = new ros::NodeHandle; s_initialized = true; std::cerr << "INFO! INITIALIZING ROS NODE HANDLE\n"; diff --git a/ambf_simulator/src/ambf_simulator.cpp b/ambf_simulator/src/ambf_simulator.cpp index 1448149c8..d90e007ed 100644 --- a/ambf_simulator/src/ambf_simulator.cpp +++ b/ambf_simulator/src/ambf_simulator.cpp @@ -98,7 +98,7 @@ struct CommandLineOptions{ // Control whether to run headless or not bool showGUI; // // Override the default world namespace - std::string prepend_namespace; + std::string namespace_prefix; // The running speed of the simulation. 1.0 indicates a stepping of one second. double simulation_speed; @@ -247,7 +247,7 @@ int main(int argc, char* argv[]) ("override_max_comm_freq", p_opt::value(), "Override the maximum publishing frequency for all afObjects (default: 1000 Hz)") ("override_min_comm_freq", p_opt::value(), "Override the minimum publishing frequency for all afObjects (default: 50 Hz)") ("show_gui,g", p_opt::value()->default_value(true), "Show GUI") - ("ns", p_opt::value()->default_value(""), "Override the default (or specified in ADF) world namespace") + ("ns", p_opt::value()->default_value(""), "Global namespace prefix for ROS Communication") ("sim_speed_factor,s", p_opt::value()->default_value(1.0), "Override the speed of \"NON REAL-TIME\" simulation by a specified factor (Default 1.0)") ("plugins,", p_opt::value()->default_value(""), "Simulator plugins to load, .e.g. " "--plugins , loads plugin1 and plugin2 simualtor plugin") @@ -272,7 +272,7 @@ int main(int argc, char* argv[]) g_cmdOpts.useFixedHtxTimeStep = var_map["fixed_htx_timestep"].as(); g_cmdOpts.enableForceFeedback = var_map["enableforces"].as(); g_cmdOpts.showGUI = var_map["show_gui"].as(); - g_cmdOpts.prepend_namespace = var_map["ns"].as(); + g_cmdOpts.namespace_prefix = var_map["ns"].as(); g_cmdOpts.simulation_speed = var_map["sim_speed_factor"].as(); g_cmdOpts.simulator_plugins = var_map["plugins"].as(); @@ -340,7 +340,7 @@ int main(int argc, char* argv[]) glfwSetErrorCallback(errorCallback); // set OpenGL version - glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2); + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1); // set active stereo mode @@ -354,6 +354,8 @@ int main(int argc, char* argv[]) } } + cerr << "GLFW VERSION: " << glfwGetVersionString() << endl; + //----------------------------------------------------------------------- // 3D - SCENEGRAPH @@ -395,7 +397,8 @@ int main(int argc, char* argv[]) g_adfLoaderPtr->loadTeleRoboticUnitsAttribs(launchAttribs.m_inputDevicesFilepath.c_str(), &tuAttribs, devIndexes); // create a dynamic world. - g_afWorld = new afWorld(g_cmdOpts.prepend_namespace); + afComm::setGlobalNamespacePrefix(g_cmdOpts.namespace_prefix); + g_afWorld = new afWorld(); g_afWorld->m_physicsFrequency = g_cmdOpts.phxFrequency; g_afWorld->m_hapticsFrequency = g_cmdOpts.htxFrequency; g_afWorld->m_updateCounterLimit = g_cmdOpts.phxFrequency * 2; From dd588fdc3013d783d09f74be760f61432024e48b Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 19 Jul 2022 14:38:59 -0400 Subject: [PATCH 12/23] Fixed inconsistency in rotation between 'get_rpy' and 'get_pose' --- ambf_ros_modules/ambf_client/python/ambf_base_object.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ambf_ros_modules/ambf_client/python/ambf_base_object.py b/ambf_ros_modules/ambf_client/python/ambf_base_object.py index e4a36f733..ba842e71c 100755 --- a/ambf_ros_modules/ambf_client/python/ambf_base_object.py +++ b/ambf_ros_modules/ambf_client/python/ambf_base_object.py @@ -141,9 +141,9 @@ def get_pose(self): quat = self._state.pose.orientation explicit_quat = [quat.x, quat.y, quat.z, quat.w] # Edited python3 code - rpy = euler_from_quaternion(explicit_quat, 'szyx') + rpy = euler_from_quaternion(explicit_quat) # Initial python2 code - # rpy = transformations.euler_from_quaternion(explicit_quat, 'szyx') + # rpy = transformations.euler_from_quaternion(explicit_quat) pose = [self._state.pose.position.x, self._state.pose.position.y, self._state.pose.position.z, From 8866caf29255390472c3f25c36025153649b2971 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Fri, 3 Nov 2023 03:33:25 +0500 Subject: [PATCH 13/23] Implement merging duplicate vertices --- external/chai3d/src/world/CMesh.cpp | 137 +++++++++++++++++++++++ external/chai3d/src/world/CMesh.h | 3 + external/chai3d/src/world/CMultiMesh.cpp | 9 ++ external/chai3d/src/world/CMultiMesh.h | 3 + 4 files changed, 152 insertions(+) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index f2e019679..c26d55b75 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -973,6 +973,143 @@ void cMesh::clearAllEdges() } + +class afMeshWeldingSpecs{ +public: + afMeshWeldingSpecs(cVector3d a_minBounds, cVector3d a_maxBounds, double a_weldingThreshold){ + m_weldindThreshold = a_weldingThreshold; + m_minBounds = a_minBounds; + m_maxBounds = a_maxBounds; + cVector3d deltaBounds = m_maxBounds - m_minBounds; + m_deltaBoundsX = deltaBounds.x(); + m_deltaBoundsXY = m_deltaBoundsX * deltaBounds.y(); + m_deltaBoundsXYZ = m_deltaBoundsXY * deltaBounds.z(); + } + + double m_weldindThreshold; + cVector3d m_minBounds; + cVector3d m_maxBounds; + double m_deltaBoundsX; + double m_deltaBoundsXY; + double m_deltaBoundsXYZ; +}; + +class afTriVertex{ +public: + afTriVertex(const cVector3d& v, const uint &idx, const afMeshWeldingSpecs* a_weldingSpecs): m_weldingSpecs(a_weldingSpecs){ + m_x=v(0); m_y=v(1); m_z=v(2); m_idx=idx; + m_weldingSpecs = a_weldingSpecs; + computeHash(); + } + + bool operator==(const afTriVertex& rhs) const + { + if (this->m_x == rhs.m_x && this->m_y == rhs.m_y && this->m_z == rhs.m_z) return true; + else return false; + } + + void computeHash(){ + m_hash = (m_weldingSpecs->m_minBounds.x() + m_x) + + m_weldingSpecs->m_deltaBoundsX * (m_weldingSpecs->m_minBounds.y() + m_y) + + m_weldingSpecs->m_deltaBoundsXY * (m_weldingSpecs->m_minBounds.z() + m_z); + } + + const double& getHash() const{ + return m_hash; + + } + + bool operator<(const afTriVertex& rhs) const + { + return (getHash() < rhs.getHash()); + } + +public: + double m_x; + double m_y; + double m_z; + double m_hash; + const afMeshWeldingSpecs* m_weldingSpecs; + uint m_idx; +}; + + +//============================================================================== +/*! + This method removes all duplicate/repeated vertices. +*/ +//============================================================================== +bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) +{ + bool res = false; + set rMesh; + vector nIndices; nIndices.resize(m_triangles->m_indices.size()); + vector oUniqueIndexes; + uint insIdx = 0; + computeBoundaryBox(); + afMeshWeldingSpecs weldingSpecs(getBoundaryMin(), getBoundaryMax(), a_weldingThreshold); + + for(int i = 0 ; i < m_triangles->m_indices.size() ; i++){ + uint oIdx = m_triangles->m_indices[i]; + cVector3d v = m_triangles->m_vertices->getLocalPos(oIdx); + pair::iterator, bool> insIt = rMesh.insert(afTriVertex(v, insIdx, &weldingSpecs)); + uint nIdx; + if (insIt.second){ + nIdx = insIdx; + oUniqueIndexes.push_back(oIdx); + insIdx++; + } + else{ + nIdx = insIt.first->m_idx; + } + nIndices[i] = nIdx; + } + + int oS = getNumVertices(); + int nS = oUniqueIndexes.size(); + + if (nS < oS){ + cMesh* nMesh = new cMesh(); + // nMesh->m_vertices->allocateData(nS, true, false, false, false, false, false); + nMesh->m_triangles->m_vertices->allocateData(oUniqueIndexes.size(), true, true, true, true, true, false); + + for (int i = 0 ; i < nS ; i++){ + uint oIdx = oUniqueIndexes[i]; + + cVector3d position = m_vertices->getLocalPos(oIdx); + cVector3d normal = m_vertices->getNormal(oIdx); + cVector3d tex_coord = m_vertices->getTexCoord(oIdx); + cVector3d tangent = m_vertices->getTangent(oIdx); + cVector3d bit_tangent = m_vertices->getBitangent(oIdx); + + nMesh->m_vertices->setLocalPos(i, position); + nMesh->m_vertices->setNormal(i, normal); + nMesh->m_vertices->setTexCoord(i, tex_coord); + nMesh->m_vertices->setTangent(i, tangent); + nMesh->m_vertices->setBitangent(i, bit_tangent); + } + + cerr << "INFO! *** Original Mesh Size: " << getNumVertices() << endl; + cerr << "INFO! *** -----New Mesh Size: " << nMesh->getNumVertices() << endl; + + for (int i = 0 ; i < nIndices.size() ; i=i+3){ + nMesh->newTriangle(nIndices[i], nIndices[i+1], nIndices[i+2]); + } + + m_vertices->clear(); + m_vertices = nMesh->m_vertices->copy(); + + m_triangles->clear(); + m_triangles = nMesh->m_triangles->copy(); + + computeAllNormals(); + + res = true; + } + return res; +} + + //============================================================================== /*! This method sets the graphic properties for edge-rendering. diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index 978041156..b5eb62d3e 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -240,6 +240,9 @@ class cMesh : public cGenericObject //! This method clears all edges void clearAllEdges(); + //! This method removes duplicate vertices and updates triangles indices. + bool removeDuplicateVertices(double& a_weldingThreshold); + //! This method enables or disables the rendering of edges. void setShowEdges(const bool a_showEdges) { m_showEdges = a_showEdges; } diff --git a/external/chai3d/src/world/CMultiMesh.cpp b/external/chai3d/src/world/CMultiMesh.cpp index 0af6e716f..fdfb66d24 100644 --- a/external/chai3d/src/world/CMultiMesh.cpp +++ b/external/chai3d/src/world/CMultiMesh.cpp @@ -1661,6 +1661,15 @@ bool cMultiMesh::loadFromFile(string a_filename) return (result); } +bool cMultiMesh::removeDuplicateVertices(double a_weldingThreshold) +{ + bool res = false; + for (int i=0; i < m_meshes->size() ; i++){ + res |= getMesh(i)->removeDuplicateVertices(a_weldingThreshold); + }; + return res; +} + //============================================================================== /*! diff --git a/external/chai3d/src/world/CMultiMesh.h b/external/chai3d/src/world/CMultiMesh.h index 05b708814..313e396c2 100644 --- a/external/chai3d/src/world/CMultiMesh.h +++ b/external/chai3d/src/world/CMultiMesh.h @@ -414,6 +414,9 @@ class cMultiMesh : public cGenericObject //! This method loads a 3D object from a file. virtual bool loadFromFile(std::string a_filename); + //! This method finds and remove duplicate vertices and updates mesh indices + virtual bool removeDuplicateVertices(double a_weldingThreshold=0.001); + //! This method saves 3D object to a file. virtual bool saveToFile(std::string a_filename); From 461f6a0f9bab21b04238f68434cc468ea9326acc Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Fri, 10 Nov 2023 05:39:50 +0500 Subject: [PATCH 14/23] Maintain a tree of duplicate vertices --- external/chai3d/src/world/CMesh.cpp | 83 ++++++++++++++++------------- external/chai3d/src/world/CMesh.h | 25 +++++++++ 2 files changed, 72 insertions(+), 36 deletions(-) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index c26d55b75..906f59ba9 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -1041,40 +1041,16 @@ class afTriVertex{ //============================================================================== bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) { - bool res = false; - set rMesh; - vector nIndices; nIndices.resize(m_triangles->m_indices.size()); - vector oUniqueIndexes; - uint insIdx = 0; - computeBoundaryBox(); - afMeshWeldingSpecs weldingSpecs(getBoundaryMin(), getBoundaryMax(), a_weldingThreshold); + bool res = findDuplicateVertices(a_weldingThreshold); - for(int i = 0 ; i < m_triangles->m_indices.size() ; i++){ - uint oIdx = m_triangles->m_indices[i]; - cVector3d v = m_triangles->m_vertices->getLocalPos(oIdx); - pair::iterator, bool> insIt = rMesh.insert(afTriVertex(v, insIdx, &weldingSpecs)); - uint nIdx; - if (insIt.second){ - nIdx = insIdx; - oUniqueIndexes.push_back(oIdx); - insIdx++; - } - else{ - nIdx = insIt.first->m_idx; - } - nIndices[i] = nIdx; - } - - int oS = getNumVertices(); - int nS = oUniqueIndexes.size(); - - if (nS < oS){ + if (res){ cMesh* nMesh = new cMesh(); + unsigned int unique_vertex_count = m_duplicateVertexIndexTree.size(); // nMesh->m_vertices->allocateData(nS, true, false, false, false, false, false); - nMesh->m_triangles->m_vertices->allocateData(oUniqueIndexes.size(), true, true, true, true, true, false); + nMesh->m_triangles->m_vertices->allocateData(unique_vertex_count, true, true, true, true, true, false); - for (int i = 0 ; i < nS ; i++){ - uint oIdx = oUniqueIndexes[i]; + for (int i = 0 ; i < unique_vertex_count ; i++){ + uint oIdx = m_duplicateVertexIndexTree[i][0].m_vertexIndex; cVector3d position = m_vertices->getLocalPos(oIdx); cVector3d normal = m_vertices->getNormal(oIdx); @@ -1092,20 +1068,55 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) cerr << "INFO! *** Original Mesh Size: " << getNumVertices() << endl; cerr << "INFO! *** -----New Mesh Size: " << nMesh->getNumVertices() << endl; - for (int i = 0 ; i < nIndices.size() ; i=i+3){ - nMesh->newTriangle(nIndices[i], nIndices[i+1], nIndices[i+2]); + vector recomputedIndices; + recomputedIndices.resize(m_triangles->m_indices.size()); + unsigned int assignedIndices = 0; + for (int i = 0 ; i < m_duplicateVertexIndexTree.size() ; i++){ + for (int j = 0 ; j < m_duplicateVertexIndexTree[i].size() ; j++){ + recomputedIndices[m_duplicateVertexIndexTree[i][j].m_vertexIndex] = i; + assignedIndices++; + } } m_vertices->clear(); m_vertices = nMesh->m_vertices->copy(); - m_triangles->clear(); - m_triangles = nMesh->m_triangles->copy(); - + m_triangles->m_indices = recomputedIndices; computeAllNormals(); + } + return res; +} + - res = true; +//============================================================================== +/*! + This method finds duplicate vertices and computes a tree of unique vertices mapping their duplicates +*/ +//============================================================================== +bool cMesh::findDuplicateVertices(double &a_weldingThreshold){ + + set rMesh; + computeBoundaryBox(); + afMeshWeldingSpecs weldingSpecs(getBoundaryMin(), getBoundaryMax(), a_weldingThreshold); + + uint insIdx = 0; + for(int i = 0 ; i < m_triangles->m_indices.size() ; i++){ + uint oIdx = m_triangles->m_indices[i]; + cVector3d v = m_triangles->m_vertices->getLocalPos(oIdx); + pair::iterator, bool> insIt = rMesh.insert(afTriVertex(v, insIdx, &weldingSpecs)); + uint nIdx; + if (insIt.second){ + nIdx = insIdx; + insIdx++; + } + else{ + nIdx = insIt.first->m_idx; + } + m_duplicateVertexIndexTree[nIdx].push_back(cIndexMapping(oIdx, i / 3)); } + + bool res = m_duplicateVertexIndexTree.size() == m_vertices->getNumElements() ? 0 : 1; + return res; } diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index b5eb62d3e..a2bf4770b 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -59,6 +59,7 @@ //------------------------------------------------------------------------------ #include #include +#include //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ @@ -92,6 +93,16 @@ struct cEdge; //------------------------------------------------------------------------------ +struct cIndexMapping{ +public: + cIndexMapping(unsigned int vertexIndex, unsigned int triangleIndex){ + m_vertexIndex = vertexIndex; + m_triangleIndex = triangleIndex; + } + unsigned int m_triangleIndex; + unsigned int m_vertexIndex; +}; + //============================================================================== /*! \class cMesh @@ -243,6 +254,9 @@ class cMesh : public cGenericObject //! This method removes duplicate vertices and updates triangles indices. bool removeDuplicateVertices(double& a_weldingThreshold); + //! Find duplicate vertices and record their indices. + bool findDuplicateVertices(double& a_weldingThreshold); + //! This method enables or disables the rendering of edges. void setShowEdges(const bool a_showEdges) { m_showEdges = a_showEdges; } @@ -425,6 +439,17 @@ class cMesh : public cGenericObject //! Array of Edges. std::vector m_edges; + + //! Tree of duplicate vertex indices, the key is the new index (after identifying duplicates) and + //! value (rhs) is the list of original indices of duplicate vertices. + //! E.g. Imagine two triangles <123 and <456 with a shared edge between with vertices 2,3 == 4,5 + //! Original Indices = [1, 2, 3, 4, 5, 6] + //! Duplicates = [1, 2, 3, 2, 3, 6] + //! Tree = {1: [1] + //! {2: [2, 4] + //! {3: [3, 5] + //! {4: [6]} + std::map > m_duplicateVertexIndexTree; }; From 3c6ba4a2807e866431db1e5aca038a25ef0d7a90 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 14 Nov 2023 01:38:22 +0500 Subject: [PATCH 15/23] Assign the vertices Ptr to the triangleArray's copy of vertices --- external/chai3d/src/world/CMesh.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index 906f59ba9..9606ef7e9 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -1081,7 +1081,9 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) m_vertices->clear(); m_vertices = nMesh->m_vertices->copy(); + m_triangles->m_indices.clear(); m_triangles->m_indices = recomputedIndices; + m_triangles->m_vertices = m_vertices; computeAllNormals(); } return res; From 3883a0c724087a8c29af874d16511c76ca89bd40 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 14 Nov 2023 05:54:30 +0500 Subject: [PATCH 16/23] Fixed algorithm --- external/chai3d/src/world/CMesh.cpp | 20 ++++++++++---------- external/chai3d/src/world/CMesh.h | 10 +++++----- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index 9606ef7e9..6906d8402 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -1050,7 +1050,7 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) nMesh->m_triangles->m_vertices->allocateData(unique_vertex_count, true, true, true, true, true, false); for (int i = 0 ; i < unique_vertex_count ; i++){ - uint oIdx = m_duplicateVertexIndexTree[i][0].m_vertexIndex; + uint oIdx = m_duplicateVertexIndexTree[i][0].m_value; cVector3d position = m_vertices->getLocalPos(oIdx); cVector3d normal = m_vertices->getNormal(oIdx); @@ -1068,19 +1068,19 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) cerr << "INFO! *** Original Mesh Size: " << getNumVertices() << endl; cerr << "INFO! *** -----New Mesh Size: " << nMesh->getNumVertices() << endl; + + m_vertices->clear(); + m_vertices = nMesh->m_vertices->copy(); + vector recomputedIndices; recomputedIndices.resize(m_triangles->m_indices.size()); - unsigned int assignedIndices = 0; - for (int i = 0 ; i < m_duplicateVertexIndexTree.size() ; i++){ - for (int j = 0 ; j < m_duplicateVertexIndexTree[i].size() ; j++){ - recomputedIndices[m_duplicateVertexIndexTree[i][j].m_vertexIndex] = i; - assignedIndices++; + std::map >::iterator it; + for (it = m_duplicateVertexIndexTree.begin() ; it != m_duplicateVertexIndexTree.end() ; ++it){ + for (int j = 0 ; j < it->second.size() ; j++){ + recomputedIndices[it->second[j].m_index] = it->first; } } - m_vertices->clear(); - m_vertices = nMesh->m_vertices->copy(); - m_triangles->m_indices.clear(); m_triangles->m_indices = recomputedIndices; m_triangles->m_vertices = m_vertices; @@ -1114,7 +1114,7 @@ bool cMesh::findDuplicateVertices(double &a_weldingThreshold){ else{ nIdx = insIt.first->m_idx; } - m_duplicateVertexIndexTree[nIdx].push_back(cIndexMapping(oIdx, i / 3)); + m_duplicateVertexIndexTree[nIdx].push_back(cIndexMapping(i, oIdx)); } bool res = m_duplicateVertexIndexTree.size() == m_vertices->getNumElements() ? 0 : 1; diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index a2bf4770b..74913b10c 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -95,12 +95,12 @@ struct cEdge; struct cIndexMapping{ public: - cIndexMapping(unsigned int vertexIndex, unsigned int triangleIndex){ - m_vertexIndex = vertexIndex; - m_triangleIndex = triangleIndex; + cIndexMapping(unsigned int index, unsigned int value){ + m_index = index; + m_value = value; } - unsigned int m_triangleIndex; - unsigned int m_vertexIndex; + unsigned int m_value; + unsigned int m_index; }; //============================================================================== From 90661e55a909dc140d02e8333368b58a515b8b9d Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 14 Nov 2023 06:05:30 +0500 Subject: [PATCH 17/23] Set only valid vertex data --- external/chai3d/src/world/CMesh.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index 6906d8402..3c3b93fe5 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -1047,22 +1047,25 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) cMesh* nMesh = new cMesh(); unsigned int unique_vertex_count = m_duplicateVertexIndexTree.size(); // nMesh->m_vertices->allocateData(nS, true, false, false, false, false, false); - nMesh->m_triangles->m_vertices->allocateData(unique_vertex_count, true, true, true, true, true, false); + nMesh->m_vertices->allocateData(unique_vertex_count, + m_vertices->getUseNormalData(), + m_vertices->getUseTexCoordData(), + m_vertices->getUseColorData(), + m_vertices->getUseTangentData(), + m_vertices->getUseBitangentData(), + m_vertices->getUseUserData()); for (int i = 0 ; i < unique_vertex_count ; i++){ uint oIdx = m_duplicateVertexIndexTree[i][0].m_value; - cVector3d position = m_vertices->getLocalPos(oIdx); - cVector3d normal = m_vertices->getNormal(oIdx); - cVector3d tex_coord = m_vertices->getTexCoord(oIdx); - cVector3d tangent = m_vertices->getTangent(oIdx); - cVector3d bit_tangent = m_vertices->getBitangent(oIdx); - - nMesh->m_vertices->setLocalPos(i, position); - nMesh->m_vertices->setNormal(i, normal); - nMesh->m_vertices->setTexCoord(i, tex_coord); - nMesh->m_vertices->setTangent(i, tangent); - nMesh->m_vertices->setBitangent(i, bit_tangent); + nMesh->m_vertices->setLocalPos(i, m_vertices->getLocalPos(oIdx)); + + if(m_vertices->getUseNormalData()) nMesh->m_vertices->setNormal(i, m_vertices->getNormal(oIdx)); + if(m_vertices->getUseTexCoordData()) nMesh->m_vertices->setTexCoord(i, m_vertices->getTexCoord(oIdx)); + if(m_vertices->getUseColorData()) nMesh->m_vertices->setColor(i, m_vertices->getColor(oIdx)); + if(m_vertices->getUseTangentData()) nMesh->m_vertices->setTangent(i, m_vertices->getTangent(oIdx)); + if(m_vertices->getUseBitangentData()) nMesh->m_vertices->setBitangent(i, m_vertices->getBitangent(oIdx)); + if(m_vertices->getUseUserData()) nMesh->m_vertices->setUserData(i, m_vertices->getUserData(oIdx)); } cerr << "INFO! *** Original Mesh Size: " << getNumVertices() << endl; From 9a7288536373df17ec83e34d2b01d6ee2bc6e823 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Wed, 15 Nov 2023 04:17:04 +0500 Subject: [PATCH 18/23] Default to false for resizing element array on every iteration --- external/chai3d/src/graphics/CTriangleArray.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/chai3d/src/graphics/CTriangleArray.h b/external/chai3d/src/graphics/CTriangleArray.h index bc031f2fa..370dbffd5 100644 --- a/external/chai3d/src/graphics/CTriangleArray.h +++ b/external/chai3d/src/graphics/CTriangleArray.h @@ -599,7 +599,7 @@ class cTriangleArray : public cGenericArray if (m_flagMarkForResize) { glBufferData(GL_ELEMENT_ARRAY_BUFFER, 3 * numtriangles * sizeof(unsigned int), &(m_indices[0]), GL_STATIC_DRAW); - m_flagMarkForResize = true; + m_flagMarkForResize = false; } // update data if needed From 02e8c0b14953717fd02930942bd6b233540f193d Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Fri, 17 Nov 2023 23:11:45 +0500 Subject: [PATCH 19/23] Added method to set vtx pos of all duplicates by single index --- external/chai3d/src/world/CMesh.cpp | 39 +++++++++++++++++++++-------- external/chai3d/src/world/CMesh.h | 15 ++++++----- 2 files changed, 35 insertions(+), 19 deletions(-) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index 3c3b93fe5..5d2b663b5 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -676,6 +676,25 @@ void cMesh::setVertexColor(const cColorf& a_color) } +//============================================================================== +/*! + Set the local pos of a vertex and all of it's duplicates + + \param a_idx index. + \param a_pos The position. +*/ +//============================================================================== +void cMesh::setVertexLocalPosForAllDuplicates(const unsigned int &a_idx, const cVector3d& a_pos){ + auto it = m_duplicateVertexIndexTree.find(a_idx); + + if (it != m_duplicateVertexIndexTree.end()){ + for (unsigned int i = 0 ; i < it->second.m_vertexIndices.size() ; i++){ + m_vertices->setLocalPos(it->second.m_vertexIndices[i], a_pos); + } + } +} + + //============================================================================== /*! This method shifts all vertex positions by the specified amount. @@ -1056,7 +1075,7 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) m_vertices->getUseUserData()); for (int i = 0 ; i < unique_vertex_count ; i++){ - uint oIdx = m_duplicateVertexIndexTree[i][0].m_value; + uint oIdx = m_duplicateVertexIndexTree[i].m_vertexIndices[0]; nMesh->m_vertices->setLocalPos(i, m_vertices->getLocalPos(oIdx)); @@ -1067,21 +1086,18 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) if(m_vertices->getUseBitangentData()) nMesh->m_vertices->setBitangent(i, m_vertices->getBitangent(oIdx)); if(m_vertices->getUseUserData()) nMesh->m_vertices->setUserData(i, m_vertices->getUserData(oIdx)); } - - cerr << "INFO! *** Original Mesh Size: " << getNumVertices() << endl; - cerr << "INFO! *** -----New Mesh Size: " << nMesh->getNumVertices() << endl; - - + printf("INFO! Original/New vertex count [%u/%u]. Removed [%u] vertices \n", getNumVertices(), nMesh->getNumVertices(), getNumVertices() - nMesh->getNumVertices()); m_vertices->clear(); m_vertices = nMesh->m_vertices->copy(); vector recomputedIndices; recomputedIndices.resize(m_triangles->m_indices.size()); - std::map >::iterator it; - for (it = m_duplicateVertexIndexTree.begin() ; it != m_duplicateVertexIndexTree.end() ; ++it){ - for (int j = 0 ; j < it->second.size() ; j++){ - recomputedIndices[it->second[j].m_index] = it->first; + for (auto it = m_duplicateVertexIndexTree.begin() ; it != m_duplicateVertexIndexTree.end() ; ++it){ + for (int j = 0 ; j < it->second.m_elementIndices.size() ; j++){ + recomputedIndices[it->second.m_elementIndices[j]] = it->first; } + it->second.m_vertexIndices.clear(); + it->second.m_vertexIndices.push_back(it->first); } m_triangles->m_indices.clear(); @@ -1117,7 +1133,8 @@ bool cMesh::findDuplicateVertices(double &a_weldingThreshold){ else{ nIdx = insIt.first->m_idx; } - m_duplicateVertexIndexTree[nIdx].push_back(cIndexMapping(i, oIdx)); + m_duplicateVertexIndexTree[nIdx].m_elementIndices.push_back(i); + m_duplicateVertexIndexTree[nIdx].m_vertexIndices.push_back(oIdx); } bool res = m_duplicateVertexIndexTree.size() == m_vertices->getNumElements() ? 0 : 1; diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index 74913b10c..e0d83d6d9 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -93,14 +93,10 @@ struct cEdge; //------------------------------------------------------------------------------ -struct cIndexMapping{ +struct cDuplicateVertexData{ public: - cIndexMapping(unsigned int index, unsigned int value){ - m_index = index; - m_value = value; - } - unsigned int m_value; - unsigned int m_index; + std::vector m_elementIndices; // From array of elements representing triangle vertices + std::vector m_vertexIndices; // From array of vertex indices }; //============================================================================== @@ -197,6 +193,9 @@ class cMesh : public cGenericObject //! This method sets the color of each vertex. void setVertexColor(const cColorf& a_color); + //! Set the local pos of a vertex and all of it's duplicates + void setVertexLocalPosForAllDuplicates(const unsigned int& a_idx, const cVector3d& a_pos); + //-------------------------------------------------------------------------- // PUBLIC METHODS - TRIANGLES @@ -449,7 +448,7 @@ class cMesh : public cGenericObject //! {2: [2, 4] //! {3: [3, 5] //! {4: [6]} - std::map > m_duplicateVertexIndexTree; + std::map m_duplicateVertexIndexTree; }; From 05b104e82f65db63fb3a917e6428fe61efcb470e Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 21 Nov 2023 04:33:40 +0500 Subject: [PATCH 20/23] Minor cleanup --- external/chai3d/src/world/CMesh.cpp | 42 +++++++++++++++++++++++- external/chai3d/src/world/CMesh.h | 11 ++++++- external/chai3d/src/world/CMultiMesh.cpp | 9 +++++ external/chai3d/src/world/CMultiMesh.h | 5 ++- 4 files changed, 64 insertions(+), 3 deletions(-) diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index 5d2b663b5..fec92b378 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -124,6 +124,10 @@ cMesh::cMesh(cMaterialPtr a_material) { m_material = a_material; } + + m_duplicateVerticesFound = false; + + m_duplicateVerticesRemoved = false; } @@ -685,6 +689,9 @@ void cMesh::setVertexColor(const cColorf& a_color) */ //============================================================================== void cMesh::setVertexLocalPosForAllDuplicates(const unsigned int &a_idx, const cVector3d& a_pos){ + if (!m_duplicateVerticesFound){ + findDuplicateVertices(); + } auto it = m_duplicateVertexIndexTree.find(a_idx); if (it != m_duplicateVertexIndexTree.end()){ @@ -695,6 +702,30 @@ void cMesh::setVertexLocalPosForAllDuplicates(const unsigned int &a_idx, const c } +//============================================================================== +/*! + Set the local pos of a vertex and all of it's duplicates + + \param a_idx index. + \param a_x x. + \param a_y y. + \param a_z z. +*/ +//============================================================================== +void cMesh::setVertexLocalPosForAllDuplicates(const unsigned int &a_idx, const double &a_x, const double &a_y, const double &a_z){ + if (!m_duplicateVerticesFound){ + findDuplicateVertices(); + } + auto it = m_duplicateVertexIndexTree.find(a_idx); + + if (it != m_duplicateVertexIndexTree.end()){ + for (unsigned int i = 0 ; i < it->second.m_vertexIndices.size() ; i++){ + m_vertices->setLocalPos(it->second.m_vertexIndices[i], a_x, a_y, a_z); + } + } +} + + //============================================================================== /*! This method shifts all vertex positions by the specified amount. @@ -1060,6 +1091,10 @@ class afTriVertex{ //============================================================================== bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) { + if (m_duplicateVerticesRemoved){ + return true; + } + bool res = findDuplicateVertices(a_weldingThreshold); if (res){ @@ -1104,6 +1139,7 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) m_triangles->m_indices = recomputedIndices; m_triangles->m_vertices = m_vertices; computeAllNormals(); + m_duplicateVerticesRemoved = true; } return res; } @@ -1114,7 +1150,10 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) This method finds duplicate vertices and computes a tree of unique vertices mapping their duplicates */ //============================================================================== -bool cMesh::findDuplicateVertices(double &a_weldingThreshold){ +bool cMesh::findDuplicateVertices(double a_weldingThreshold){ + if (m_duplicateVerticesFound){ + return true; + } set rMesh; computeBoundaryBox(); @@ -1139,6 +1178,7 @@ bool cMesh::findDuplicateVertices(double &a_weldingThreshold){ bool res = m_duplicateVertexIndexTree.size() == m_vertices->getNumElements() ? 0 : 1; + m_duplicateVerticesFound = true; return res; } diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index e0d83d6d9..dc5b9e0b0 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -196,6 +196,9 @@ class cMesh : public cGenericObject //! Set the local pos of a vertex and all of it's duplicates void setVertexLocalPosForAllDuplicates(const unsigned int& a_idx, const cVector3d& a_pos); + //! Set the local pos of a vertex and all of it's duplicates + void setVertexLocalPosForAllDuplicates(const unsigned int& a_idx, const double& a_x, const double& a_y, const double& a_z); + //-------------------------------------------------------------------------- // PUBLIC METHODS - TRIANGLES @@ -254,7 +257,7 @@ class cMesh : public cGenericObject bool removeDuplicateVertices(double& a_weldingThreshold); //! Find duplicate vertices and record their indices. - bool findDuplicateVertices(double& a_weldingThreshold); + bool findDuplicateVertices(double a_weldingThreshold=0.0); //! This method enables or disables the rendering of edges. void setShowEdges(const bool a_showEdges) { m_showEdges = a_showEdges; } @@ -449,6 +452,12 @@ class cMesh : public cGenericObject //! {3: [3, 5] //! {4: [6]} std::map m_duplicateVertexIndexTree; + + //! Flag to check if duplicate vertices have been removed + bool m_duplicateVerticesFound; + + //! Flag to check if duplicate vertices have been computed + bool m_duplicateVerticesRemoved; }; diff --git a/external/chai3d/src/world/CMultiMesh.cpp b/external/chai3d/src/world/CMultiMesh.cpp index fdfb66d24..1f2b2f9cc 100644 --- a/external/chai3d/src/world/CMultiMesh.cpp +++ b/external/chai3d/src/world/CMultiMesh.cpp @@ -1661,6 +1661,15 @@ bool cMultiMesh::loadFromFile(string a_filename) return (result); } +bool cMultiMesh::findDuplicateVertices(double a_weldingThreshold) +{ + bool res = false; + for (int i=0; i < m_meshes->size() ; i++){ + res |= getMesh(i)->findDuplicateVertices(a_weldingThreshold); + }; + return res; +} + bool cMultiMesh::removeDuplicateVertices(double a_weldingThreshold) { bool res = false; diff --git a/external/chai3d/src/world/CMultiMesh.h b/external/chai3d/src/world/CMultiMesh.h index 313e396c2..9dd264f33 100644 --- a/external/chai3d/src/world/CMultiMesh.h +++ b/external/chai3d/src/world/CMultiMesh.h @@ -415,7 +415,10 @@ class cMultiMesh : public cGenericObject virtual bool loadFromFile(std::string a_filename); //! This method finds and remove duplicate vertices and updates mesh indices - virtual bool removeDuplicateVertices(double a_weldingThreshold=0.001); + virtual bool findDuplicateVertices(double a_weldingThreshold=0.0); + + //! This method finds and remove duplicate vertices and updates mesh indices + virtual bool removeDuplicateVertices(double a_weldingThreshold=0.0); //! This method saves 3D object to a file. virtual bool saveToFile(std::string a_filename); From e29c71e8d633eb1cc0c3dea1f5d47bb7cd67008e Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Tue, 21 Nov 2023 04:35:33 +0500 Subject: [PATCH 21/23] Cleanup creating collision objects and softbody implementation --- ambf_framework/afFramework.cpp | 806 ++++++--------------------------- ambf_framework/afFramework.h | 20 +- 2 files changed, 132 insertions(+), 694 deletions(-) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index ff668a755..bbd39597a 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -231,67 +231,68 @@ btCollisionShape *afShapeUtils::createCollisionShape(const afPrimitiveShapeAttri return collisionShape; } -btCollisionShape* afShapeUtils::createCollisionShape(const cMesh *a_collisionMesh, - double a_margin, - afCollisionMeshShapeType a_meshType) -{ - // create the collision shape - btCollisionShape* collisionShape; - switch (a_meshType) { - case afCollisionMeshShapeType::CONCAVE_MESH:{ - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); +/// +/// \brief cMeshTObtTriangleMesh +/// \param a_mesh +/// \return +/// +btTriangleMesh* cMeshTObtTriangleMesh(const cMesh* a_mesh){ - // read number of triangles of the object - unsigned int numTriangles = a_collisionMesh->m_triangles->getNumElements(); + // bullet mesh + btTriangleMesh* bulletMesh = new btTriangleMesh(); - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = a_collisionMesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = a_collisionMesh->m_triangles->getVertexIndex2(i); + // read number of indices of the object + unsigned int numIndices = a_mesh->m_triangles->getNumElements(); - cVector3d vertex0 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex2); + // add all indices to Bullet model + for (unsigned int i=0; im_triangles->getVertexIndex0(i); + unsigned int vertexIndex1 = a_mesh->m_triangles->getVertexIndex1(i); + unsigned int vertexIndex2 = a_mesh->m_triangles->getVertexIndex2(i); + bulletMesh->addTriangleIndices(vertexIndex0, vertexIndex1, vertexIndex2); + } - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); - } + unsigned int numVertices = a_mesh->m_vertices->getNumElements(); - // create mesh collision model - collisionShape = new btGImpactMeshShape(bulletMesh); - break; + for (unsigned int i=0; im_vertices->getLocalPos(i); + bulletMesh->findOrAddVertex(btVector3(vertex(0), vertex(1), vertex(2)), false); } - case afCollisionMeshShapeType::CONVEX_MESH:{ - - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); - // read number of triangles of the object - unsigned int numTriangles = a_collisionMesh->m_triangles->getNumElements(); + return bulletMesh; +} - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = a_collisionMesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = a_collisionMesh->m_triangles->getVertexIndex2(i); - cVector3d vertex0 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex2); +/// +/// \brief afShapeUtils::createCollisionShape +/// \param a_collisionMesh +/// \param a_margin +/// \param a_meshType +/// \return +/// +btCollisionShape* afShapeUtils::createCollisionShape(cMesh *a_collisionMesh, + double a_margin, + afCollisionMeshShapeType a_meshType) +{ + // create the collision shape + btCollisionShape* collisionShape; - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); - } + switch (a_meshType) { + case afCollisionMeshShapeType::CONCAVE_MESH: + case afCollisionMeshShapeType::CONVEX_MESH:{ + // bullet mesh + btTriangleMesh* bulletMesh = cMeshTObtTriangleMesh(a_collisionMesh); + if (a_meshType == afCollisionMeshShapeType::CONCAVE_MESH){ // create mesh collision model - collisionShape = new btConvexTriangleMeshShape(bulletMesh); + collisionShape = new btGImpactMeshShape(bulletMesh); + } + else{ + collisionShape = new btConvexTriangleMeshShape(bulletMesh); + } break; } case afCollisionMeshShapeType::CONVEX_HULL:{ @@ -312,7 +313,15 @@ btCollisionShape* afShapeUtils::createCollisionShape(const cMesh *a_collisionMes } -btCompoundShape *afShapeUtils::createCollisionShape(const cMultiMesh *a_collisionMultiMesh, +/// +/// \brief afShapeUtils::createCollisionShape +/// \param a_collisionMultiMesh +/// \param a_margin +/// \param m_inertialOffset +/// \param a_meshType +/// \return +/// +btCompoundShape *afShapeUtils::createCollisionShape(cMultiMesh *a_collisionMultiMesh, double a_margin, afTransform m_inertialOffset, afCollisionMeshShapeType a_meshType){ @@ -323,76 +332,28 @@ btCompoundShape *afShapeUtils::createCollisionShape(const cMultiMesh *a_collisio inverseInertialOffsetTransform << m_inertialOffset.getInverse(); switch (a_meshType) { - case afCollisionMeshShapeType::CONCAVE_MESH:{ - // create collision detector for each mesh - std::vector::iterator it; - for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) - { - cMesh* mesh = (*it); - - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); - - // read number of triangles of the object - unsigned int numTriangles = mesh->m_triangles->getNumElements(); - - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = mesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = mesh->m_triangles->getVertexIndex2(i); - - cVector3d vertex0 = mesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = mesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = mesh->m_vertices->getLocalPos(vertexIndex2); - - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); - } - - // create mesh collision model - collisionShape = new btGImpactMeshShape(bulletMesh); - collisionShape->setMargin(a_margin); - ((btGImpactMeshShape*) collisionShape)->updateBound(); - compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); - } - break; - } + case afCollisionMeshShapeType::CONCAVE_MESH: case afCollisionMeshShapeType::CONVEX_MESH:{ // create collision detector for each mesh std::vector::iterator it; for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) { cMesh* mesh = (*it); + btTriangleMesh* bulletMesh = cMeshTObtTriangleMesh(mesh); - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); - - // read number of triangles of the object - unsigned int numTriangles = mesh->m_triangles->getNumElements(); - - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = mesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = mesh->m_triangles->getVertexIndex2(i); - - cVector3d vertex0 = mesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = mesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = mesh->m_vertices->getLocalPos(vertexIndex2); - - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); + if (a_meshType == afCollisionMeshShapeType::CONCAVE_MESH){ + // create mesh collision model + collisionShape = new btGImpactMeshShape(bulletMesh); + collisionShape->setMargin(a_margin); + ((btGImpactMeshShape*) collisionShape)->updateBound(); + compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); + } + else{ + // create mesh collision model + collisionShape = new btConvexTriangleMeshShape(bulletMesh); + collisionShape->setMargin(a_margin); + compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); } - - // create mesh collision model - collisionShape = new btConvexTriangleMeshShape(bulletMesh); - collisionShape->setMargin(a_margin); - compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); } break; } @@ -417,19 +378,14 @@ btCompoundShape *afShapeUtils::createCollisionShape(const cMultiMesh *a_collisio } case afCollisionMeshShapeType::POINT_CLOUD:{ std::vector::iterator it; + a_collisionMultiMesh->removeDuplicateVertices(); for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) { cMesh* mesh = (*it); - std::vector filteredVtx; - std::vector filterTri; - std::vector vtxTree; - afMeshCleanup::computeUniqueVerticesandTrianglesSequential(mesh, &filteredVtx, &filterTri, &vtxTree, nullptr, false); - - int numVtx = filteredVtx.size() / 3; - for (uint i = 0 ; i < numVtx ; i++){ + for (uint i = 0 ; i < mesh->m_vertices->getNumElements() ; i++){ collisionShape = new btSphereShape(a_margin); btTransform lT; - btVector3 btPos(filteredVtx[i * 3 + 0], filteredVtx[i * 3 + 1], filteredVtx[i * 3 + 2]); + btVector3 btPos = to_btVector(mesh->m_vertices->getLocalPos(i)); lT.setOrigin(btPos); compoundCollisionShape->addChildShape(inverseInertialOffsetTransform * lT, collisionShape); } @@ -507,8 +463,8 @@ bool afVisualUtils::createFromAttribs(afVisualAttributes *attribs, cMultiMesh *m if (attribs->m_geometryType == afGeometryType::MESH){ if (mesh->loadFromFile(attribs->m_meshFilepath.c_str()) ){ // mesh->scale(m_scale); +// mesh->removeDuplicateVertices(); mesh->setUseDisplayList(true); - // m_visualMesh->markForUpdate(false); } else{ cerr << "WARNING! OBJECT " @@ -2484,6 +2440,7 @@ bool afRigidBody::createFromAttribs(afRigidBodyAttributes *a_attribs) if (m_collisionGeometryType == afGeometryType::MESH){ if (m_collisionMesh->loadFromFile(m_collisionMeshFilePath.c_str()) ){ + m_collisionMesh->removeDuplicateVertices(); m_collisionMesh->scale(m_scale); // Override the inertial offset if it is required by attribs if (a_attribs->m_inertialAttribs.m_estimateInertialOffset){ @@ -2866,486 +2823,6 @@ void afMeshCleanup::updateMaxs(cVector3d &vMax, cVector3d &v){ } -/// -/// \brief afMeshCleanup::clearArrays -/// \param vtxChkBlock -/// \param vtxIdxBlock -/// \param blockSize -/// -void afMeshCleanup::clearArrays(bool *vtxChkBlock, int *vtxIdxBlock, int blockSize){ - int s = blockSize*blockSize*blockSize; - memset(vtxChkBlock, false, s*sizeof(bool)); // Initialize all the vtx check blocks to 0 - memset(vtxIdxBlock, -1, s*sizeof(int)); // Initialize all the vtx index blocks to -1 -} - - -/// -/// \brief afMeshCleanup::computeUniqueVerticesandTriangles -/// \param mesh -/// \param outputVertices -/// \param outputTriangles -/// \param a_vertexTrees -/// \param outputLines -/// \param print_debug_info -/// -void afMeshCleanup::computeUniqueVerticesandTriangles(const cMesh *mesh, std::vector *outputVertices, std::vector *outputTriangles, std::vector* a_vertexTrees, std::vector > *outputLines, bool print_debug_info) -{ - // read number of triangles of the object - int numTriangles = mesh->m_triangles->getNumElements(); - int numVertices = mesh->m_vertices->getNumElements(); - - if (print_debug_info){ - printf("# Triangles %d, # Vertices %d \n", numTriangles, numVertices); - } - - // The max number of vertices to check per block - int vtxBlockSize = 1000; - // Number of default blocks - int numBlocks = 1; - //Define bound for lowest value of vertices - cVector3d vMin(9999,9999,9999); - //Define bound for max value of vertices - cVector3d vMax(-9999,-9999,-9999); - // Length of the bounds (max - min) for each x,y,z - cVector3d vBounds; - - // Update the min and max value (x,y,z) of vertices to get bounds - for (int x = 0 ; x < numVertices ; x++){ - cVector3d v = mesh->m_vertices->getLocalPos(x); - updateMins(vMin, v); // Iterative search to get the min distance - updateMaxs(vMax, v); // Iterative search to get the max distance - } - // Update magnitude of bound - vBounds = vMax - vMin; - if (print_debug_info){ - printf("***************************************\n"); - printf("Vmin = [%f, %f, %f] \n", vMin.x(), vMin.y(), vMin.z()); - printf("Vmax = [%f, %f, %f] \n", vMax.x(), vMax.y(), vMax.z()); - printf("VBounds = [%f, %f, %f] \n", vBounds.x(), vBounds.y(), vBounds.z()); - printf("***************************************\n"); - } - // Place holder for count of repeat and duplicate vertices - int uniqueVtxCount = 0; - int duplicateVtxCount = 0; - - // If number of vertices is greater the vertices per block, increase no of blocks - // This is to prevent memory exhaustion - if (numVertices > vtxBlockSize){ - numBlocks = std::ceil((float)numVertices / (float)vtxBlockSize); - } - - if (print_debug_info){ - printf("Using %d blocks \n", numBlocks); - } - // Copy over the vertices to process without altering the original data - auto vtxArrCopy = mesh->m_vertices->copy(); - // This data-structure is to store the unaltered indices in the first row vertices referring to their - // original copy in the second row. The third row contains the indexes to the vertices after - // the unique vertices have been placed in the outputVertices array - // . E.g. if a vertex at idx 5 was a repeat of vtx at idx 3, orderedVtxList[5][0] = 5 ; orderedVtxList[5][1] = 3; - // and if the vertex was added to the array of unique vertices at Idx 2 then orderedVtxList[5][2] = 2; - int* orderedVtxList = new int [numVertices*3]; - memset(orderedVtxList, -1, numVertices*3*sizeof(int)); - -#define ordVtxIdx(a, b) (numVertices * b + a) - - // This forms a 3D block with all value initiazlied to false - // If we visit a specific 3D idx, it's set to true to know that we have been there - bool* vtxChkBlock = new bool[vtxBlockSize*vtxBlockSize*vtxBlockSize]; - - int vtxBlockSizeSquare = vtxBlockSize * vtxBlockSize; - -#define vtxChkIdx(a, b, c) (vtxBlockSizeSquare * c + vtxBlockSize * b + a) - // This forms a 3D block with all values init to -1 - // What ever 3D idx we visited we set the corresponding corrected idx value in this 3D block - int* vtxIdxBlock = new int[vtxBlockSize*vtxBlockSize*vtxBlockSize]; - // To reduce computational cost, if we have already checked a vertex, we can mark it - bool* vtxAlreadyChkd = new bool[numVertices]; - // Initialize all the vertex check index to false - memset(vtxAlreadyChkd, false, numVertices*sizeof(bool)); - // Upper a lower bound for block in x direction - int xblockLowerBound; - int xblockUpperBound; - // Upper a lower bound for block in y direction - int yblockLowerBound; - int yblockUpperBound; - // Upper a lower bound for block in z direction - int zblockLowerBound; - int zblockUpperBound; - - int vxKey; // X key to look up in the block - int vyKey; // Y key to look up in the block - int vzKey; // Z ket to look up in the block - - double xRes; // X Resolution - double yRes; // Y Resolution - double zRes; // X Resolution - - cVector3d vPos; // The position of a vertex - if(vBounds.x() == 0){ - xRes = 0; // If planar in x direction, set x res to 0 - } - else{ - - xRes = (double) (numVertices - 1) / vBounds.x(); - } - if(vBounds.y() == 0){ - yRes = 0; // If planar in y direction, set x res to 0 - } - else{ - - yRes = (double) (numVertices - 1) / vBounds.y(); - } - if(vBounds.z() == 0){ - zRes = 0; // If planar in z direction, set x res to 0 - } - else{ - zRes = (double) (numVertices - 1) / vBounds.z(); - } - - bool first_print = false; - - // Begin the loop to create a hash grid and check for unique vertices - for (int xblockNum = 0 ; xblockNum < numBlocks ; xblockNum ++){ - xblockLowerBound = xblockNum * vtxBlockSize; - xblockUpperBound = xblockLowerBound + vtxBlockSize; - // first_print = true; - for (int yblockNum = 0 ; yblockNum < numBlocks ; yblockNum ++){ - yblockLowerBound = yblockNum * vtxBlockSize; - yblockUpperBound = yblockLowerBound + vtxBlockSize; - first_print = true; - for (int zblockNum = 0 ; zblockNum < numBlocks ; zblockNum ++){ - zblockLowerBound = zblockNum * vtxBlockSize; - zblockUpperBound = zblockLowerBound + vtxBlockSize; - if (print_debug_info) { - if (first_print){ - printf("Block Num [%d, %d, %d] \n", xblockNum, yblockNum, zblockNum); - first_print = false; - } - } - // Clear the 3D idx and chk arrays to be reused for the new block - clearArrays(&vtxChkBlock[vtxChkIdx(0,0,0)], &vtxIdxBlock[vtxChkIdx(0,0,0)], vtxBlockSize); - for(int idx = 0; idx < numVertices ; idx++){ - if (!vtxAlreadyChkd[idx]){ - vPos = vtxArrCopy->getLocalPos(idx); - // Generate keys to parse the 3D idx and chk block - vxKey = xRes * (vPos.x() - vMin.x()); - vyKey = yRes * (vPos.y() - vMin.y()); - vzKey = zRes * (vPos.z() - vMin.z()); - // Check if the generated keys are in the bounds of the current block - if (vxKey >= xblockLowerBound){ - if (vyKey >= yblockLowerBound){ - if (vzKey >= zblockLowerBound){ - if (vxKey <= xblockUpperBound){ - if (vyKey <= yblockUpperBound){ - if (vzKey <= zblockUpperBound){ - // If the key lies inside the block, offset the value to the block bounds - vxKey -= xblockLowerBound; vyKey -= yblockLowerBound; vzKey -= zblockLowerBound; - // Mark that we already checked this vertex, so we don't have to check it again - vtxAlreadyChkd[idx] = true; - // Set the vertexIdx Pair value - orderedVtxList[ordVtxIdx(idx, 0)] = idx; - // Check if the key is already set in the chk block - if (vtxChkBlock[vtxChkIdx(vxKey,vyKey,vzKey)] == false){ - // Unique vertex, so mark it as such in the corresponding blocks - vtxChkBlock[vtxChkIdx(vxKey,vyKey,vzKey)] = true; - // Set the idx block to the original idx - vtxIdxBlock[vtxChkIdx(vxKey,vyKey,vzKey)] = idx; - orderedVtxList[ordVtxIdx(idx, 1)] = idx; - uniqueVtxCount ++; - } - else{ - // This is not a unique vertex, so get the original idx - // and set it in the corresponding blocks - orderedVtxList[ordVtxIdx(idx, 1)] = vtxIdxBlock[vtxChkIdx(vxKey,vyKey,vzKey)]; - duplicateVtxCount++; - } - } - } - } - } - } - } - } - } - } - } - } - - //Resize once to save on iterative push/pop time - outputVertices->resize(uniqueVtxCount*3); - outputTriangles->resize(numTriangles*3); - a_vertexTrees->resize(uniqueVtxCount); - - // In this loop we append the index of the newly resized array containing - // the unique vertices to the index of the original array of duplicated vertices. - // This is an example of the orderedVtxList might look like for usual run - // After above steps - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1} - // And we want: - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 3, 2, 1, 4, 3, 4, 5, 3} - int vtxCounted = 0; - for (int aIdx = 0 ; aIdx < numVertices ; aIdx++){ - if (orderedVtxList[ordVtxIdx(aIdx,1)] == orderedVtxList[ordVtxIdx(aIdx,0)] && orderedVtxList[ordVtxIdx(aIdx,2)] == -1){ // A unique vertex - vPos = mesh->m_vertices->getLocalPos(aIdx); - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - - orderedVtxList[ordVtxIdx(aIdx,2)] = vtxCounted; // Record the index in queue where the unique vertex is added - (*a_vertexTrees)[vtxCounted].vertexIdx.push_back(aIdx); - vtxCounted++; // Increase the queue idx by 1 - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] < orderedVtxList[ordVtxIdx(aIdx,0)]){ // Not a unique vertex - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - int cIdx = orderedVtxList[ordVtxIdx(bIdx,2)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)] || cIdx == -1){ - // This shouldn't happend. This means that we haven't assigned the third row - // and row 1 is greater than row 2 - throw "Algorithm Failed for (b[i] < a[i]), a[b[i]] != b[b[i]] : %d and c[b[i]] != -1"; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = cIdx; - (*a_vertexTrees)[cIdx].vertexIdx.push_back(aIdx); - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] > orderedVtxList[ordVtxIdx(aIdx,0)]){ - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)]){ - throw "Algorithm Failed for (b[i] > a[i]), a[b[i]] != b[b[i]] : %d"; - } - if (orderedVtxList[ordVtxIdx(bIdx,2)] == -1){ - vPos = mesh->m_vertices->getLocalPos(bIdx); - vtxCounted++; - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - orderedVtxList[ordVtxIdx(bIdx,2)] = vtxCounted; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = orderedVtxList[ordVtxIdx(bIdx,2)]; - } - } - - // This last loop iterates over the triangle idxes and assigns the re-idxd vertices from the - // third row of orderedVtxList - for (int i = 0 ; i < mesh->m_triangles->m_indices.size() ; i++){ - int triIdx = mesh->m_triangles->m_indices[i]; - if ( triIdx >= numVertices){ - std::cerr << "ERROR ! Triangle Vtx Index " << triIdx << " >= # Vertices " << numVertices << std::endl; - } - else{ - (*outputTriangles)[i] = orderedVtxList[ordVtxIdx(triIdx,2)]; - } - } - - // This last loop iterates over the lines and assigns the re-idxd vertices to the - // lines - if (outputLines){ - for (int i = 0 ; i < outputLines->size() ; i++){ - std::vector originalLine = (*outputLines)[i]; - std::vector reIndexedLine = originalLine; - for (int vtx = 0 ; vtx < originalLine.size() ; vtx++){ - int original_idx = originalLine[vtx]; - reIndexedLine[vtx] = orderedVtxList[ordVtxIdx(original_idx,2)]; - } - (*outputLines)[i].clear(); - (*outputLines)[i] = reIndexedLine; - } - } - - if (print_debug_info){ - printf("*** PARALLEL COMPUTE UNIQUE VERTICES AND TRIANGLE INDICES ***\n"); - - for (int i = 0 ; i < uniqueVtxCount; i ++){ - printf("Vertex %d = [%f, %f, %f] \n", i, (*outputVertices)[3*i + 0], (*outputVertices)[3*i + 1], (*outputVertices)[3*i + 2]); - } - - for (int i = 0 ; i < uniqueVtxCount; i ++){ - printf("%d) Children = [", i ); - for (int j = 0 ; j < (*a_vertexTrees)[i].vertexIdx.size(); j++){ - printf(" %d", (*a_vertexTrees)[i].vertexIdx[j]); - } - printf(" ]\n"); - } - - for (int i = 0 ; i < numTriangles; i ++){ - printf("Triangle %d = [%d, %d, %d] \n", i, (*outputTriangles)[3*i], (*outputTriangles)[3*i+1], (*outputTriangles)[3*i+2]); - } - - for (int i = 0 ; i < numVertices ; i++){ - printf("%d) v[0] = %d \t v[1] = %d \t v[2] = %d \n", i, orderedVtxList[ordVtxIdx(i,0)], orderedVtxList[ordVtxIdx(i,1)], orderedVtxList[ordVtxIdx(i,2)]); - } - } - - printf("Unique Vertices Found = %d, Duplicate Vertices Found = %d\n", uniqueVtxCount, duplicateVtxCount); - delete[] orderedVtxList; - delete[] vtxIdxBlock; - delete[] vtxChkBlock; - delete[] vtxAlreadyChkd; -} - - -/// -/// \brief afMeshCleanup::computeUniqueVerticesandTrianglesSequential -/// \param mesh -/// \param outputVertices -/// \param outputTriangles -/// \param a_vertexTrees -/// \param outputLines -/// \param print_debug_info -/// -void afMeshCleanup::computeUniqueVerticesandTrianglesSequential(const cMesh *mesh, std::vector *outputVertices, std::vector *outputTriangles, std::vector* a_vertexTrees , std::vector > *outputLines, bool print_debug_info) -{ - // read number of triangles of the object - int numTriangles = mesh->m_triangles->getNumElements(); - int numVertices = mesh->m_vertices->getNumElements(); - - // Place holder for count of repeat and duplicate vertices - int uniqueVtxCount = 0; - int duplicateVtxCount = 0; - - if (print_debug_info){ - printf("# Triangles %d, # Vertices %d \n", numTriangles, numVertices); - } - - int* orderedVtxList = new int[numVertices*3]; - -#define ordVtxIdx(a, b) (numVertices * b + a) - - orderedVtxList[ordVtxIdx(0,0)] = 0; - orderedVtxList[ordVtxIdx(0,1)] = 0; - orderedVtxList[ordVtxIdx(0,2)] = -1; - - cVector3d v1Pos, v2Pos; - for (int i = 0 ; i < numVertices ; i++){ - orderedVtxList[ordVtxIdx(i,0)] = i; - orderedVtxList[ordVtxIdx(i,1)] = -1; - orderedVtxList[ordVtxIdx(i,2)] = -1; - } - - for (int aIdx = 0 ; aIdx < numVertices - 1 ; aIdx++){ - if (orderedVtxList[ordVtxIdx(aIdx,1)] == -1){ - orderedVtxList[ordVtxIdx(aIdx,1)] = aIdx; - uniqueVtxCount++; - } - else{ - duplicateVtxCount++; - } - for (int bIdx = aIdx + 1 ; bIdx < numVertices ; bIdx++){ - v1Pos = mesh->m_vertices->getLocalPos(aIdx); - v2Pos = mesh->m_vertices->getLocalPos(bIdx); - - if (orderedVtxList[ordVtxIdx(bIdx,1)] == -1){ - if ( (v1Pos - v2Pos).length() == 0 ){ - orderedVtxList[ordVtxIdx(bIdx,1)] = aIdx; - } - } - } - } - - // Check if the last vtx index was assigned - if (orderedVtxList[ordVtxIdx(numVertices-1,1)] == -1){ - orderedVtxList[ordVtxIdx(numVertices-1,1)] = orderedVtxList[ordVtxIdx(numVertices-1,0)]; - uniqueVtxCount++; - } - - outputVertices->resize(uniqueVtxCount*3); - outputTriangles->resize(numTriangles*3); - a_vertexTrees->resize(uniqueVtxCount); - - // In this loop we append the index of the newly resized array containing - // the unique vertices to the index of the original array of duplicated vertices. - // This is an example of the orderedVtxList might look like for usual run - // After above steps - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1} - // And we want: - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 3, 2, 1, 4, 3, 4, 5, 3} - int vtxCounted = 0; - cVector3d vPos; - for (int aIdx = 0 ; aIdx < numVertices ; aIdx++){ - if (orderedVtxList[ordVtxIdx(aIdx,1)] == orderedVtxList[ordVtxIdx(aIdx,0)] && orderedVtxList[ordVtxIdx(aIdx,2)] == -1){ // A unique vertex - vPos = mesh->m_vertices->getLocalPos(aIdx); - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - - orderedVtxList[ordVtxIdx(aIdx,2)] = vtxCounted; // Record the index in queue where the unique vertex is added - (*a_vertexTrees)[vtxCounted].vertexIdx.push_back(aIdx); - vtxCounted++; // Increase the queue idx by 1 - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] < orderedVtxList[ordVtxIdx(aIdx,0)]){ // Not a unique vertex - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - int cIdx = orderedVtxList[ordVtxIdx(bIdx,2)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)] || cIdx == -1){ - // This shouldn't happend. This means that we haven't assigned the third row - // and row 1 is greater than row 2 - throw "Algorithm Failed for (b[i] < a[i]), a[b[i]] != b[b[i]] : and c[b[i]] != -1"; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = cIdx; - (*a_vertexTrees)[cIdx].vertexIdx.push_back(aIdx); - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] > orderedVtxList[ordVtxIdx(aIdx,0)]){ - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)]){ - throw "Algorithm Failed for (b[i] > a[i]), a[b[i]] != b[b[i]] : %d"; - } - if (orderedVtxList[ordVtxIdx(bIdx,2)] == -1){ - vPos = mesh->m_vertices->getLocalPos(bIdx); - vtxCounted++; - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - orderedVtxList[ordVtxIdx(bIdx,2)] = vtxCounted; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = orderedVtxList[ordVtxIdx(bIdx,2)]; - } - } - - // This last loop iterates over the triangle idxes and assigns the re-idxd vertices from the - // third row of orderedVtxList - for (int i = 0 ; i < mesh->m_triangles->m_indices.size() ; i++){ - int triIdx = mesh->m_triangles->m_indices[i]; if ( triIdx >= numVertices){ - std::cerr << "ERROR ! Triangle Vtx Index " << triIdx << " >= # Vertices " << numVertices << std::endl; - } - else{ - (*outputTriangles)[i] = orderedVtxList[ordVtxIdx(triIdx,2)]; - } - } - - // This last loop iterates over the lines and assigns the re-idxd vertices to the - // lines - if (outputLines){ - for (int i = 0 ; i < outputLines->size() ; i++){ - std::vector originalLine = (*outputLines)[i]; - std::vector reIndexedLine = originalLine; - for (int vtx = 0 ; vtx < originalLine.size() ; vtx++){ - reIndexedLine[vtx] = orderedVtxList[ordVtxIdx(originalLine[vtx],2)]; - } - (*outputLines)[i].clear(); - (*outputLines)[i] = reIndexedLine; - } - } - - if(print_debug_info){ - printf("*** SEQUENTIAL COMPUTE UNIQUE VERTICES AND TRIANGLE INDICES ***\n"); - - printf("# Unique Vertices = %d, # Duplicate Vertices = %d\n", uniqueVtxCount, duplicateVtxCount); - - for (int i = 0 ; i < numVertices ; i++){ - std::cerr << i << ")\t" << orderedVtxList[ordVtxIdx(i,0)] << " ,\t" << orderedVtxList[ordVtxIdx(i,1)] << " ,\t" << orderedVtxList[ordVtxIdx(i,2)] << std::endl; - } - } - - delete[] orderedVtxList; -} - - /// /// \brief afSoftBody::afSoftBody /// \param a_afWorld @@ -3354,6 +2831,11 @@ afSoftBody::afSoftBody(afWorldPtr a_afWorld, afModelPtr a_modelPtr): afInertialO } +/// +/// \brief afSoftBody::createFromAttribs +/// \param a_attribs +/// \return +/// bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) { storeAttributes(a_attribs); @@ -3372,7 +2854,6 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) if (afVisualUtils::createFromAttribs(&a_attribs->m_visualAttribs, m_visualMesh, m_name)){ m_visualMesh->scale(m_scale); - m_meshReductionSuccessful = false; } else { @@ -3384,10 +2865,10 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) } if (m_collisionMesh->loadFromFile(a_attribs->m_collisionAttribs.m_meshFilepath.c_str())){ + m_collisionMesh->removeDuplicateVertices(); m_collisionMesh->scale(m_scale); // Use the visual mesh for generating the softbody generateFromMesh(m_collisionMesh, a_attribs->m_collisionAttribs.m_margin); - cleanupMesh(m_visualMesh, m_afVertexTree, m_trianglesPtr); } else { @@ -3474,12 +2955,12 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) int remappedIdx = nIt->second[0]; int j = 0; bool found = false; - while (j < m_afVertexTree.size() && !found){ - for (int k = 0 ; k < m_afVertexTree[j].vertexIdx.size() ; k++){ - if (remappedIdx == m_afVertexTree[j].vertexIdx[k]){ -// cerr << "Node Idx: " << nodeIdx -// << " | Original Vtx Idx: " << nIt->first -// << " | Remapped Vtx Idx: " << remappedIdx << endl; + while (j < m_collisionMesh->getMesh(0)->m_duplicateVertexIndexTree.size() && !found){ + for (int k = 0 ; k < m_collisionMesh->getMesh(0)->m_duplicateVertexIndexTree[j].m_vertexIndices.size() ; k++){ + if (remappedIdx == m_collisionMesh->getMesh(0)->m_duplicateVertexIndexTree[j].m_vertexIndices[k]){ + cerr << "Node Idx: " << nodeIdx + << " | Original Vtx Idx: " << nIt->first + << " | Remapped Vtx Idx: " << remappedIdx << endl; softBody->setMass(j, 0); found = true; break; @@ -3539,23 +3020,12 @@ void afSoftBody::toggleSkeletalModelVisibility(){ void afSoftBody::updateSceneObjects(){ cMesh * mesh = m_visualMesh->getMesh(0); - if (m_meshReductionSuccessful){ - for (int i = 0 ; i < m_bulletSoftBody->m_nodes.size() ; i++){ - btVector3 nodePos = m_bulletSoftBody->m_nodes[i].m_x; - mesh->m_vertices->setLocalPos(i, nodePos[0], nodePos[1], nodePos[2]); - } - } - else{ - for (int i = 0 ; i < m_afVertexTree.size() ; i++){ - btVector3 nodePos = m_bulletSoftBody->m_nodes[i].m_x; - for (int j = 0 ; j < m_afVertexTree[i].vertexIdx.size() ; j++){ - int idx = m_afVertexTree[i].vertexIdx[j]; - mesh->m_vertices->setLocalPos(idx, nodePos[0], nodePos[1], nodePos[2]); - } - } + for (int i = 0 ; i < m_bulletSoftBody->m_nodes.size() ; i++){ + mesh->setVertexLocalPosForAllDuplicates(i, m_bulletSoftBody->m_nodes[i].m_x[0], m_bulletSoftBody->m_nodes[i].m_x[1], m_bulletSoftBody->m_nodes[i].m_x[2]); } mesh->computeAllNormals(); + mesh->markForUpdate(true); afBaseObject::updateSceneObjects(); } @@ -3605,66 +3075,42 @@ bool afSoftBody::cleanupMesh(cMultiMesh *multiMesh, std::vector &a reducedMesh->setShowEdges(false); multiMesh->m_meshes->clear(); multiMesh->addMesh(reducedMesh); - m_meshReductionSuccessful = true; } return valid; } + +/// +/// \brief afSoftBody::generateFromMesh +/// \param multiMesh +/// \param a_margin +/// \return +/// bool afSoftBody::generateFromMesh(cMultiMesh *multiMesh, const double a_margin) { // create compound shape m_bulletCollisionShape = new btCompoundShape();; - std::vector *v_meshes; - v_meshes = multiMesh->m_meshes; - // create collision detector for each mesh - std::vector::iterator it; - for (it = v_meshes->begin(); it < v_meshes->end(); it++) + for (int mi=0; mi < multiMesh->getNumMeshes() ; mi++) { - cMesh* mesh = (*it); - // read number of triangles of the object - int numTriangles = mesh->m_triangles->getNumElements(); - std::vector > polyLines = mesh->m_lines; - // computeUniqueVerticesandTriangles(mesh, &m_verticesPtr, &m_trianglesPtr, &m_afVertexTree, &polyLines, false); - afMeshCleanup::computeUniqueVerticesandTrianglesSequential(mesh, &m_verticesPtr, &m_trianglesPtr, &m_afVertexTree, &polyLines, false); - if (m_trianglesPtr.size() > 0){ - m_bulletSoftBody = createFromMesh(*m_afWorld->m_bulletSoftBodyWorldInfo, - m_verticesPtr.data(), m_verticesPtr.size() / 3, m_trianglesPtr.data(), numTriangles); - createLinksFromLines(m_bulletSoftBody, &polyLines, mesh); - } - else{ - m_bulletSoftBody = new btSoftBody(m_afWorld->m_bulletSoftBodyWorldInfo); - /* Default material */ - btSoftBody::Material* pm = m_bulletSoftBody->appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = btSoftBody::fMaterial::Default; - if (m_bulletSoftBody){ - m_bulletSoftBody->m_nodes.resize(mesh->m_vertices->getNumElements()); - for(int nIdx = 0 ; nIdx < m_bulletSoftBody->m_nodes.size() ; nIdx++){ - btVector3 vPos = to_btVector(mesh->m_vertices->getLocalPos(nIdx)); - btSoftBody::Node& n = m_bulletSoftBody->m_nodes[nIdx]; - n.m_im = 1; - n.m_im = 1 / n.m_im; - n.m_x = vPos; - n.m_q = n.m_x; - n.m_n = btVector3(0, 0, 1); - n.m_leaf = m_bulletSoftBody->m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, 0.1), &n); - n.m_material = m_bulletSoftBody->m_materials[0]; - } - } - createLinksFromLines(m_bulletSoftBody, &mesh->m_lines, mesh); - } + cMesh* mesh = multiMesh->getMesh(mi); + m_bulletSoftBody = createFromMesh(m_afWorld->m_bulletSoftBodyWorldInfo, mesh, false); + createLinksFromLines(m_bulletSoftBody, &mesh->m_lines, mesh); m_bulletSoftBody->getCollisionShape()->setMargin(a_margin); } - return true; } +/// +/// \brief afSoftBody::createLinksFromLines +/// \param a_softBody +/// \param a_lines +/// \param a_mesh +/// \return +/// bool afSoftBody::createLinksFromLines(btSoftBody *a_softBody, std::vector< std::vector > *a_lines, cMesh* a_mesh){ if (a_softBody && a_lines){ for(int lIdx = 0 ; lIdx < a_lines->size() ; lIdx++){ @@ -3713,30 +3159,35 @@ bool afSoftBody::createLinksFromLines(btSoftBody *a_softBody, std::vector< std:: } -btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, int nNodes, - const unsigned int* triangles, int ntriangles, bool randomizeConstraints) -{ +/// +/// \brief afSoftBody::createFromMesh +/// \param worldInfo +/// \param a_mesh +/// \param randomizeConstraints +/// \return +/// +btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo* worldInfo, cMesh *a_mesh, bool randomizeConstraints){ unsigned int maxidx = 0; int i, j, ni; - for (i = 0, ni = ntriangles * 3; i < ni; ++i) + for (i = 0, ni = a_mesh->getNumTriangles() * 3; i < ni; ++i) { - maxidx = btMax(triangles[i], maxidx); + maxidx = btMax(a_mesh->m_triangles->m_indices[i], maxidx); } ++maxidx; btAlignedObjectArray chks; btAlignedObjectArray vtx; chks.resize(maxidx * maxidx, false); - vtx.resize(nNodes); - for (i = 0, j = 0; i < nNodes * 3; ++j, i += 3) + vtx.resize(a_mesh->getNumVertices()); + for (i = 0; i < a_mesh->getNumVertices(); i++) { - vtx[j] = btVector3(vertices[i], vertices[i + 1], vertices[i + 2]); + vtx[i] = to_btVector(a_mesh->m_vertices->getLocalPos(i)); } - btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0); - for (i = 0, ni = ntriangles * 3; i < ni; i += 3) + btSoftBody* psb = new btSoftBody(worldInfo, vtx.size(), &vtx[0], 0); + for (i = 0, ni = a_mesh->getNumTriangles() * 3; i < ni; i += 3) { - const unsigned int idx[] = {triangles[i], triangles[i + 1], triangles[i + 2]}; -#define IDX(_x_, _y_) ((_y_)*maxidx + (_x_)) + const unsigned int idx[] = {a_mesh->m_triangles->m_indices[i], a_mesh->m_triangles->m_indices[i + 1], a_mesh->m_triangles->m_indices[i + 2]}; + #define IDX(_x_, _y_) ((_y_)*maxidx + (_x_)) for (int j = 2, k = 0; k < 3; j = k++) { if (!chks[IDX(idx[j], idx[k])]) @@ -3746,7 +3197,7 @@ btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo& worldInfo, const btS psb->appendLink(idx[j], idx[k]); } } -#undef IDX + #undef IDX psb->appendFace(idx[0], idx[1], idx[2]); } @@ -3756,6 +3207,7 @@ btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo& worldInfo, const btS } return (psb); + } diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index af00cb574..d980e86a5 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -174,9 +174,9 @@ class afShapeUtils{ static btCollisionShape* createCollisionShape(const afPrimitiveShapeAttributes* a_primitiveShape, double a_margin); - static btCollisionShape* createCollisionShape(const cMesh* a_collisionMesh, double a_margin, afCollisionMeshShapeType a_meshType); + static btCollisionShape* createCollisionShape(cMesh* a_collisionMesh, double a_margin, afCollisionMeshShapeType a_meshType); - static btCompoundShape* createCollisionShape(const cMultiMesh* a_collisionMesh, double a_margin, afTransform m_inertialOffset, afCollisionMeshShapeType a_meshType); + static btCompoundShape* createCollisionShape(cMultiMesh* a_collisionMesh, double a_margin, afTransform m_inertialOffset, afCollisionMeshShapeType a_meshType); static std::vector createRayAttribs(cMultiMesh* a_contourMesh, double a_range); }; @@ -1214,24 +1214,10 @@ class afSoftBody: public afInertialObject{ bool createLinksFromLines(btSoftBody* a_sb, std::vector< std::vector>* a_lines, cMesh* a_mesh); // Copied from btSoftBodyHelpers with few modifications - btSoftBody* createFromMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, int nNodes, const unsigned int* triangles, int ntriangles, bool randomizeConstraints=true); + btSoftBody* createFromMesh(btSoftBodyWorldInfo* worldInfo, cMesh* a_mesh, bool randomizeConstraints=true); //! This method toggles the drawing of skeletal model. void toggleSkeletalModelVisibility(); - -private: - // Ptr to scalar vertex arrays of the sofy body - std::vector m_verticesPtr; - - // Ptr to Triangles arrays referring to vertices by indices - std::vector m_trianglesPtr; - - // Vertex Tree containing vtx idx's that are repeated for a given vtx - std::vector m_afVertexTree; - - // Boolean flag to indicate if we have been successful in reducing the mesh. - // A reduced mesh should speed up rendering. - bool m_meshReductionSuccessful; }; From 3683cdaa945c24048b45e2470a837dccbbac6e81 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Wed, 29 Nov 2023 23:02:36 +0500 Subject: [PATCH 22/23] Cleaned up fixing softbody nodes impl --- ambf_framework/afFramework.cpp | 44 ++++++++++++----------------- external/chai3d/src/world/CMesh.cpp | 35 ++++++++++++++++------- external/chai3d/src/world/CMesh.h | 7 +++++ 3 files changed, 50 insertions(+), 36 deletions(-) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index bbd39597a..bcabfaf97 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -2945,35 +2945,27 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) bool useOriginalIndexes = getVisualObject()->m_vtxIdxMap.size() > 0 ? true : false; for (uint i = 0 ; i < a_attribs->m_fixedNodes.size() ; i++){ + uint nodeIdx = a_attribs->m_fixedNodes[i]; - if ( nodeIdx < softBody->m_nodes.size()){ - if (useOriginalIndexes){ - // Find the node's original vertex index - map >::iterator nIt = getVisualObject()->m_vtxIdxMap.find(nodeIdx); - if ( nIt != getVisualObject()->m_vtxIdxMap.end()){ - if (nIt->second.size() > 0){ - int remappedIdx = nIt->second[0]; - int j = 0; - bool found = false; - while (j < m_collisionMesh->getMesh(0)->m_duplicateVertexIndexTree.size() && !found){ - for (int k = 0 ; k < m_collisionMesh->getMesh(0)->m_duplicateVertexIndexTree[j].m_vertexIndices.size() ; k++){ - if (remappedIdx == m_collisionMesh->getMesh(0)->m_duplicateVertexIndexTree[j].m_vertexIndices[k]){ - cerr << "Node Idx: " << nodeIdx - << " | Original Vtx Idx: " << nIt->first - << " | Remapped Vtx Idx: " << remappedIdx << endl; - softBody->setMass(j, 0); - found = true; - break; - } - } - j++; - } - } + if ( nodeIdx > softBody->m_nodes.size()){break;} + + if (useOriginalIndexes){ + // Find the node's original vertex index + map >::iterator nIt = getVisualObject()->m_vtxIdxMap.find(nodeIdx); + if ( nIt != getVisualObject()->m_vtxIdxMap.end()){ + if (nIt->second.size() == 0){break;} + int originalVtxIdx = nIt->second[0]; + unsigned int newIdx = m_collisionMesh->getMesh(0)->getNewVertexIndex(originalVtxIdx); + if (newIdx > 0){ + cerr << "INFO! Fixing Softbody Node. Original Node Idx: " << nodeIdx + << " | Old Vertex/Node Idx: " << originalVtxIdx + << " | New Vertex/Node Idx: " << newIdx << endl; + softBody->setMass(newIdx, 0); } } - else{ - softBody->setMass(nodeIdx, 0); - } + } + else{ + softBody->setMass(nodeIdx, 0); } } diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index fec92b378..fdcdc07c2 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -1109,17 +1109,18 @@ bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) m_vertices->getUseBitangentData(), m_vertices->getUseUserData()); - for (int i = 0 ; i < unique_vertex_count ; i++){ - uint oIdx = m_duplicateVertexIndexTree[i].m_vertexIndices[0]; + for (auto it = m_duplicateVertexIndexTree.begin() ; it != m_duplicateVertexIndexTree.end() ; ++it){ + uint newIdx = it->first; + uint origIdx = it->second.m_vertexIndices[0]; - nMesh->m_vertices->setLocalPos(i, m_vertices->getLocalPos(oIdx)); + nMesh->m_vertices->setLocalPos(newIdx, m_vertices->getLocalPos(origIdx)); - if(m_vertices->getUseNormalData()) nMesh->m_vertices->setNormal(i, m_vertices->getNormal(oIdx)); - if(m_vertices->getUseTexCoordData()) nMesh->m_vertices->setTexCoord(i, m_vertices->getTexCoord(oIdx)); - if(m_vertices->getUseColorData()) nMesh->m_vertices->setColor(i, m_vertices->getColor(oIdx)); - if(m_vertices->getUseTangentData()) nMesh->m_vertices->setTangent(i, m_vertices->getTangent(oIdx)); - if(m_vertices->getUseBitangentData()) nMesh->m_vertices->setBitangent(i, m_vertices->getBitangent(oIdx)); - if(m_vertices->getUseUserData()) nMesh->m_vertices->setUserData(i, m_vertices->getUserData(oIdx)); + if(m_vertices->getUseNormalData()) nMesh->m_vertices->setNormal(newIdx, m_vertices->getNormal(origIdx)); + if(m_vertices->getUseTexCoordData()) nMesh->m_vertices->setTexCoord(newIdx, m_vertices->getTexCoord(origIdx)); + if(m_vertices->getUseColorData()) nMesh->m_vertices->setColor(newIdx, m_vertices->getColor(origIdx)); + if(m_vertices->getUseTangentData()) nMesh->m_vertices->setTangent(newIdx, m_vertices->getTangent(origIdx)); + if(m_vertices->getUseBitangentData()) nMesh->m_vertices->setBitangent(newIdx, m_vertices->getBitangent(origIdx)); + if(m_vertices->getUseUserData()) nMesh->m_vertices->setUserData(newIdx, m_vertices->getUserData(origIdx)); } printf("INFO! Original/New vertex count [%u/%u]. Removed [%u] vertices \n", getNumVertices(), nMesh->getNumVertices(), getNumVertices() - nMesh->getNumVertices()); m_vertices->clear(); @@ -1158,12 +1159,13 @@ bool cMesh::findDuplicateVertices(double a_weldingThreshold){ set rMesh; computeBoundaryBox(); afMeshWeldingSpecs weldingSpecs(getBoundaryMin(), getBoundaryMax(), a_weldingThreshold); + m_originalToNewMapping.resize(m_vertices->getNumElements()); uint insIdx = 0; for(int i = 0 ; i < m_triangles->m_indices.size() ; i++){ uint oIdx = m_triangles->m_indices[i]; cVector3d v = m_triangles->m_vertices->getLocalPos(oIdx); - pair::iterator, bool> insIt = rMesh.insert(afTriVertex(v, insIdx, &weldingSpecs)); + auto insIt = rMesh.insert(afTriVertex(v, insIdx, &weldingSpecs)); uint nIdx; if (insIt.second){ nIdx = insIdx; @@ -1174,6 +1176,7 @@ bool cMesh::findDuplicateVertices(double a_weldingThreshold){ } m_duplicateVertexIndexTree[nIdx].m_elementIndices.push_back(i); m_duplicateVertexIndexTree[nIdx].m_vertexIndices.push_back(oIdx); + m_originalToNewMapping[oIdx] = nIdx; } bool res = m_duplicateVertexIndexTree.size() == m_vertices->getNumElements() ? 0 : 1; @@ -2102,6 +2105,18 @@ void cEdge::set(cMesh* a_parent, } } +unsigned int cMesh::getNewVertexIndex(const unsigned int &a_idx) +{ + if (!m_duplicateVerticesFound){ + findDuplicateVertices(); + } + if (a_idx >= m_originalToNewMapping.size()){ + cerr << "ERROR! REQUESTED INDEX " << a_idx << " GREATER THAN THE SIZE OF ORIGINAL VERTEX LIST" << m_originalToNewMapping.size() << "\n"; + return -1; + } + return m_originalToNewMapping[a_idx]; +} + //------------------------------------------------------------------------------ #endif // DOXYGEN_SHOULD_SKIP_THIS //------------------------------------------------------------------------------ diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index dc5b9e0b0..e482da068 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -442,6 +442,10 @@ class cMesh : public cGenericObject //! Array of Edges. std::vector m_edges; +public: + + unsigned int getNewVertexIndex(const unsigned int& a_idx); + //! Tree of duplicate vertex indices, the key is the new index (after identifying duplicates) and //! value (rhs) is the list of original indices of duplicate vertices. //! E.g. Imagine two triangles <123 and <456 with a shared edge between with vertices 2,3 == 4,5 @@ -453,6 +457,9 @@ class cMesh : public cGenericObject //! {4: [6]} std::map m_duplicateVertexIndexTree; + // Vector to main a relationship between old indices (with duplicates) and new indices (without dpulicates) + std::vector m_originalToNewMapping; + //! Flag to check if duplicate vertices have been removed bool m_duplicateVerticesFound; From 0a1f55e65ee192545808e22e3d2ed16ce19b2d66 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Sat, 2 Dec 2023 01:45:53 +0500 Subject: [PATCH 23/23] Allow for controlling whether to remove or not remove duplicates --- adf_loader/version_1_0/adf_loader_1_0.cpp | 8 ++++++++ ambf_framework/afAttributes.h | 8 ++++++++ ambf_framework/afFramework.cpp | 15 ++++++++++++--- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/adf_loader/version_1_0/adf_loader_1_0.cpp b/adf_loader/version_1_0/adf_loader_1_0.cpp index 700c7647a..80343bf82 100644 --- a/adf_loader/version_1_0/adf_loader_1_0.cpp +++ b/adf_loader/version_1_0/adf_loader_1_0.cpp @@ -211,6 +211,7 @@ bool ADFUtils::getVisualAttribsFromNode(YAML::Node *a_node, afVisualAttributes * YAML::Node& node = *a_node; YAML::Node meshNode = node["mesh"]; + YAML::Node meshRemoveDuplicatesNode = node["mesh remove duplicates"]; YAML::Node shapeNode = node["shape"]; YAML::Node compoundShapeNode = node["compound shape"]; YAML::Node geometryNode = node["geometry"]; @@ -258,6 +259,13 @@ bool ADFUtils::getVisualAttribsFromNode(YAML::Node *a_node, afVisualAttributes * attribs->m_meshFilepath = localPath / meshNode.as(); if (!attribs->m_meshFilepath.c_str().empty()){ attribs->m_geometryType = afGeometryType::MESH; + + if (meshRemoveDuplicatesNode.IsDefined()){ + if (meshRemoveDuplicatesNode.as() == true){ + attribs->m_meshRemoveDuplicates = afStatusFlag::TRUE;} + else{ + attribs->m_meshRemoveDuplicates = afStatusFlag::FALSE;} + } } else{ valid = false; diff --git a/ambf_framework/afAttributes.h b/ambf_framework/afAttributes.h index 66e387198..1acdce0ba 100644 --- a/ambf_framework/afAttributes.h +++ b/ambf_framework/afAttributes.h @@ -53,6 +53,12 @@ typedef unsigned int uint; namespace ambf { +enum class afStatusFlag{ + UNDEFINED, + TRUE, + FALSE, +}; + /// /// \brief The afKinematicAttributes struct /// @@ -362,9 +368,11 @@ struct afColorAttributes{ struct afVisualAttributes{ afVisualAttributes(){ m_visible = true; + m_meshRemoveDuplicates = afStatusFlag::UNDEFINED; } afPath m_meshFilepath; + afStatusFlag m_meshRemoveDuplicates; afGeometryType m_geometryType; vector m_primitiveShapes; afColorAttributes m_colorAttribs; diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index bcabfaf97..a07d64f3f 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -378,7 +378,6 @@ btCompoundShape *afShapeUtils::createCollisionShape(cMultiMesh *a_collisionMulti } case afCollisionMeshShapeType::POINT_CLOUD:{ std::vector::iterator it; - a_collisionMultiMesh->removeDuplicateVertices(); for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) { cMesh* mesh = (*it); @@ -462,8 +461,17 @@ cMaterial afMaterialUtils::createFromAttribs(afColorAttributes *a_color) bool afVisualUtils::createFromAttribs(afVisualAttributes *attribs, cMultiMesh *mesh, string obj_name){ if (attribs->m_geometryType == afGeometryType::MESH){ if (mesh->loadFromFile(attribs->m_meshFilepath.c_str()) ){ - // mesh->scale(m_scale); -// mesh->removeDuplicateVertices(); + if (attribs->m_meshRemoveDuplicates == afStatusFlag::TRUE){ + mesh->removeDuplicateVertices(); + } + else if (attribs->m_meshRemoveDuplicates == afStatusFlag::UNDEFINED){ + for (auto m : *(mesh->m_meshes)){ + if (m->getNumVertices() > 10000){ + double wd = 0.; + m->removeDuplicateVertices(wd); + } + } + } mesh->setUseDisplayList(true); } else{ @@ -8125,6 +8133,7 @@ bool afGhostObject::createFromAttribs(afGhostObjectAttributes *a_attribs) if (a_attribs->m_collisionAttribs.m_geometryType == afGeometryType::MESH){ if (m_collisionMesh->loadFromFile(a_attribs->m_collisionAttribs.m_meshFilepath.c_str()) ){ m_collisionMesh->scale(m_scale); + m_collisionMesh->removeDuplicateVertices(); m_bulletCollisionShape = afShapeUtils::createCollisionShape(m_collisionMesh, a_attribs->m_collisionAttribs.m_margin, afTransform(),