From c9c837cc6754274db1a532737c584756eee7e054 Mon Sep 17 00:00:00 2001 From: hujiajie Date: Mon, 18 Jul 2016 23:07:26 +0800 Subject: [PATCH 01/36] [WIP] Suppress compiler warnings. Fix Visual Studio warning C4373: previous versions of the compiler did not override when parameters only differed by const/volatile qualifiers. --- Extras/InverseDynamics/DillCreator.cpp | 2 +- Extras/InverseDynamics/DillCreator.hpp | 2 +- Extras/InverseDynamics/btMultiBodyTreeCreator.cpp | 2 +- Extras/InverseDynamics/btMultiBodyTreeCreator.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp index a1ddf00c2..46f68e698 100644 --- a/Extras/InverseDynamics/DillCreator.cpp +++ b/Extras/InverseDynamics/DillCreator.cpp @@ -44,7 +44,7 @@ int DillCreator::getNumBodies(int* num_bodies) const { return 0; } -int DillCreator::getBody(int body_index, int* parent_index, JointType* joint_type, +int DillCreator::getBody(const int body_index, int* parent_index, JointType* joint_type, vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, void** user_ptr) const { diff --git a/Extras/InverseDynamics/DillCreator.hpp b/Extras/InverseDynamics/DillCreator.hpp index d52e0d06d..fbe0e8e22 100644 --- a/Extras/InverseDynamics/DillCreator.hpp +++ b/Extras/InverseDynamics/DillCreator.hpp @@ -22,7 +22,7 @@ public: ///\copydoc MultiBodyTreeCreator::getNumBodies int getNumBodies(int* num_bodies) const; ///\copydoc MultiBodyTreeCreator::getBody - int getBody(int body_index, int* parent_index, JointType* joint_type, + int getBody(const int body_index, int* parent_index, JointType* joint_type, vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, void** user_ptr) const; diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index b91a10cce..ef3ebf6b8 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -237,7 +237,7 @@ int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const { return 0; } -int btMultiBodyTreeCreator::getBody(int body_index, int *parent_index, JointType *joint_type, +int btMultiBodyTreeCreator::getBody(const int body_index, int *parent_index, JointType *joint_type, vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int, diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp index 0e2d614f2..3844cf3d3 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -25,7 +25,7 @@ public: /// \copydoc MultiBodyTreeCreator::getNumBodies int getNumBodies(int *num_bodies) const; ///\copydoc MultiBodyTreeCreator::getBody - int getBody(int body_index, int *parent_index, JointType *joint_type, + int getBody(const int body_index, int *parent_index, JointType *joint_type, vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int, void **user_ptr) const; From 51ec707230e5763b3cfb71d215f9eca4718cc87a Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Wed, 20 Jul 2016 11:01:02 -0700 Subject: [PATCH 02/36] fix GetJointInfo to use a param called jointIndex (not linkIndex) for naming consistency --- examples/SharedMemory/PhysicsClientC_API.cpp | 4 ++-- examples/SharedMemory/PhysicsClientC_API.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1ff24b66c..4197544b2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -702,10 +702,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) } -int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; - return cl->getJointInfo(bodyIndex, linkIndex,*info); + return cl->getJointInfo(bodyIndex, jointIndex, *info); } b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a3b6dbd5c..2e2f126e2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -56,8 +56,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, ///give a unique body index (after loading the body) return the number of joints. int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); -///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h -int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); +///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); ///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h From d3a94248d484615dc4dd4d5c29819bf3efb125a0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 20 Jul 2016 12:48:01 -0700 Subject: [PATCH 03/36] fix one more multi-threading issue --- .../MultiThreading/b3PosixThreadSupport.cpp | 24 +++++++++---------- .../MultiThreading/b3PosixThreadSupport.h | 12 ++++++++-- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/examples/MultiThreading/b3PosixThreadSupport.cpp b/examples/MultiThreading/b3PosixThreadSupport.cpp index c19ab1b48..9a00d48aa 100644 --- a/examples/MultiThreading/b3PosixThreadSupport.cpp +++ b/examples/MultiThreading/b3PosixThreadSupport.cpp @@ -44,8 +44,6 @@ b3PosixThreadSupport::~b3PosixThreadSupport() #define NAMED_SEMAPHORES #endif -// this semaphore will signal, if and how many threads are finished with their work -static sem_t* mainSemaphore=0; static sem_t* createSem(const char* baseName) { @@ -100,12 +98,12 @@ static void *threadFunction(void *argument) b3Assert(status->m_status); status->m_userThreadFunc(userPtr,status->m_lsMemory); status->m_status = 2; - checkPThreadFunction(sem_post(mainSemaphore)); + checkPThreadFunction(sem_post(status->m_mainSemaphore)); status->threadUsed++; } else { //exit Thread status->m_status = 3; - checkPThreadFunction(sem_post(mainSemaphore)); + checkPThreadFunction(sem_post(status->m_mainSemaphore)); printf("Thread with taskId %i exiting\n",status->m_taskId); break; } @@ -160,13 +158,14 @@ bool b3PosixThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1, b3Assert(m_activeThreadStatus.size()); // wait for any of the threads to finish - int result = sem_trywait(mainSemaphore); + int result = sem_trywait(m_mainSemaphore); if (result==0) { // get at least one thread which has finished - size_t last = -1; - - for(size_t t=0; t < size_t(m_activeThreadStatus.size()); ++t) { + int last = -1; + int status = -1; + for(int t=0; t < int (m_activeThreadStatus.size()); ++t) { + status = m_activeThreadStatus[t].m_status; if(2 == m_activeThreadStatus[t].m_status) { last = t; break; @@ -200,7 +199,7 @@ void b3PosixThreadSupport::waitForResponse( int *puiArgument0, int *puiArgument b3Assert(m_activeThreadStatus.size()); // wait for any of the threads to finish - checkPThreadFunction(sem_wait(mainSemaphore)); + checkPThreadFunction(sem_wait(m_mainSemaphore)); // get at least one thread which has finished size_t last = -1; @@ -231,7 +230,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi printf("%s creating %i threads.\n", __FUNCTION__, threadConstructionInfo.m_numThreads); m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads); - mainSemaphore = createSem("main"); + m_mainSemaphore = createSem("main"); //checkPThreadFunction(sem_wait(mainSemaphore)); for (int i=0;i < threadConstructionInfo.m_numThreads;i++) @@ -249,6 +248,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi spuStatus.m_taskId = i; spuStatus.m_commandId = 0; spuStatus.m_status = 0; + spuStatus.m_mainSemaphore = m_mainSemaphore; spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc(); spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; spuStatus.threadUsed = 0; @@ -271,7 +271,7 @@ void b3PosixThreadSupport::stopThreads() spuStatus.m_userPtr = 0; checkPThreadFunction(sem_post(spuStatus.startSemaphore)); - checkPThreadFunction(sem_wait(mainSemaphore)); + checkPThreadFunction(sem_wait(m_mainSemaphore)); printf("destroy semaphore\n"); destroySem(spuStatus.startSemaphore); @@ -280,7 +280,7 @@ void b3PosixThreadSupport::stopThreads() } printf("destroy main semaphore\n"); - destroySem(mainSemaphore); + destroySem(m_mainSemaphore); printf("main semaphore destroyed\n"); m_activeThreadStatus.clear(); } diff --git a/examples/MultiThreading/b3PosixThreadSupport.h b/examples/MultiThreading/b3PosixThreadSupport.h index ae23dd610..3f54d4ab5 100644 --- a/examples/MultiThreading/b3PosixThreadSupport.h +++ b/examples/MultiThreading/b3PosixThreadSupport.h @@ -57,14 +57,22 @@ public: void* m_userPtr; //for taskDesc etc void* m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc - pthread_t thread; - sem_t* startSemaphore; + pthread_t thread; + //each tread will wait until this signal to start its work + sem_t* startSemaphore; + // this is a copy of m_mainSemaphore, + //each tread will signal once it is finished with its work + sem_t* m_mainSemaphore; unsigned long threadUsed; }; private: b3AlignedObjectArray m_activeThreadStatus; + + // m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work + sem_t* m_mainSemaphore; + public: ///Setup and initialize SPU/CELL/Libspe2 From 75e86051c2724f159cecf12bd6b683ab8b83b82b Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Sun, 24 Jul 2016 22:22:42 -0700 Subject: [PATCH 04/36] Add inverse kinematics example with implementations by Sam Buss. Uses Kuka IIWA model description and 4 methods: Selectively Damped Least Squares,Damped Least Squares, Jacobi Transpose, Jacobi Pseudo Inverse Tweak some PD values in Inverse Dynamics example and Robot example. --- examples/ExampleBrowser/CMakeLists.txt | 13 + examples/ExampleBrowser/ExampleEntries.cpp | 19 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 1 + examples/ExampleBrowser/premake4.lua | 4 +- .../InverseDynamicsExample.cpp | 21 +- .../InverseKinematicsExample.cpp | 382 ++++ .../InverseKinematicsExample.h | 8 + .../SharedMemory/PhysicsClientExample.cpp | 6 +- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 640 ++++++ examples/ThirdPartyLibs/BussIK/Jacobian.h | 132 ++ examples/ThirdPartyLibs/BussIK/LinearR2.cpp | 101 + examples/ThirdPartyLibs/BussIK/LinearR2.h | 981 ++++++++ examples/ThirdPartyLibs/BussIK/LinearR3.cpp | 822 +++++++ examples/ThirdPartyLibs/BussIK/LinearR3.h | 2027 +++++++++++++++++ examples/ThirdPartyLibs/BussIK/LinearR4.cpp | 467 ++++ examples/ThirdPartyLibs/BussIK/LinearR4.h | 1099 +++++++++ examples/ThirdPartyLibs/BussIK/MathMisc.h | 384 ++++ examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 984 ++++++++ examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 402 ++++ examples/ThirdPartyLibs/BussIK/Misc.cpp | 346 +++ examples/ThirdPartyLibs/BussIK/Node.cpp | 90 + examples/ThirdPartyLibs/BussIK/Node.h | 101 + examples/ThirdPartyLibs/BussIK/Spherical.h | 298 +++ examples/ThirdPartyLibs/BussIK/Tree.cpp | 217 ++ examples/ThirdPartyLibs/BussIK/Tree.h | 92 + examples/ThirdPartyLibs/BussIK/VectorRn.cpp | 46 + examples/ThirdPartyLibs/BussIK/VectorRn.h | 238 ++ examples/TinyRenderer/tgaimage.h | 6 +- .../Featherstone/btMultiBodyJointMotor.cpp | 28 +- 29 files changed, 9926 insertions(+), 29 deletions(-) create mode 100644 examples/InverseKinematics/InverseKinematicsExample.cpp create mode 100644 examples/InverseKinematics/InverseKinematicsExample.h create mode 100644 examples/ThirdPartyLibs/BussIK/Jacobian.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/Jacobian.h create mode 100644 examples/ThirdPartyLibs/BussIK/LinearR2.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/LinearR2.h create mode 100644 examples/ThirdPartyLibs/BussIK/LinearR3.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/LinearR3.h create mode 100644 examples/ThirdPartyLibs/BussIK/LinearR4.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/LinearR4.h create mode 100644 examples/ThirdPartyLibs/BussIK/MathMisc.h create mode 100644 examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/MatrixRmn.h create mode 100644 examples/ThirdPartyLibs/BussIK/Misc.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/Node.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/Node.h create mode 100644 examples/ThirdPartyLibs/BussIK/Spherical.h create mode 100644 examples/ThirdPartyLibs/BussIK/Tree.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/Tree.h create mode 100644 examples/ThirdPartyLibs/BussIK/VectorRn.cpp create mode 100644 examples/ThirdPartyLibs/BussIK/VectorRn.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index da15ba32b..0678eaf4a 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -155,6 +155,8 @@ SET(BulletExampleBrowser_SRCS ../BasicDemo/BasicExample.h ../InverseDynamics/InverseDynamicsExample.cpp ../InverseDynamics/InverseDynamicsExample.h + ../InverseKinematics/InverseKinematicsExample.cpp + ../InverseKinematics/InverseKinematicsExample.h ../ForkLift/ForkLiftDemo.cpp ../ForkLift/ForkLiftDemo.h ../Tutorial/Tutorial.cpp @@ -298,6 +300,17 @@ SET(BulletExampleBrowser_SRCS ../ThirdPartyLibs/stb_image/stb_image.cpp ../ThirdPartyLibs/stb_image/stb_image.h + + ../ThirdPartyLibs/BussIK/Jacobian.cpp + ../ThirdPartyLibs/BussIK/Tree.cpp + ../ThirdPartyLibs/BussIK/Node.cpp + ../ThirdPartyLibs/BussIK/LinearR2.cpp + ../ThirdPartyLibs/BussIK/LinearR3.cpp + ../ThirdPartyLibs/BussIK/LinearR4.cpp + ../ThirdPartyLibs/BussIK/MatrixRmn.cpp + ../ThirdPartyLibs/BussIK/VectorRn.cpp + ../ThirdPartyLibs/BussIK/Misc.cpp + ../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp ../ThirdPartyLibs/tinyxml/tinystr.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c5901ca34..a6abd4fd7 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -46,6 +46,7 @@ #include "../MultiThreading/MultiThreadingExample.h" #include "../InverseDynamics/InverseDynamicsExample.h" #include "../RoboticsLearning/R2D2GraspExample.h" +#include "../InverseKinematics/InverseKinematicsExample.h" #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" @@ -95,7 +96,6 @@ struct ExampleEntry static ExampleEntry gDefaultExamples[]= { - ExampleEntry(0,"API"), ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc), @@ -124,14 +124,21 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc), ExampleEntry(1,"TestPendulum","Simulate a pendulum using btMultiBody with a constant joint torque applied. The same code is also used as a unit test comparing Bullet with the numerical solution of second-order non-linear differential equation stored in pendulum_gold.h", TestPendulumCreateFunc), - ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), + ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), - ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0), + ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0), - ExampleEntry(0,"Inverse Dynamics"), - ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), - ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY), + ExampleEntry(0,"Inverse Dynamics"), + ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), + ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY), + + ExampleEntry(0, "Inverse Kinematics"), + ExampleEntry(1, "SDLS", "Selectively Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_SDLS), + ExampleEntry(1, "DLS", "Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS), + ExampleEntry(1, "Jacobi Transpose", "Jacobi Transpose by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_JACOB_TRANS), + ExampleEntry(1, "Jacobi Pseudo Inv", "Jacobi Pseudo Inverse Method by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_PURE_PSEUDO), + ExampleEntry(0,"Tutorial"), ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index e06d5f205..44d0baf53 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -800,6 +800,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) s_app = new SimpleOpenGL2App(title,width,height); s_app->m_renderer = new SimpleOpenGL2Renderer(width,height); } + #ifndef NO_OPENGL3 else { diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 7d11eaed9..2010c744e 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -45,9 +45,10 @@ project "App_BulletExampleBrowser" defines {"INCLUDE_CLOTH_DEMOS"} files { + "main.cpp", "ExampleEntries.cpp", - + "../InverseKinematics/*", "../TinyRenderer/geometry.cpp", "../TinyRenderer/model.cpp", "../TinyRenderer/tgaimage.cpp", @@ -116,6 +117,7 @@ project "App_BulletExampleBrowser" "../ThirdPartyLibs/stb_image/*", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", "../ThirdPartyLibs/tinyxml/*", + "../ThirdPartyLibs/BussIK/*", "../GyroscopicDemo/GyroscopicSetup.cpp", "../GyroscopicDemo/GyroscopicSetup.h", "../ThirdPartyLibs/tinyxml/tinystr.cpp", diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index ebf7efb0d..9eb9287f4 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -157,7 +157,7 @@ void InverseDynamicsExample::initPhysics() BulletURDFImporter u2b(m_guiHelper,0); - bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf"); + bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf"); if (loadOk) { int rootLinkIndex = u2b.getRootLinkIndex(); @@ -225,13 +225,13 @@ void InverseDynamicsExample::initPhysics() { qd[dof] = 0; char tmp[25]; - sprintf(tmp,"q_desired[%zu]",dof); + sprintf(tmp,"q_desired[%u]",dof); qd_name[dof] = tmp; SliderParams slider(qd_name[dof].c_str(),&qd[dof]); slider.m_minVal=-3.14; slider.m_maxVal=3.14; - sprintf(tmp,"q[%zu]",dof); + sprintf(tmp,"q[%u]",dof); q_name[dof] = tmp; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); btVector4 color = sJointCurveColors[dof&7]; @@ -340,6 +340,21 @@ void InverseDynamicsExample::stepSimulation(float deltaTime) // todo(thomas) check that this is correct: // want to advance by 10ms, with 1ms timesteps m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3); + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + m_multiBody->forwardKinematics(scratch_q, scratch_m); + for (int i = 0; i < m_multiBody->getNumLinks(); i++) + { + //btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin(); + btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform; + btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector); + btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec; + //printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z()); + + + + + } } } diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp new file mode 100644 index 000000000..ae1ed8f17 --- /dev/null +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -0,0 +1,382 @@ +#include "InverseKinematicsExample.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../OpenGLWindow/OpenGLInclude.h" + +#include "BussIK/Node.h" +#include "BussIK/Tree.h" +#include "BussIK/Jacobian.h" +#include "BussIK/VectorRn.h" + +#define RADIAN(X) ((X)*RadiansToDegrees) + +#define MAX_NUM_NODE 1000 +#define MAX_NUM_THETA 1000 +#define MAX_NUM_EFFECT 100 + +double T = 0; +VectorR3 target1[MAX_NUM_EFFECT]; + + + +// Make slowdown factor larger to make the simulation take larger, less frequent steps +// Make the constant factor in Tstep larger to make time pass more quickly +//const int SlowdownFactor = 40; +const int SlowdownFactor = 0; // Make higher to take larger steps less frequently +const int SleepsPerStep=SlowdownFactor; +int SleepCounter=0; +//const double Tstep = 0.0005*(double)SlowdownFactor; // Time step + + +int AxesList; /* list to hold the axes */ +int AxesOn; /* ON or OFF */ + +float Scale, Scale2; /* scaling factors */ + + + +int JointLimitsOn; +int RestPositionOn; +int UseJacobianTargets1; + + +int numIteration = 1; +double error = 0.0; +double errorDLS = 0.0; +double errorSDLS = 0.0; +double sumError = 0.0; +double sumErrorDLS = 0.0; +double sumErrorSDLS = 0.0; + +#ifdef _DYNAMIC +bool initMaxDist = true; +extern double Excess[]; +extern double dsnorm[]; +#endif + + + + +void Reset(Tree &tree, Jacobian* m_ikJacobian) +{ + AxesOn = false; + + Scale = 1.0; + Scale2 = 0.0; /* because add 1. to it in Display() */ + + JointLimitsOn = true; + RestPositionOn = false; + UseJacobianTargets1 = false; + + + tree.Init(); + tree.Compute(); + m_ikJacobian->Reset(); + +} + +// Update target positions + +void UpdateTargets( double T2, Tree & treeY) { + double T = T2 / 20.; + target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T)); +} + + +// Does a single update (on one kind of tree) +void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) { + + if ( SleepCounter==0 ) { + T += Tstep; + UpdateTargets( T , treeY); + } + + if ( UseJacobianTargets1 ) { + jacob->SetJtargetActive(); + } + else { + jacob->SetJendActive(); + } + jacob->ComputeJacobian(); // Set up Jacobian and deltaS vectors + + // Calculate the change in theta values + switch (ikMethod) { + case IK_JACOB_TRANS: + jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method + break; + case IK_DLS: + jacob->CalcDeltaThetasDLS(); // Damped least squares method + break; + case IK_PURE_PSEUDO: + jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method + break; + case IK_SDLS: + jacob->CalcDeltaThetasSDLS(); // Selectively damped least squares method + break; + default: + jacob->ZeroDeltaThetas(); + break; + } + + if ( SleepCounter==0 ) { + jacob->UpdateThetas(); // Apply the change in the theta values + jacob->UpdatedSClampValue(); + SleepCounter = SleepsPerStep; + } + else { + SleepCounter--; + } + + +} + + + + + + + + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class InverseKinematicsExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + int m_ikMethod; + Tree m_ikTree; + b3AlignedObjectArray m_ikNodes; + Jacobian* m_ikJacobian; + + float m_x; + float m_y; + float m_z; + b3AlignedObjectArray m_movingInstances; + int m_targetInstance; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + InverseKinematicsExample(CommonGraphicsApp* app, int option) + :m_app(app), + m_x(0), + m_y(0), + m_z(0), + m_targetInstance(-1), + m_ikMethod(option) + { + m_app->setUpAxis(2); + + { + b3Vector3 extents=b3MakeVector3(100,100,100); + extents[m_app->getUpAxis()]=1; + + int xres = 20; + int yres = 20; + + b3Vector4 color0=b3MakeVector4(0.4, 0.4, 0.4,1); + b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1); + m_app->registerGrid(xres, yres, color0, color1); + } + + ///create some graphics proxy for the tracking target + ///the endeffector tries to track it using Inverse Kinematics + { + int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM); + b3Vector3 pos = b3MakeVector3(1,1,1); + pos[app->getUpAxis()] = 1; + b3Quaternion orn(0, 0, 0, 1); + b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1); + b3Vector3 scaling = b3MakeVector3(.02, .02, .02); + m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling); + } + m_app->m_renderer->writeTransforms(); + } + virtual ~InverseKinematicsExample() + { + m_app->m_renderer->enableBlend(false); + } + + + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + BuildKukaIIWAShape(); + m_ikJacobian = new Jacobian(&m_ikTree); + Reset(m_ikTree,m_ikJacobian); + } + virtual void exitPhysics() + { + delete m_ikJacobian; + m_ikJacobian = 0; + } + + void BuildKukaIIWAShape(); + + void getLocalTransform(const Node* node, b3Transform& act) + { + b3Vector3 axis = b3MakeVector3(node->v.x, node->v.y, node->v.z); + b3Quaternion rot(0, 0, 0, 1); + if (axis.length()) + { + rot = b3Quaternion (axis, node->theta); + } + act.setIdentity(); + act.setRotation(rot); + act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z)); + } + void MyDrawTree(Node* node, const b3Transform& tr) + { + b3Vector3 lineColor = b3MakeVector3(0, 0, 0); + int lineWidth = 2; + if (node != 0) { + // glPushMatrix(); + b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z); + b3Vector3 color = b3MakeVector3(0, 1, 0); + int pointSize = 10; + m_app->m_renderer->drawPoint(pos, color, pointSize); + + m_app->m_renderer->drawLine(pos, pos+ 0.05*tr.getBasis().getColumn(0), b3MakeVector3(1,0,0), lineWidth); + m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth); + m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth); + + b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z); + b3Vector3 axisWorld = tr.getBasis()*axisLocal; + + m_app->m_renderer->drawLine(pos, pos + 0.1*axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5); + + //node->DrawNode(node == root); // Recursively draw node and update ModelView matrix + if (node->left) { + b3Transform act; + getLocalTransform(node->left, act); + + b3Transform trl = tr*act; + m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth); + MyDrawTree(node->left, trl); // Draw tree of children recursively + } + // glPopMatrix(); + if (node->right) { + b3Transform act; + getLocalTransform(node->right, act); + b3Transform trr = tr*act; + m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth); + MyDrawTree(node->right,trr); // Draw right siblings recursively + } + } + + } + virtual void stepSimulation(float deltaTime) + { + DoUpdateStep(deltaTime, m_ikTree, m_ikJacobian, m_ikMethod); + } + virtual void renderScene() + { + + + b3Transform act; + getLocalTransform(m_ikTree.GetRoot(), act); + MyDrawTree(m_ikTree.GetRoot(), act); + + b3Vector3 pos = b3MakeVector3(target1[0].x, target1[0].y, target1[0].z); + b3Quaternion orn(0, 0, 0, 1); + + m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance); + m_app->m_renderer->writeTransforms(); + m_app->m_renderer->renderScene(); + } + + + virtual void physicsDebugDraw() + { + + } + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 1.3; + float pitch = 120; + float yaw = 13; + float targetPos[3]={-0.35,0.14,0.25}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]); + } + } + +}; + +void InverseKinematicsExample::BuildKukaIIWAShape() +{ + const VectorR3& unitx = VectorR3::UnitX; + const VectorR3& unity = VectorR3::UnitY; + const VectorR3& unitz = VectorR3::UnitZ; + const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0); + const VectorR3& zero = VectorR3::Zero; + + float minTheta = -4 * PI; + float maxTheta = 4 * PI; + + m_ikNodes.resize(8);//7DOF+additional endeffector + + m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.)); + m_ikTree.InsertRoot(m_ikNodes[0]); + + m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.)); + m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]); + + m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]); + + m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]); + + m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]); + + m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]); + + m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]); + + m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR); + m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]); + +} + + +class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new InverseKinematicsExample(options.m_guiHelper->getAppInterface(), options.m_option); +} + + + + + diff --git a/examples/InverseKinematics/InverseKinematicsExample.h b/examples/InverseKinematics/InverseKinematicsExample.h new file mode 100644 index 000000000..ca0d1e3b5 --- /dev/null +++ b/examples/InverseKinematics/InverseKinematicsExample.h @@ -0,0 +1,8 @@ +#ifndef INVERSE_KINEMATICSEXAMPLE_H +#define INVERSE_KINEMATICSEXAMPLE_H + +enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS }; + +class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options); + +#endif //INVERSE_KINEMATICSEXAMPLE_H diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 9faa782a9..b5788bd09 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -171,10 +171,10 @@ protected: b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos); b3JointControlSetDesiredVelocity(commandHandle,uIndex,0); - b3JointControlSetKp(commandHandle, qIndex, 0.01); - b3JointControlSetKd(commandHandle, uIndex, 0.1); + b3JointControlSetKp(commandHandle, qIndex, 0.2); + b3JointControlSetKd(commandHandle, uIndex, 1.); - b3JointControlSetMaximumForce(commandHandle,uIndex,1000); + b3JointControlSetMaximumForce(commandHandle,uIndex,5000); } } virtual void physicsDebugDraw(int debugFlags) diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp new file mode 100644 index 000000000..646513b86 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -0,0 +1,640 @@ +/* +* +* Inverse Kinematics software, with several solvers including +* Selectively Damped Least Squares Method +* Damped Least Squares Method +* Pure Pseudoinverse Method +* Jacobian Transpose Method +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +#include +#include +#include +#include +using namespace std; + +#include "../OpenGLWindow/OpenGLInclude.h" + +#include "Jacobian.h" + +void Arrow(const VectorR3& tail, const VectorR3& head); + +//extern RestPositionOn; +extern VectorR3 target1[]; + +// Optimal damping values have to be determined in an ad hoc manner (Yuck!) +const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter) +//const double Jacobian::DefaultDampingLambda = 1.1; // Optimal for the DLS "double Y" shape (any lower gives jitter) +// const double Jacobian::DefaultDampingLambda = 0.7; // Optimal for the DLS "double Y" shape with distance clamping (lower gives jitter) + +const double Jacobian::PseudoInverseThresholdFactor = 0.01; +const double Jacobian::MaxAngleJtranspose = 30.0*DegreesToRadians; +const double Jacobian::MaxAnglePseudoinverse = 5.0*DegreesToRadians; +const double Jacobian::MaxAngleDLS = 45.0*DegreesToRadians; +const double Jacobian::MaxAngleSDLS = 45.0*DegreesToRadians; +const double Jacobian::BaseMaxTargetDist = 0.4; + +Jacobian::Jacobian(Tree* tree) +{ + Jacobian::tree = tree; + nEffector = tree->GetNumEffector(); + nJoint = tree->GetNumJoint(); + nRow = 3 * nEffector; + nCol = nJoint; + + Jend.SetSize(nRow, nCol); // The Jocobian matrix + Jend.SetZero(); + Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions + Jtarget.SetZero(); + SetJendActive(); + + U.SetSize(nRow, nRow); // The U matrix for SVD calculations + w .SetLength(Min(nRow, nCol)); + V.SetSize(nCol, nCol); // The V matrix for SVD calculations + + dS.SetLength(nRow); // (Target positions) - (End effector positions) + dTheta.SetLength(nCol); // Changes in joint angles + dPreTheta.SetLength(nCol); + + // Used by Jacobian transpose method & DLS & SDLS + dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta + + // Used by the Selectively Damped Least Squares Method + //dT.SetLength(nRow); + dSclamp.SetLength(nEffector); + errorArray.SetLength(nEffector); + Jnorms.SetSize(nEffector, nCol); // Holds the norms of the active J matrix + + Reset(); +} + +void Jacobian::Reset() +{ + // Used by Damped Least Squares Method + DampingLambda = DefaultDampingLambda; + DampingLambdaSq = Square(DampingLambda); + // DampingLambdaSDLS = 1.5*DefaultDampingLambda; + + dSclamp.Fill(HUGE_VAL); +} + +// Compute the deltaS vector, dS, (the error in end effector positions +// Compute the J and K matrices (the Jacobians) +void Jacobian::ComputeJacobian() +{ + // Traverse tree to find all end effectors + VectorR3 temp; + Node* n = tree->GetRoot(); + while ( n ) { + if ( n->IsEffector() ) { + int i = n->GetEffectorNum(); + const VectorR3& targetPos = target1[i]; + + // Compute the delta S value (differences from end effectors to target positions. + temp = targetPos; + temp -= n->GetS(); + dS.SetTriple(i, temp); + + // Find all ancestors (they will usually all be joints) + // Set the corresponding entries in the Jacobians J, K. + Node* m = tree->GetParent(n); + while ( m ) { + int j = m->GetJointNum(); + assert ( 0 <=i && iIsFrozen() ) { + Jend.SetTriple(i, j, VectorR3::Zero); + Jtarget.SetTriple(i, j, VectorR3::Zero); + } + else { + temp = m->GetS(); // joint pos. + temp -= n->GetS(); // -(end effector pos. - joint pos.) + temp *= m->GetW(); // cross product with joint rotation axis + Jend.SetTriple(i, j, temp); + temp = m->GetS(); // joint pos. + temp -= targetPos; // -(target pos. - joint pos.) + temp *= m->GetW(); // cross product with joint rotation axis + Jtarget.SetTriple(i, j, temp); + } + m = tree->GetParent( m ); + } + } + n = tree->GetSuccessor( n ); + } +} + +// The delta theta values have been computed in dTheta array +// Apply the delta theta values to the joints +// Nothing is done about joint limits for now. +void Jacobian::UpdateThetas() +{ + // Traverse the tree to find all joints + // Update the joint angles + Node* n = tree->GetRoot(); + while ( n ) { + if ( n->IsJoint() ) { + int i = n->GetJointNum(); + n->AddToTheta( dTheta[i] ); + + } + n = tree->GetSuccessor( n ); + } + + // Update the positions and rotation axes of all joints/effectors + tree->Compute(); +} + +void Jacobian::CalcDeltaThetas() +{ + switch (CurrentUpdateMode) { + case JACOB_Undefined: + ZeroDeltaThetas(); + break; + case JACOB_JacobianTranspose: + CalcDeltaThetasTranspose(); + break; + case JACOB_PseudoInverse: + CalcDeltaThetasPseudoinverse(); + break; + case JACOB_DLS: + CalcDeltaThetasDLS(); + break; + case JACOB_SDLS: + CalcDeltaThetasSDLS(); + break; + } +} + +void Jacobian::ZeroDeltaThetas() +{ + dTheta.SetZero(); +} + +// Find the delta theta values using inverse Jacobian method +// Uses a greedy method to decide scaling factor +void Jacobian::CalcDeltaThetasTranspose() +{ + const MatrixRmn& J = ActiveJacobian(); + + J.MultiplyTranspose( dS, dTheta ); + + // Scale back the dTheta values greedily + J.Multiply ( dTheta, dT1 ); // dT = J * dTheta + double alpha = Dot(dS,dT1) / dT1.NormSq(); + assert ( alpha>0.0 ); + // Also scale back to be have max angle change less than MaxAngleJtranspose + double maxChange = dTheta.MaxAbs(); + double beta = MaxAngleJtranspose/maxChange; + dTheta *= Min(alpha, beta); + +} + +void Jacobian::CalcDeltaThetasPseudoinverse() +{ + MatrixRmn& J = const_cast(ActiveJacobian()); + + // Compute Singular Value Decomposition + // This an inefficient way to do Pseudoinverse, but it is convenient since we need SVD anyway + + J.ComputeSVD( U, w, V ); + + // Next line for debugging only + assert(J.DebugCheckSVD(U, w , V)); + + // Calculate response vector dTheta that is the DLS solution. + // Delta target values are the dS values + // We multiply by Moore-Penrose pseudo-inverse of the J matrix + double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs(); + + long diagLength = w.GetLength(); + double* wPtr = w.GetPtr(); + dTheta.SetZero(); + for ( long i=0; ipseudoInverseThreshold ) { + alpha = 1.0/alpha; + MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha ); + } + } + + // Scale back to not exceed maximum angle changes + double maxChange = dTheta.MaxAbs(); + if ( maxChange>MaxAnglePseudoinverse ) { + dTheta *= MaxAnglePseudoinverse/maxChange; + } + +} + +void Jacobian::CalcDeltaThetasDLS() +{ + const MatrixRmn& J = ActiveJacobian(); + + MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T) + U.AddToDiagonal( DampingLambdaSq ); + + // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. + // CalcdTClampedFromdS(); + // VectorRn dTextra(3*nEffector); + // U.Solve( dT, &dTextra ); + // J.MultiplyTranspose( dTextra, dTheta ); + + // Use these two lines for the traditional DLS method + U.Solve( dS, &dT1 ); + J.MultiplyTranspose( dT1, dTheta ); + + // Scale back to not exceed maximum angle changes + double maxChange = dTheta.MaxAbs(); + if ( maxChange>MaxAngleDLS ) { + dTheta *= MaxAngleDLS/maxChange; + } +} + +void Jacobian::CalcDeltaThetasDLSwithSVD() +{ + const MatrixRmn& J = ActiveJacobian(); + + // Compute Singular Value Decomposition + // This an inefficient way to do DLS, but it is convenient since we need SVD anyway + + J.ComputeSVD( U, w, V ); + + // Next line for debugging only + assert(J.DebugCheckSVD(U, w , V)); + + // Calculate response vector dTheta that is the DLS solution. + // Delta target values are the dS values + // We multiply by DLS inverse of the J matrix + long diagLength = w.GetLength(); + double* wPtr = w.GetPtr(); + dTheta.SetZero(); + for ( long i=0; iMaxAngleDLS ) { + dTheta *= MaxAngleDLS/maxChange; + } +} + + +void Jacobian::CalcDeltaThetasSDLS() +{ + const MatrixRmn& J = ActiveJacobian(); + + // Compute Singular Value Decomposition + + J.ComputeSVD( U, w, V ); + + // Next line for debugging only + assert(J.DebugCheckSVD(U, w , V)); + + // Calculate response vector dTheta that is the SDLS solution. + // Delta target values are the dS values + int nRows = J.GetNumRows(); + int numEndEffectors = tree->GetNumEffector(); // Equals the number of rows of J divided by three + int nCols = J.GetNumColumns(); + dTheta.SetZero(); + + // Calculate the norms of the 3-vectors in the Jacobian + long i; + const double *jx = J.GetPtr(); + double *jnx = Jnorms.GetPtr(); + for ( i=nCols*numEndEffectors; i>0; i-- ) { + double accumSq = Square(*(jx++)); + accumSq += Square(*(jx++)); + accumSq += Square(*(jx++)); + *(jnx++) = sqrt(accumSq); + } + + // Clamp the dS values + CalcdTClampedFromdS(); + + // Loop over each singular vector + for ( i=0; i0; j-- ) { + double tmp; + alpha += (*ux)*(*(dTx++)); + tmp = Square( *(ux++) ); + alpha += (*ux)*(*(dTx++)); + tmp += Square(*(ux++)); + alpha += (*ux)*(*(dTx++)); + tmp += Square(*(ux++)); + N += sqrt(tmp); + } + + // M is the quasi-1-norm of the response to angles changing according to the i-th column of V + // Then is multiplied by the wiInv value. + double M = 0.0; + double *vx = V.GetColumnPtr(i); + jnx = Jnorms.GetPtr(); + for ( j=nCols; j>0; j-- ) { + double accum=0.0; + for ( long k=numEndEffectors; k>0; k-- ) { + accum += *(jnx++); + } + M += fabs((*(vx++)))*accum; + } + M *= fabs(wiInv); + + double gamma = MaxAngleSDLS; + if ( NMaxAngleSDLS ) { + dTheta *= MaxAngleSDLS/(MaxAngleSDLS+maxChange); + //dTheta *= MaxAngleSDLS/maxChange; + } +} + +void Jacobian::CalcdTClampedFromdS() +{ + long len = dS.GetLength(); + long j = 0; + for ( long i=0; iSquare(dSclamp[j]) ) { + double factor = dSclamp[j]/sqrt(normSq); + dT1[i] = dS[i]*factor; + dT1[i+1] = dS[i+1]*factor; + dT1[i+2] = dS[i+2]*factor; + } + else { + dT1[i] = dS[i]; + dT1[i+1] = dS[i+1]; + dT1[i+2] = dS[i+2]; + } + } +} + +double Jacobian::UpdateErrorArray() +{ + double totalError = 0.0; + + // Traverse tree to find all end effectors + VectorR3 temp; + Node* n = tree->GetRoot(); + while ( n ) { + if ( n->IsEffector() ) { + int i = n->GetEffectorNum(); + const VectorR3& targetPos = target1[i]; + temp = targetPos; + temp -= n->GetS(); + double err = temp.Norm(); + errorArray[i] = err; + totalError += err; + } + n = tree->GetSuccessor( n ); + } + return totalError; +} + +void Jacobian::UpdatedSClampValue() +{ + // Traverse tree to find all end effectors + VectorR3 temp; + Node* n = tree->GetRoot(); + while ( n ) { + if ( n->IsEffector() ) { + int i = n->GetEffectorNum(); + const VectorR3& targetPos = target1[i]; + + // Compute the delta S value (differences from end effectors to target positions. + // While we are at it, also update the clamping values in dSclamp; + temp = targetPos; + temp -= n->GetS(); + double normSi = sqrt(Square(dS[i])+Square(dS[i+1])+Square(dS[i+2])); + double changedDist = temp.Norm()-normSi; + if ( changedDist>0.0 ) { + dSclamp[i] = BaseMaxTargetDist + changedDist; + } + else { + dSclamp[i] = BaseMaxTargetDist; + } + } + n = tree->GetSuccessor( n ); + } +} + +void Jacobian::DrawEigenVectors() const +{ + int i, j; + VectorR3 tail; + VectorR3 head; + Node *node; + + for (i=0; iGetEffector(j); + tail = node->GetS(); + U.GetTriple( j, i, &head ); + head += tail; + glDisable(GL_LIGHTING); + glColor3f(1.0f, 0.2f, 0.0f); + glLineWidth(2.0); + glBegin(GL_LINES); + glVertex3f(tail.x, tail.y, tail.z); + glVertex3f(head.x, head.y, tail.z); + glEnd(); + Arrow(tail, head); + glLineWidth(1.0); + glEnable(GL_LIGHTING); + } + } +} + +void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 ) +{ + const VectorRn& e1 = j1.errorArray; + const VectorRn& e2 = j2.errorArray; + double ret1 = 0.0; + double ret2 = 0.0; + int len = e1.GetLength(); + for ( long i=0; iGetNumEffector(); // Equals the number of rows of J divided by three + int nCols = J.GetNumColumns(); + dTheta.SetZero(); + + // Calculate the norms of the 3-vectors in the Jacobian + long i; + const double *jx = J.GetPtr(); + double *jnx = Jnorms.GetPtr(); + for ( i=nCols*numEndEffectors; i>0; i-- ) { + double accumSq = Square(*(jx++)); + accumSq += Square(*(jx++)); + accumSq += Square(*(jx++)); + *(jnx++) = sqrt(accumSq); + } + + // Loop over each singular vector + for ( i=0; i0; j-- ) { + double tmp; + alpha += (*ux)*(*(dSx++)); + tmp = Square( *(ux++) ); + alpha += (*ux)*(*(dSx++)); + tmp += Square(*(ux++)); + alpha += (*ux)*(*(dSx++)); + tmp += Square(*(ux++)); + N += sqrt(tmp); + } + + // P is the quasi-1-norm of the response to angles changing according to the i-th column of V + double P = 0.0; + double *vx = V.GetColumnPtr(i); + jnx = Jnorms.GetPtr(); + for ( j=nCols; j>0; j-- ) { + double accum=0.0; + for ( long k=numEndEffectors; k>0; k-- ) { + accum += *(jnx++); + } + P += fabs((*(vx++)))*accum; + } + + double lambda = 1.0; + if ( N

MaxAngleSDLS ) { + dTheta *= MaxAngleSDLS/maxChange; + } +} */ + + + + diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h new file mode 100644 index 000000000..1c0bc4572 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -0,0 +1,132 @@ + +/* +* +* Inverse Kinematics software, with several solvers including +* Selectively Damped Least Squares Method +* Damped Least Squares Method +* Pure Pseudoinverse Method +* Jacobian Transpose Method +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +#include "Node.h" +#include "Tree.h" +#include "MathMisc.h" +#include "LinearR3.h" +#include "VectorRn.h" +#include "MatrixRmn.h" + +#ifndef _CLASS_JACOBIAN +#define _CLASS_JACOBIAN + +#ifdef _DYNAMIC +const double BASEMAXDIST = 0.02; +#else +const double MAXDIST = 0.08; // optimal value for double Y shape : 0.08 +#endif +const double DELTA = 0.4; +const long double LAMBDA = 2.0; // only for DLS. optimal : 0.24 +const double NEARZERO = 0.0000000001; + +enum UpdateMode { + JACOB_Undefined = 0, + JACOB_JacobianTranspose = 1, + JACOB_PseudoInverse = 2, + JACOB_DLS = 3, + JACOB_SDLS = 4 }; + +class Jacobian { +public: + Jacobian(Tree*); + + void ComputeJacobian(); + const MatrixRmn& ActiveJacobian() const { return *Jactive; } + void SetJendActive() { Jactive = &Jend; } // The default setting is Jend. + void SetJtargetActive() { Jactive = &Jtarget; } + + void CalcDeltaThetas(); // Use this only if the Current Mode has been set. + void ZeroDeltaThetas(); + void CalcDeltaThetasTranspose(); + void CalcDeltaThetasPseudoinverse(); + void CalcDeltaThetasDLS(); + void CalcDeltaThetasDLSwithSVD(); + void CalcDeltaThetasSDLS(); + + void UpdateThetas(); + double UpdateErrorArray(); // Returns sum of errors + const VectorRn& GetErrorArray() const { return errorArray; } + void UpdatedSClampValue(); + void DrawEigenVectors() const; + + void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; } + UpdateMode GetCurrentMode() const { return CurrentUpdateMode; } + void SetDampingDLS( double lambda ) { DampingLambda = lambda; DampingLambdaSq = Square(lambda); } + + void Reset(); + + static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 ); + static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies ); + +private: + Tree* tree; // tree associated with this Jacobian matrix + int nEffector; // Number of end effectors + int nJoint; // Number of joints + int nRow; // Total number of rows the real J (= 3*number of end effectors for now) + int nCol; // Total number of columns in the real J (= number of joints for now) + + MatrixRmn Jend; // Jacobian matrix based on end effector positions + MatrixRmn Jtarget; // Jacobian matrix based on target positions + MatrixRmn Jnorms; // Norms of 3-vectors in active Jacobian (SDLS only) + + MatrixRmn U; // J = U * Diag(w) * V^T (Singular Value Decomposition) + VectorRn w; + MatrixRmn V; + + UpdateMode CurrentUpdateMode; + + VectorRn dS; // delta s + VectorRn dT1; // delta t -- these are delta S values clamped to smaller magnitude + VectorRn dSclamp; // Value to clamp magnitude of dT at. + VectorRn dTheta; // delta theta + VectorRn dPreTheta; // delta theta for single eigenvalue (SDLS only) + + VectorRn errorArray; // Distance of end effectors from target after updating + + // Parameters for pseudoinverses + static const double PseudoInverseThresholdFactor; // Threshold for treating eigenvalue as zero (fraction of largest eigenvalue) + + // Parameters for damped least squares + static const double DefaultDampingLambda; + double DampingLambda; + double DampingLambdaSq; + //double DampingLambdaSDLS; + + // Cap on max. value of changes in angles in single update step + static const double MaxAngleJtranspose; + static const double MaxAnglePseudoinverse; + static const double MaxAngleDLS; + static const double MaxAngleSDLS; + MatrixRmn* Jactive; + + void CalcdTClampedFromdS(); + static const double BaseMaxTargetDist; + +}; + +#endif \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/LinearR2.cpp b/examples/ThirdPartyLibs/BussIK/LinearR2.cpp new file mode 100644 index 000000000..9e3398f65 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/LinearR2.cpp @@ -0,0 +1,101 @@ + /* + * + * Mathematics Subpackage (VrMath) + * + * + * Author: Samuel R. Buss, sbuss@ucsd.edu. + * Web page: http://math.ucsd.edu/~sbuss/MathCG + * + * + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + * + * + */ + +#include "LinearR2.h" + + +#include + +// ****************************************************** +// * VectorR2 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +const VectorR2 VectorR2::Zero(0.0, 0.0); +const VectorR2 VectorR2::UnitX( 1.0, 0.0); +const VectorR2 VectorR2::UnitY( 0.0, 1.0); +const VectorR2 VectorR2::NegUnitX(-1.0, 0.0); +const VectorR2 VectorR2::NegUnitY( 0.0,-1.0); + +const Matrix2x2 Matrix2x2::Identity(1.0, 0.0, 0.0, 1.0); + +// ****************************************************** +// * Matrix2x2 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + +// ****************************************************** +// * LinearMapR2 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + +LinearMapR2 LinearMapR2::Inverse() const // Returns inverse +{ + + + register double detInv = 1.0/(m11*m22 - m12*m21) ; + + return( LinearMapR2( m22*detInv, -m21*detInv, -m12*detInv, m11*detInv ) ); +} + +LinearMapR2& LinearMapR2::Invert() // Converts into inverse. +{ + register double detInv = 1.0/(m11*m22 - m12*m21) ; + + double temp; + temp = m11*detInv; + m11= m22*detInv; + m22=temp; + m12 = -m12*detInv; + m21 = -m22*detInv; + + return ( *this ); +} + +VectorR2 LinearMapR2::Solve(const VectorR2& u) const // Returns solution +{ + // Just uses Inverse() for now. + return ( Inverse()*u ); +} + +// ****************************************************** +// * RotationMapR2 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + + +// *************************************************************** +// * 2-space vector and matrix utilities * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + + + + +// *************************************************************** +// Stream Output Routines * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +ostream& operator<< ( ostream& os, const VectorR2& u ) +{ + return (os << "<" << u.x << "," << u.y << ">"); +} + + diff --git a/examples/ThirdPartyLibs/BussIK/LinearR2.h b/examples/ThirdPartyLibs/BussIK/LinearR2.h new file mode 100644 index 000000000..4c4140c71 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/LinearR2.h @@ -0,0 +1,981 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +// +// Linear Algebra Classes over R2 +// +// +// A. Vector and Position classes +// +// A.1. VectorR2: a column vector of length 2 +// +// A.2. VectorHgR2 - homogenous vector for R2 (a 3-Vector) +// +// B. Matrix Classes +// +// B.1 LinearMapR2 - arbitrary linear map; 2x2 real matrix +// +// B.2 RotationMapR2 - orthonormal 2x2 matrix +// + +#ifndef LINEAR_R2_H +#define LINEAR_R2_H + +#include +#include +#include +#include "MathMisc.h" +using namespace std; + +class VectorR2; // R2 Vector +class VectorHgR2; +class Matrix2x2; +class LinearMapR2; // 2x2 real matrix +class AffineMapR3; // Affine Map (3x4 Matrix) +class RotationMapR2; // 2x2 rotation map + +// ************************************** +// VectorR2 class * +// * * * * * * * * * * * * * * * * * * ** + +class VectorR2 { + +public: + double x, y; // The x & y coordinates. + +public: + VectorR2( ) : x(0.0), y(0.0) {} + VectorR2( double xVal, double yVal ) + : x(xVal), y(yVal) {} + VectorR2( const VectorHgR2& uH ); + + VectorR2& SetZero() { x=0.0; y=0.0; return *this;} + VectorR2& Set( double xx, double yy ) + { x=xx; y=yy; return *this;} + VectorR2& Load( const double* v ); + VectorR2& Load( const float* v ); + void Dump( double* v ) const; + void Dump( float* v ) const; + + static const VectorR2 Zero; + static const VectorR2 UnitX; + static const VectorR2 UnitY; + static const VectorR2 NegUnitX; + static const VectorR2 NegUnitY; + + VectorR2& operator+= ( const VectorR2& v ) + { x+=v.x; y+=v.y; return(*this); } + VectorR2& operator-= ( const VectorR2& v ) + { x-=v.x; y-=v.y; return(*this); } + VectorR2& operator*= ( double m ) + { x*=m; y*=m; return(*this); } + VectorR2& operator/= ( double m ) + { register double mInv = 1.0/m; + x*=mInv; y*=mInv; + return(*this); } + VectorR2 operator- () const { return ( VectorR2(-x, -y) ); } + VectorR2& ArrayProd(const VectorR2&); // Component-wise product + + VectorR2& AddScaled( const VectorR2& u, double s ); + + double Norm() const { return ( sqrt( x*x + y*y ) ); } + double L1Norm() const { return (Max(fabs(x),fabs(y))); } + double Dist( const VectorR2& u ) const; // Distance from u + double DistSq( const VectorR2& u ) const; // Distance from u + double NormSq() const { return ( x*x + y*y ); } + double MaxAbs() const; + VectorR2& Normalize () { *this /= Norm(); return *this;} // No error checking + VectorR2& MakeUnit(); // Normalize() with error checking + VectorR2& ReNormalize(); + bool IsUnit( double tolerance = 1.0e-15 ) const + { register double norm = Norm(); + return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); } + bool IsZero() const { return ( x==0.0 && y==0.0 ); } + bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );} + // tolerance should be non-negative + + VectorR2& Rotate( double theta ); // rotate through angle theta + VectorR2& Rotate( double costheta, double sintheta ); + +}; + +inline VectorR2 operator+( const VectorR2& u, const VectorR2& v ); +inline VectorR2 operator-( const VectorR2& u, const VectorR2& v ); +inline VectorR2 operator*( const VectorR2& u, double m); +inline VectorR2 operator*( double m, const VectorR2& u); +inline VectorR2 operator/( const VectorR2& u, double m); +inline int operator==( const VectorR2& u, const VectorR2& v ); + +inline double operator^ (const VectorR2& u, const VectorR2& v ); // Dot Product +inline VectorR2 ArrayProd ( const VectorR2& u, const VectorR2& v ); + +inline double Mag(const VectorR2& u) { return u.Norm(); } +inline double Dist(const VectorR2& u, const VectorR2& v) { return u.Dist(v); } +inline double DistSq(const VectorR2& u, const VectorR2& v) { return u.DistSq(v); } +inline double NormalizeError (const VectorR2&); + +// **************************************** +// VectorHgR2 class * +// * * * * * * * * * * * * * * * * * * * ** + +class VectorHgR2 { + +public: + double x, y, w; // The x & y & w coordinates. + +public: + VectorHgR2( ) : x(0.0), y(0.0), w(1.0) {} + VectorHgR2( double xVal, double yVal ) + : x(xVal), y(yVal), w(1.0) {} + VectorHgR2( double xVal, double yVal, double wVal ) + : x(xVal), y(yVal), w(wVal) {} + VectorHgR2 ( const VectorR2& u ) : x(u.x), y(u.y), w(1.0) {} +}; + +// ******************************************************************** +// Matrix2x2 - base class for 2x2 matrices * +// * * * * * * * * * * * * * * * * * * * * * ************************** + +class Matrix2x2 { + +public: + double m11, m12, m21, m22; + + // Implements a 2x2 matrix: m_i_j - row-i and column-j entry + + static const Matrix2x2 Identity; + +public: + + inline Matrix2x2(); + inline Matrix2x2( const VectorR2&, const VectorR2& ); // Sets by columns! + inline Matrix2x2( double, double, double, double ); // Sets by columns + + inline void SetIdentity (); // Set to the identity map + inline void SetZero (); // Set to the zero map + inline void Set( const VectorR2&, const VectorR2& ); + inline void Set( double, double, double, double ); + inline void SetByRows( const VectorR2&, const VectorR2& ); + inline void SetByRows( double, double, double, double ); + inline void SetColumn1 ( double, double ); + inline void SetColumn2 ( double, double ); + inline void SetColumn1 ( const VectorR2& ); + inline void SetColumn2 ( const VectorR2& ); + inline VectorR2 Column1() const; + inline VectorR2 Column2() const; + + inline void SetRow1 ( double, double ); + inline void SetRow2 ( double, double ); + inline void SetRow1 ( const VectorR2& ); + inline void SetRow2 ( const VectorR2& ); + inline VectorR2 Row1() const; + inline VectorR2 Row2() const; + + inline void SetDiagonal( double, double ); + inline void SetDiagonal( const VectorR2& ); + inline double Diagonal( int ); + + inline void MakeTranspose(); // Transposes it. + inline void operator*= (const Matrix2x2& B); // Matrix product + inline Matrix2x2& ReNormalize(); + + inline void Transform( VectorR2* ) const; + inline void Transform( const VectorR2& src, VectorR2* dest) const; + +}; + +inline double NormalizeError( const Matrix2x2& ); +inline VectorR2 operator* ( const Matrix2x2&, const VectorR2& ); + +ostream& operator<< ( ostream& os, const Matrix2x2& A ); + + +// ***************************************** +// LinearMapR2 class * +// * * * * * * * * * * * * * * * * * * * * * + +class LinearMapR2 : public Matrix2x2 { + +public: + + LinearMapR2(); + LinearMapR2( const VectorR2&, const VectorR2& ); // Sets by columns! + LinearMapR2( double, double, double, double ); // Sets by columns + LinearMapR2 ( const Matrix2x2& ); + + inline void Negate(); + inline LinearMapR2& operator+= (const Matrix2x2& ); + inline LinearMapR2& operator-= (const Matrix2x2& ); + inline LinearMapR2& operator*= (double); + inline LinearMapR2& operator/= (double); + inline LinearMapR2& operator*= (const Matrix2x2& ); // Matrix product + + inline LinearMapR2 Transpose() const; + inline double Determinant () const; // Returns the determinant + LinearMapR2 Inverse() const; // Returns inverse + LinearMapR2& Invert(); // Converts into inverse. + VectorR2 Solve(const VectorR2&) const; // Returns solution + LinearMapR2 PseudoInverse() const; // Returns pseudo-inverse TO DO + VectorR2 PseudoSolve(const VectorR2&); // Finds least squares solution TO DO +}; + +inline LinearMapR2 operator+ ( const LinearMapR2&, const LinearMapR2&); +inline LinearMapR2 operator- ( const Matrix2x2& ); +inline LinearMapR2 operator- ( const LinearMapR2&, const LinearMapR2&); +inline LinearMapR2 operator* ( const LinearMapR2&, double); +inline LinearMapR2 operator* ( double, const LinearMapR2& ); +inline LinearMapR2 operator/ ( const LinearMapR2&, double ); +inline LinearMapR2 operator* ( const Matrix2x2&, const LinearMapR2& ); +inline LinearMapR2 operator* ( const LinearMapR2&, const Matrix2x2& ); +inline LinearMapR2 operator* ( const LinearMapR2&, const LinearMapR2& ); + // Matrix product (composition) + + +// ******************************************* +// RotationMapR2class * +// * * * * * * * * * * * * * * * * * * * * * * + +class RotationMapR2 : public Matrix2x2 { + +public: + + RotationMapR2(); + RotationMapR2( const VectorR2&, const VectorR2& ); // Sets by columns! + RotationMapR2( double, double, double, double ); // Sets by columns! + + RotationMapR2& SetZero(); // IT IS AN ERROR TO USE THIS FUNCTION! + + inline RotationMapR2& operator*= (const RotationMapR2& ); // Matrix product + + inline RotationMapR2 Transpose() const; + inline RotationMapR2 Inverse() const { return Transpose(); }; // Returns the transpose + inline RotationMapR2& Invert() { MakeTranspose(); return *this; }; // Transposes it. + inline VectorR2 Invert(const VectorR2&) const; // Returns solution +}; + +inline RotationMapR2 operator* ( const RotationMapR2&, const RotationMapR2& ); + // Matrix product (composition) + + + +// *************************************************************** +// * 2-space vector and matrix utilities (prototypes) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns the angle between vectors u and v. +// Use AngleUnit if both vectors are unit vectors +inline double Angle( const VectorR2& u, const VectorR2& v); +inline double AngleUnit( const VectorR2& u, const VectorR2& v ); + +// Returns a righthanded orthonormal basis to complement vector u +// The vector u must be unit. +inline VectorR2 GetOrtho( const VectorR2& u ); + +// Projections + +inline VectorR2 ProjectToUnit ( const VectorR2& u, const VectorR2& v); + // Project u onto v +inline VectorR2 ProjectPerpUnit ( const VectorR2& u, const VectorR2 & v); + // Project perp to v +// v must be a unit vector. + +// Projection maps (LinearMapR2's) + +inline LinearMapR2 VectorProjectMap( const VectorR2& u ); + +inline LinearMapR2 PerpProjectMap ( const VectorR2& u ); +// u - must be unit vector. + +// Rotation Maps + +inline RotationMapR2 RotateToMap( const VectorR2& fromVec, const VectorR2& toVec); +// fromVec and toVec should be unit vectors + + + +// *************************************************************** +// * Stream Output Routines (Prototypes) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +ostream& operator<< ( ostream& os, const VectorR2& u ); + + +// ***************************************************** +// * VectorR2 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * + +inline VectorR2& VectorR2::Load( const double* v ) +{ + x = *v; + y = *(v+1); + return *this; +} + +inline VectorR2& VectorR2::Load( const float* v ) +{ + x = *v; + y = *(v+1); + return *this; +} + +inline void VectorR2::Dump( double* v ) const +{ + *v = x; + *(v+1) = y; +} + +inline void VectorR2::Dump( float* v ) const +{ + *v = (float)x; + *(v+1) = (float)y; +} + +inline VectorR2& VectorR2::ArrayProd (const VectorR2& v) // Component-wise Product +{ + x *= v.x; + y *= v.y; + return ( *this ); +} + +inline VectorR2& VectorR2::MakeUnit () // Convert to unit vector (or leave zero). +{ + double nSq = NormSq(); + if (nSq != 0.0) { + *this /= sqrt(nSq); + } + return *this; +} + +inline VectorR2& VectorR2::ReNormalize() // Convert near unit back to unit +{ + double nSq = NormSq(); + register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor + *this *= mFact; + return *this; +} + +// Rotate through angle theta +inline VectorR2& VectorR2::Rotate( double theta ) +{ + double costheta = cos(theta); + double sintheta = sin(theta); + double tempx = x*costheta - y*sintheta; + y = y*costheta + x*sintheta; + x = tempx; + return *this; +} + +inline VectorR2& VectorR2::Rotate( double costheta, double sintheta ) +{ + double tempx = x*costheta + y*sintheta; + y = y*costheta - x*sintheta; + x = tempx; + return *this; +} + +inline double VectorR2::MaxAbs() const +{ + register double m; + m = (x>=0.0) ? x : -x; + if ( y>m ) m=y; + else if ( -y >m ) m = -y; + return m; +} + +inline VectorR2 operator+( const VectorR2& u, const VectorR2& v ) +{ + return VectorR2(u.x+v.x, u.y+v.y ); +} +inline VectorR2 operator-( const VectorR2& u, const VectorR2& v ) +{ + return VectorR2(u.x-v.x, u.y-v.y ); +} +inline VectorR2 operator*( const VectorR2& u, register double m) +{ + return VectorR2( u.x*m, u.y*m ); +} +inline VectorR2 operator*( register double m, const VectorR2& u) +{ + return VectorR2( u.x*m, u.y*m ); +} +inline VectorR2 operator/( const VectorR2& u, double m) +{ + register double mInv = 1.0/m; + return VectorR2( u.x*mInv, u.y*mInv ); +} + +inline int operator==( const VectorR2& u, const VectorR2& v ) +{ + return ( u.x==v.x && u.y==v.y ); +} + +inline double operator^ ( const VectorR2& u, const VectorR2& v ) // Dot Product +{ + return ( u.x*v.x + u.y*v.y ); +} + +inline VectorR2 ArrayProd ( const VectorR2& u, const VectorR2& v ) +{ + return ( VectorR2( u.x*v.x, u.y*v.y ) ); +} + +inline VectorR2& VectorR2::AddScaled( const VectorR2& u, double s ) +{ + x += s*u.x; + y += s*u.y; + return(*this); +} + +inline VectorR2::VectorR2( const VectorHgR2& uH ) +: x(uH.x), y(uH.y) +{ + *this /= uH.w; +} + +inline double NormalizeError (const VectorR2& u) +{ + register double discrepancy; + discrepancy = u.x*u.x + u.y*u.y - 1.0; + if ( discrepancy < 0.0 ) { + discrepancy = -discrepancy; + } + return discrepancy; +} + +inline double VectorR2::Dist( const VectorR2& u ) const // Distance from u +{ + return sqrt( DistSq(u) ); +} + +inline double VectorR2::DistSq( const VectorR2& u ) const // Distance from u +{ + return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) ); +} + + +// ********************************************************* +// * Matrix2x2 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ***** + +inline Matrix2x2::Matrix2x2() {} + +inline Matrix2x2::Matrix2x2( const VectorR2& u, const VectorR2& v ) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m12 = v.x; // Column 2 + m22 = v.y; +} + +inline Matrix2x2::Matrix2x2( double a11, double a21, double a12, double a22 ) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m21 = a21; // Row 2 + m22 = a22; +} + +inline void Matrix2x2::SetIdentity ( ) +{ + m11 = m22 = 1.0; + m12 = m21 = 0.0; +} + +inline void Matrix2x2::Set( const VectorR2& u, const VectorR2& v ) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m12 = v.x; // Column 2 + m22 = v.y; +} + +inline void Matrix2x2::Set( double a11, double a21, double a12, double a22 ) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m21 = a21; // Row 2 + m22 = a22; +} + +inline void Matrix2x2::SetZero( ) +{ + m11 = m12 = m21 = m22 = 0.0; +} + +inline void Matrix2x2::SetByRows( const VectorR2& u, const VectorR2& v ) +{ + m11 = u.x; // Row 1 + m12 = u.y; + m21 = v.x; // Row 2 + m22 = v.y; +} + +inline void Matrix2x2::SetByRows( double a11, double a12, double a21, double a22 ) + // Values specified in row order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m21 = a21; // Row 2 + m22 = a22; +} + +inline void Matrix2x2::SetColumn1 ( double x, double y ) +{ + m11 = x; m21 = y; +} + +inline void Matrix2x2::SetColumn2 ( double x, double y ) +{ + m12 = x; m22 = y; +} + +inline void Matrix2x2::SetColumn1 ( const VectorR2& u ) +{ + m11 = u.x; m21 = u.y; +} + +inline void Matrix2x2::SetColumn2 ( const VectorR2& u ) +{ + m12 = u.x; m22 = u.y; +} + +VectorR2 Matrix2x2::Column1() const +{ + return ( VectorR2(m11, m21) ); +} + +VectorR2 Matrix2x2::Column2() const +{ + return ( VectorR2(m12, m22) ); +} + +inline void Matrix2x2::SetRow1 ( double x, double y ) +{ + m11 = x; m12 = y; +} + +inline void Matrix2x2::SetRow2 ( double x, double y ) +{ + m21 = x; m22 = y; +} + +inline void Matrix2x2::SetRow1 ( const VectorR2& u ) +{ + m11 = u.x; m12 = u.y; +} + +inline void Matrix2x2::SetRow2 ( const VectorR2& u ) +{ + m21 = u.x; m22 = u.y; +} + +VectorR2 Matrix2x2::Row1() const +{ + return ( VectorR2(m11, m12) ); +} + +VectorR2 Matrix2x2::Row2() const +{ + return ( VectorR2(m21, m22) ); +} + +inline void Matrix2x2::SetDiagonal( double x, double y ) +{ + m11 = x; + m22 = y; +} + +inline void Matrix2x2::SetDiagonal( const VectorR2& u ) +{ + SetDiagonal ( u.x, u.y ); +} + +inline double Matrix2x2::Diagonal( int i ) +{ + switch (i) { + case 0: + return m11; + case 1: + return m22; + default: + assert(0); + return 0.0; + } +} +inline void Matrix2x2::MakeTranspose() // Transposes it. +{ + register double temp; + temp = m12; + m12 = m21; + m21=temp; +} + +inline void Matrix2x2::operator*= (const Matrix2x2& B) // Matrix product +{ + double t1; // temporary value + + t1 = m11*B.m11 + m12*B.m21; + m12 = m11*B.m12 + m12*B.m22; + m11 = t1; + + t1 = m21*B.m11 + m22*B.m21; + m22 = m21*B.m12 + m22*B.m22; + m21 = t1; +} + +inline Matrix2x2& Matrix2x2::ReNormalize() // Re-normalizes nearly orthonormal matrix +{ + register double alpha = m11*m11+m21*m21; // First column's norm squared + register double beta = m12*m12+m22*m22; // Second column's norm squared + alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor + beta = 1.0 - 0.5*(beta-1.0); + m11 *= alpha; // Renormalize first column + m21 *= alpha; + m12 *= beta; // Renormalize second column + m22 *= beta; + alpha = m11*m12+m21*m22; // Columns' inner product + alpha *= 0.5; // times 1/2 + register double temp; + temp = m11-alpha*m12; // Subtract alpha times other column + m12 -= alpha*m11; + m11 = temp; + temp = m21-alpha*m22; + m22 -= alpha*m21; + m11 = temp; + return *this; +} + +// Gives a measure of how far the matrix is from being normalized. +// Mostly intended for diagnostic purposes. +inline double NormalizeError( const Matrix2x2& A) +{ + register double discrepancy; + register double newdisc; + discrepancy = A.m11*A.m11 + A.m21*A.m21 -1.0; // First column - inner product - 1 + if (discrepancy<0.0) { + discrepancy = -discrepancy; + } + newdisc = A.m12*A.m12 + A.m22*A.m22 - 1.0; // Second column inner product - 1 + if ( newdisc<0.0 ) { + newdisc = -newdisc; + } + if ( newdisc>discrepancy ) { + discrepancy = newdisc; + } + newdisc = A.m11*A.m12 + A.m21*A.m22; // Inner product of two columns + if ( newdisc<0.0 ) { + newdisc = -newdisc; + } + if ( newdisc>discrepancy ) { + discrepancy = newdisc; + } + return discrepancy; +} + +inline VectorR2 operator* ( const Matrix2x2& A, const VectorR2& u) +{ + return(VectorR2 ( A.m11*u.x + A.m12*u.y, + A.m21*u.x + A.m22*u.y ) ); +} + +inline void Matrix2x2::Transform( VectorR2* u ) const { + double newX; + newX = m11*u->x + m12*u->y; + u->y = m21*u->x + m22*u->y; + u->x = newX; +} + +inline void Matrix2x2::Transform( const VectorR2& src, VectorR2* dest ) const { + dest->x = m11*src.x + m12*src.y; + dest->y = m21*src.x + m22*src.y; +} + + + +// ****************************************************** +// * LinearMapR2 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline LinearMapR2::LinearMapR2() +{ + SetZero(); + return; +} + +inline LinearMapR2::LinearMapR2( const VectorR2& u, const VectorR2& v ) +:Matrix2x2 ( u, v ) +{ } + +inline LinearMapR2::LinearMapR2(double a11, double a21, double a12, double a22) + // Values specified in column order!!! +:Matrix2x2 ( a11, a21, a12, a22 ) +{ } + +inline LinearMapR2::LinearMapR2 ( const Matrix2x2& A ) +: Matrix2x2 (A) +{} + +inline void LinearMapR2::Negate () +{ + m11 = -m11; + m12 = -m12; + m21 = -m21; + m22 = -m22; +} + +inline LinearMapR2& LinearMapR2::operator+= (const Matrix2x2& B) +{ + m11 += B.m11; + m12 += B.m12; + m21 += B.m21; + m22 += B.m22; + return ( *this ); +} + +inline LinearMapR2& LinearMapR2::operator-= (const Matrix2x2& B) +{ + m11 -= B.m11; + m12 -= B.m12; + m21 -= B.m21; + m22 -= B.m22; + return( *this ); +} + +inline LinearMapR2 operator+ (const LinearMapR2& A, const LinearMapR2& B) +{ + return( LinearMapR2( A.m11+B.m11, A.m21+B.m21, + A.m12+B.m12, A.m22+B.m22 ) ); +} + +inline LinearMapR2 operator- (const Matrix2x2& A) +{ + return( LinearMapR2( -A.m11, -A.m21, -A.m12, -A.m22 ) ); +} + +inline LinearMapR2 operator- (const LinearMapR2& A, const LinearMapR2& B) +{ + return( LinearMapR2( A.m11-B.m11, A.m21-B.m21, + A.m12-B.m12, A.m22-B.m22 ) ); +} + +inline LinearMapR2& LinearMapR2::operator*= (register double b) +{ + m11 *= b; + m12 *= b; + m21 *= b; + m22 *= b; + return ( *this); +} + +inline LinearMapR2 operator* ( const LinearMapR2& A, register double b) +{ + return( LinearMapR2( A.m11*b, A.m21*b, + A.m12*b, A.m22*b ) ); +} + +inline LinearMapR2 operator* ( register double b, const LinearMapR2& A) +{ + return( LinearMapR2( A.m11*b, A.m21*b, + A.m12*b, A.m22*b ) ); +} + +inline LinearMapR2 operator/ ( const LinearMapR2& A, double b) +{ + register double bInv = 1.0/b; + return ( A*bInv ); +} + +inline LinearMapR2& LinearMapR2::operator/= (register double b) +{ + register double bInv = 1.0/b; + return ( *this *= bInv ); +} + +inline LinearMapR2 LinearMapR2::Transpose() const // Returns the transpose +{ + return (LinearMapR2( m11, m12, m21, m22 ) ); +} + +inline LinearMapR2& LinearMapR2::operator*= (const Matrix2x2& B) // Matrix product +{ + (*this).Matrix2x2::operator*=(B); + + return( *this ); +} + +inline LinearMapR2 operator* ( const LinearMapR2& A, const Matrix2x2& B) +{ + LinearMapR2 AA(A); + AA.Matrix2x2::operator*=(B); + return AA; +} + +inline LinearMapR2 operator* ( const Matrix2x2& A, const LinearMapR2& B) +{ + LinearMapR2 AA(A); + AA.Matrix2x2::operator*=(B); + return AA; +} + +inline LinearMapR2 operator* ( const LinearMapR2& A, const LinearMapR2& B) +{ + LinearMapR2 AA(A); + AA.Matrix2x2::operator*=(B); + return AA; +} + +inline double LinearMapR2::Determinant () const // Returns the determinant +{ + return ( m11*m22 - m12*m21 ); +} + +// ****************************************************** +// * RotationMapR2 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline RotationMapR2::RotationMapR2() +{ + SetIdentity(); + return; +} + +inline RotationMapR2::RotationMapR2( const VectorR2& u, const VectorR2& v ) +:Matrix2x2 ( u, v ) +{ } + +inline RotationMapR2::RotationMapR2( + double a11, double a21, double a12, double a22 ) + // Values specified in column order!!! +:Matrix2x2 ( a11, a21, a12, a22 ) +{ } + +inline RotationMapR2 RotationMapR2::Transpose() const // Returns the transpose +{ + return ( RotationMapR2( m11, m12, + m21, m22 ) ); +} + +inline VectorR2 RotationMapR2::Invert(const VectorR2& u) const // Returns solution +{ + return (VectorR2( m11*u.x + m21*u.y, // Multiply with Transpose + m12*u.x + m22*u.y ) ); +} + +inline RotationMapR2& RotationMapR2::operator*= (const RotationMapR2& B) // Matrix product +{ + (*this).Matrix2x2::operator*=(B); + + return( *this ); +} + +inline RotationMapR2 operator* ( const RotationMapR2& A, const RotationMapR2& B) +{ + RotationMapR2 AA(A); + AA.Matrix2x2::operator*=(B); + return AA; +} + + +// *************************************************************** +// * 2-space vector and matrix utilities (inlined functions) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns a righthanded orthonormal basis to complement vector u +// The vector u must be unit. +inline VectorR2 GetOrtho( const VectorR2& u ) +{ + return VectorR2 ( -u.y, u.x ); +} + +// Returns the projection of u onto unit v +inline VectorR2 ProjectToUnit ( const VectorR2& u, const VectorR2& v) +{ + return (u^v)*v; +} + +// Returns the projection of u onto the plane perpindicular to the unit vector v +inline VectorR2 ProjectPerpUnit ( const VectorR2& u, const VectorR2& v) +{ + return ( u - ((u^v)*v) ); +} + +// Returns the projection of u onto the plane perpindicular to the unit vector v +// This one is more stable when u and v are nearly equal. +inline VectorR2 ProjectPerpUnitDiff ( const VectorR2& u, const VectorR2& v) +{ + VectorR2 ans = u; + ans -= v; + ans -= ((ans^v)*v); + return ans; // ans = (u-v) - ((u-v)^v)*v +} + +// Returns the solid angle between vectors u and v. +inline double Angle( const VectorR2& u, const VectorR2& v) +{ + double nSqU = u.NormSq(); + double nSqV = v.NormSq(); + if ( nSqU==0.0 && nSqV==0.0 ) { + return (0.0); + } + else { + return ( AngleUnit( u/sqrt(nSqU), v/sqrt(nSqV) ) ); + } +} + +inline double AngleUnit( const VectorR2& u, const VectorR2& v ) +{ + return ( atan2 ( (ProjectPerpUnit(v,u)).Norm(), u^v ) ); +} + +// Projection maps (LinearMapR2's) + +// VectorProjectMap returns map projecting onto a given vector u. +// u should be a unit vector (otherwise the returned map is +// scaled according to the magnitude of u. +inline LinearMapR2 VectorProjectMap( const VectorR2& u ) +{ + double xy = u.x*u.y; + return( LinearMapR2( u.x*u.x, xy, xy, u.y*u.y ) ) ; +} + +// PlaneProjectMap returns map projecting onto a given plane. +// The plane is the plane orthognal to u. +// u must be a unit vector (otherwise the returned map is +// garbage). +inline LinearMapR2 PerpProjectMap ( const VectorR2& u ) +{ + double nxy = -u.x*u.y; + return ( LinearMapR2 ( 1.0-u.x*u.x, nxy, nxy, 1.0-u.y*u.y ) ); +} + +// fromVec and toVec should be unit vectors +inline RotationMapR2 RotateToMap( const VectorR2& fromVec, const VectorR2& toVec) +{ + double costheta = fromVec.x*toVec.x + fromVec.y*toVec.y; + double sintheta = fromVec.x*toVec.y - fromVec.y*toVec.x; + return( RotationMapR2( costheta, sintheta, -sintheta, costheta ) ); +} + +#endif // LINEAR_R2_H diff --git a/examples/ThirdPartyLibs/BussIK/LinearR3.cpp b/examples/ThirdPartyLibs/BussIK/LinearR3.cpp new file mode 100644 index 000000000..c74d2b2b5 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/LinearR3.cpp @@ -0,0 +1,822 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + + +#include "MathMisc.h" +#include "LinearR3.h" +#include "Spherical.h" + +// ****************************************************** +// * VectorR3 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +const VectorR3 UnitVecIR3(1.0, 0.0, 0.0); +const VectorR3 UnitVecJR3(0.0, 1.0, 0.0); +const VectorR3 UnitVecKR3(0.0, 0.0, 1.0); + +const VectorR3 VectorR3::Zero(0.0, 0.0, 0.0); +const VectorR3 VectorR3::UnitX( 1.0, 0.0, 0.0); +const VectorR3 VectorR3::UnitY( 0.0, 1.0, 0.0); +const VectorR3 VectorR3::UnitZ( 0.0, 0.0, 1.0); +const VectorR3 VectorR3::NegUnitX(-1.0, 0.0, 0.0); +const VectorR3 VectorR3::NegUnitY( 0.0,-1.0, 0.0); +const VectorR3 VectorR3::NegUnitZ( 0.0, 0.0,-1.0); + +const Matrix3x3 Matrix3x3::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); +const Matrix3x4 Matrix3x4::Identity(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0); + +double VectorR3::MaxAbs() const +{ + register double m; + m = (x>0.0) ? x : -x; + if ( y>m ) m=y; + else if ( -y >m ) m = -y; + if ( z>m ) m=z; + else if ( -z>m ) m = -z; + return m; +} + +VectorR3& VectorR3::Set( const Quaternion& q ) +{ + double sinhalf = sqrt( Square(q.x)+Square(q.y)+Square(q.z) ); + if (sinhalf>0.0) { + double theta = atan2( sinhalf, q.w ); + theta += theta; + this->Set( q.x, q.y, q.z ); + (*this) *= (theta/sinhalf); + } + else { + this->SetZero(); + } + return *this; +} + + +// ********************************************************************* +// Rotation routines * +// ********************************************************************* + +// s.Rotate(theta, u) rotates s and returns s +// rotated theta degrees around unit vector w. +VectorR3& VectorR3::Rotate( double theta, const VectorR3& w) +{ + double c = cos(theta); + double s = sin(theta); + double dotw = (x*w.x + y*w.y + z*w.z); + double v0x = dotw*w.x; + double v0y = dotw*w.y; // v0 = provjection onto w + double v0z = dotw*w.z; + double v1x = x-v0x; + double v1y = y-v0y; // v1 = projection onto plane normal to w + double v1z = z-v0z; + double v2x = w.y*v1z - w.z*v1y; + double v2y = w.z*v1x - w.x*v1z; // v2 = w * v1 (cross product) + double v2z = w.x*v1y - w.y*v1x; + + x = v0x + c*v1x + s*v2x; + y = v0y + c*v1y + s*v2y; + z = v0z + c*v1z + s*v2z; + + return ( *this ); +} + +// Rotate unit vector x in the direction of "dir": length of dir is rotation angle. +// x must be a unit vector. dir must be perpindicular to x. +VectorR3& VectorR3::RotateUnitInDirection ( const VectorR3& dir) +{ + double theta = dir.NormSq(); + if ( theta==0.0 ) { + return *this; + } + else { + theta = sqrt(theta); + double costheta = cos(theta); + double sintheta = sin(theta); + VectorR3 dirUnit = dir/theta; + *this = costheta*(*this) + sintheta*dirUnit; + return ( *this ); + } +} + +// ****************************************************** +// * Matrix3x3 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +Matrix3x3& Matrix3x3::ReNormalize() // Re-normalizes nearly orthonormal matrix +{ + register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared + register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared + register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared + alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor + beta = 1.0 - 0.5*(beta-1.0); + gamma = 1.0 - 0.5*(gamma-1.0); + m11 *= alpha; // Renormalize first column + m21 *= alpha; + m31 *= alpha; + m12 *= beta; // Renormalize second column + m22 *= beta; + m32 *= beta; + m13 *= gamma; + m23 *= gamma; + m33 *= gamma; + alpha = m11*m12+m21*m22+m31*m32; // First and second column dot product + beta = m11*m13+m21*m23+m31*m33; // First and third column dot product + gamma = m12*m13+m22*m23+m32*m33; // Second and third column dot product + alpha *= 0.5; + beta *= 0.5; + gamma *= 0.5; + register double temp1, temp2; + temp1 = m11-alpha*m12-beta*m13; // Update row1 + temp2 = m12-alpha*m11-gamma*m13; + m13 -= beta*m11+gamma*m12; + m11 = temp1; + m12 = temp2; + temp1 = m21-alpha*m22-beta*m23; // Update row2 + temp2 = m22-alpha*m21-gamma*m23; + m23 -= beta*m21+gamma*m22; + m21 = temp1; + m22 = temp2; + temp1 = m31-alpha*m32-beta*m33; // Update row3 + temp2 = m32-alpha*m31-gamma*m33; + m33 -= beta*m31+gamma*m32; + m31 = temp1; + m32 = temp2; + return *this; +} + +void Matrix3x3::OperatorTimesEquals(const Matrix3x3& B) // Matrix product +{ + double t1, t2; // temporary values + t1 = m11*B.m11 + m12*B.m21 + m13*B.m31; + t2 = m11*B.m12 + m12*B.m22 + m13*B.m32; + m13 = m11*B.m13 + m12*B.m23 + m13*B.m33; + m11 = t1; + m12 = t2; + + t1 = m21*B.m11 + m22*B.m21 + m23*B.m31; + t2 = m21*B.m12 + m22*B.m22 + m23*B.m32; + m23 = m21*B.m13 + m22*B.m23 + m23*B.m33; + m21 = t1; + m22 = t2; + + t1 = m31*B.m11 + m32*B.m21 + m33*B.m31; + t2 = m31*B.m12 + m32*B.m22 + m33*B.m32; + m33 = m31*B.m13 + m32*B.m23 + m33*B.m33; + m31 = t1; + m32 = t2; + return; +} + +VectorR3 Matrix3x3::Solve(const VectorR3& u) const // Returns solution +{ // based on Cramer's rule + double sd11 = m22*m33-m23*m32; + double sd21 = m32*m13-m12*m33; + double sd31 = m12*m23-m22*m13; + double sd12 = m31*m23-m21*m33; + double sd22 = m11*m33-m31*m13; + double sd32 = m21*m13-m11*m23; + double sd13 = m21*m32-m31*m22; + double sd23 = m31*m12-m11*m32; + double sd33 = m11*m22-m21*m12; + + register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + + double rx = (u.x*sd11 + u.y*sd21 + u.z*sd31)*detInv; + double ry = (u.x*sd12 + u.y*sd22 + u.z*sd32)*detInv; + double rz = (u.x*sd13 + u.y*sd23 + u.z*sd33)*detInv; + + return ( VectorR3( rx, ry, rz ) ); +} + + +// ****************************************************** +// * Matrix3x4 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +Matrix3x4& Matrix3x4::ReNormalize() // Re-normalizes nearly orthonormal matrix +{ + register double alpha = m11*m11+m21*m21+m31*m31; // First column's norm squared + register double beta = m12*m12+m22*m22+m32*m32; // Second column's norm squared + register double gamma = m13*m13+m23*m23+m33*m33; // Third column's norm squared + alpha = 1.0 - 0.5*(alpha-1.0); // Get mult. factor + beta = 1.0 - 0.5*(beta-1.0); + gamma = 1.0 - 0.5*(gamma-1.0); + m11 *= alpha; // Renormalize first column + m21 *= alpha; + m31 *= alpha; + m12 *= beta; // Renormalize second column + m22 *= beta; + m32 *= beta; + m13 *= gamma; + m23 *= gamma; + m33 *= gamma; + alpha = m11*m12+m21*m22+m31*m32; // First and second column dot product + beta = m11*m13+m21*m23+m31*m33; // First and third column dot product + gamma = m12*m13+m22*m23+m32*m33; // Second and third column dot product + alpha *= 0.5; + beta *= 0.5; + gamma *= 0.5; + register double temp1, temp2; + temp1 = m11-alpha*m12-beta*m13; // Update row1 + temp2 = m12-alpha*m11-gamma*m13; + m13 -= beta*m11+gamma*m12; + m11 = temp1; + m12 = temp2; + temp1 = m21-alpha*m22-beta*m23; // Update row2 + temp2 = m22-alpha*m21-gamma*m23; + m23 -= beta*m21+gamma*m22; + m21 = temp1; + m22 = temp2; + temp1 = m31-alpha*m32-beta*m33; // Update row3 + temp2 = m32-alpha*m31-gamma*m33; + m33 -= beta*m31+gamma*m32; + m31 = temp1; + m32 = temp2; + return *this; +} + +void Matrix3x4::OperatorTimesEquals (const Matrix3x4& B) // Composition +{ + m14 += m11*B.m14 + m12*B.m24 + m13*B.m34; + m24 += m21*B.m14 + m22*B.m24 + m23*B.m34; + m34 += m31*B.m14 + m32*B.m24 + m33*B.m34; + + double t1, t2; // temporary values + t1 = m11*B.m11 + m12*B.m21 + m13*B.m31; + t2 = m11*B.m12 + m12*B.m22 + m13*B.m32; + m13 = m11*B.m13 + m12*B.m23 + m13*B.m33; + m11 = t1; + m12 = t2; + + t1 = m21*B.m11 + m22*B.m21 + m23*B.m31; + t2 = m21*B.m12 + m22*B.m22 + m23*B.m32; + m23 = m21*B.m13 + m22*B.m23 + m23*B.m33; + m21 = t1; + m22 = t2; + + t1 = m31*B.m11 + m32*B.m21 + m33*B.m31; + t2 = m31*B.m12 + m32*B.m22 + m33*B.m32; + m33 = m31*B.m13 + m32*B.m23 + m33*B.m33; + m31 = t1; + m32 = t2; +} + +void Matrix3x4::OperatorTimesEquals (const Matrix3x3& B) // Composition +{ + double t1, t2; // temporary values + t1 = m11*B.m11 + m12*B.m21 + m13*B.m31; + t2 = m11*B.m12 + m12*B.m22 + m13*B.m32; + m13 = m11*B.m13 + m12*B.m23 + m13*B.m33; + m11 = t1; + m12 = t2; + + t1 = m21*B.m11 + m22*B.m21 + m23*B.m31; + t2 = m21*B.m12 + m22*B.m22 + m23*B.m32; + m23 = m21*B.m13 + m22*B.m23 + m23*B.m33; + m21 = t1; + m22 = t2; + + t1 = m31*B.m11 + m32*B.m21 + m33*B.m31; + t2 = m31*B.m12 + m32*B.m22 + m33*B.m32; + m33 = m31*B.m13 + m32*B.m23 + m33*B.m33; + m31 = t1; + m32 = t2; +} + +// ****************************************************** +// * LinearMapR3 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + +LinearMapR3 operator* ( const LinearMapR3& A, const LinearMapR3& B) +{ + return( LinearMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31, + A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31, + A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31, + A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32, + A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32, + A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32, + A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33, + A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33, + A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 ) ); +} + +double LinearMapR3::Determinant () const // Returns the determinant +{ + return ( m11*(m22*m33-m23*m32) + - m12*(m21*m33-m31*m23) + + m13*(m21*m23-m31*m22) ); +} + +LinearMapR3 LinearMapR3::Inverse() const // Returns inverse +{ + double sd11 = m22*m33-m23*m32; + double sd21 = m32*m13-m12*m33; + double sd31 = m12*m23-m22*m13; + double sd12 = m31*m23-m21*m33; + double sd22 = m11*m33-m31*m13; + double sd32 = m21*m13-m11*m23; + double sd13 = m21*m32-m31*m22; + double sd23 = m31*m12-m11*m32; + double sd33 = m11*m22-m21*m12; + + register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + + return( LinearMapR3( sd11*detInv, sd12*detInv, sd13*detInv, + sd21*detInv, sd22*detInv, sd23*detInv, + sd31*detInv, sd32*detInv, sd33*detInv ) ); +} + +LinearMapR3& LinearMapR3::Invert() // Converts into inverse. +{ + double sd11 = m22*m33-m23*m32; + double sd21 = m32*m13-m12*m33; + double sd31 = m12*m23-m22*m13; + double sd12 = m31*m23-m21*m33; + double sd22 = m11*m33-m31*m13; + double sd32 = m21*m13-m11*m23; + double sd13 = m21*m32-m31*m22; + double sd23 = m31*m12-m11*m32; + double sd33 = m11*m22-m21*m12; + + register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + + m11 = sd11*detInv; + m12 = sd21*detInv; + m13 = sd31*detInv; + m21 = sd12*detInv; + m22 = sd22*detInv; + m23 = sd32*detInv; + m31 = sd13*detInv; + m32 = sd23*detInv; + m33 = sd33*detInv; + + return ( *this ); +} + + +// ****************************************************** +// * AffineMapR3 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +AffineMapR3 operator* ( const AffineMapR3& A, const AffineMapR3& B ) +{ + return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31, + A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31, + A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31, + A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32, + A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32, + A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32, + A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33, + A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33, + A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33, + A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34 + A.m14, + A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34 + A.m24, + A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 + A.m34)); +} + +AffineMapR3 operator* ( const LinearMapR3& A, const AffineMapR3& B ) +{ + return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31, + A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31, + A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31, + A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32, + A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32, + A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32, + A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33, + A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33, + A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33, + A.m11*B.m14 + A.m12*B.m24 + A.m13*B.m34, + A.m21*B.m14 + A.m22*B.m24 + A.m23*B.m34, + A.m31*B.m14 + A.m32*B.m24 + A.m33*B.m34 )); + +} + +AffineMapR3 operator* ( const AffineMapR3& A, const LinearMapR3& B ) +{ + return( AffineMapR3( A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31, + A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31, + A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31, + A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32, + A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32, + A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32, + A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33, + A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33, + A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33, + A.m14, + A.m24, + A.m34 ) ); +} + + +AffineMapR3 AffineMapR3::Inverse() const // Returns inverse +{ + double sd11 = m22*m33-m23*m32; + double sd21 = m32*m13-m12*m33; + double sd31 = m12*m23-m22*m13; + double sd12 = m31*m23-m21*m33; + double sd22 = m11*m33-m31*m13; + double sd32 = m21*m13-m11*m23; + double sd13 = m21*m32-m31*m22; + double sd23 = m31*m12-m11*m32; + double sd33 = m11*m22-m21*m12; + + register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + + // Make sd's hold the (transpose of) the inverse of the 3x3 part + sd11 *= detInv; + sd12 *= detInv; + sd13 *= detInv; + sd21 *= detInv; + sd22 *= detInv; + sd23 *= detInv; + sd31 *= detInv; + sd32 *= detInv; + sd33 *= detInv; + double sd41 = -(m14*sd11 + m24*sd21 + m34*sd31); + double sd42 = -(m14*sd12 + m24*sd22 + m34*sd32); + double sd43 = -(m14*sd12 + m24*sd23 + m34*sd33); + + return( AffineMapR3( sd11, sd12, sd13, + sd21, sd22, sd23, + sd31, sd32, sd33, + sd41, sd42, sd43 ) ); +} + +AffineMapR3& AffineMapR3::Invert() // Converts into inverse. +{ + double sd11 = m22*m33-m23*m32; + double sd21 = m32*m13-m12*m33; + double sd31 = m12*m23-m22*m13; + double sd12 = m31*m23-m21*m33; + double sd22 = m11*m33-m31*m13; + double sd32 = m21*m13-m11*m23; + double sd13 = m21*m32-m31*m22; + double sd23 = m31*m12-m11*m32; + double sd33 = m11*m22-m21*m12; + + register double detInv = 1.0/(m11*sd11 + m12*sd12 + m13*sd13); + + m11 = sd11*detInv; + m12 = sd21*detInv; + m13 = sd31*detInv; + m21 = sd12*detInv; + m22 = sd22*detInv; + m23 = sd32*detInv; + m31 = sd13*detInv; + m32 = sd23*detInv; + m33 = sd33*detInv; + double sd41 = -(m14*m11 + m24*m12 + m34*m13); // Compare to ::Inverse. + double sd42 = -(m14*m21 + m24*m22 + m34*m23); + double sd43 = -(m14*m31 + m24*m32 + m34*m33); + m14 = sd41; + m24 = sd42; + m34 = sd43; + + return ( *this ); +} + +// ************************************************************** +// * RotationMapR3 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +RotationMapR3 operator*(const RotationMapR3& A, const RotationMapR3& B) + // Matrix product (composition) +{ + return(RotationMapR3(A.m11*B.m11 + A.m12*B.m21 + A.m13*B.m31, + A.m21*B.m11 + A.m22*B.m21 + A.m23*B.m31, + A.m31*B.m11 + A.m32*B.m21 + A.m33*B.m31, + A.m11*B.m12 + A.m12*B.m22 + A.m13*B.m32, + A.m21*B.m12 + A.m22*B.m22 + A.m23*B.m32, + A.m31*B.m12 + A.m32*B.m22 + A.m33*B.m32, + A.m11*B.m13 + A.m12*B.m23 + A.m13*B.m33, + A.m21*B.m13 + A.m22*B.m23 + A.m23*B.m33, + A.m31*B.m13 + A.m32*B.m23 + A.m33*B.m33 ) ); +} + +RotationMapR3& RotationMapR3::Set( const Quaternion& quat ) +{ + double wSq = quat.w*quat.w; + double xSq = quat.x*quat.x; + double ySq = quat.y*quat.y; + double zSq = quat.z*quat.z; + double Dqwx = 2.0*quat.w*quat.x; + double Dqwy = 2.0*quat.w*quat.y; + double Dqwz = 2.0*quat.w*quat.z; + double Dqxy = 2.0*quat.x*quat.y; + double Dqyz = 2.0*quat.y*quat.z; + double Dqxz = 2.0*quat.x*quat.z; + m11 = wSq+xSq-ySq-zSq; + m22 = wSq-xSq+ySq-zSq; + m33 = wSq-xSq-ySq+zSq; + m12 = Dqxy-Dqwz; + m21 = Dqxy+Dqwz; + m13 = Dqxz+Dqwy; + m31 = Dqxz-Dqwy; + m23 = Dqyz-Dqwx; + m32 = Dqyz+Dqwx; + return *this; +} + +RotationMapR3& RotationMapR3::Set( const VectorR3& u, double theta ) +{ + assert ( fabs(u.NormSq()-1.0)<2.0e-6 ); + register double c = cos(theta); + register double s = sin(theta); + register double mc = 1.0-c; + double xmc = u.x*mc; + double xymc = xmc*u.y; + double xzmc = xmc*u.z; + double yzmc = u.y*u.z*mc; + double xs = u.x*s; + double ys = u.y*s; + double zs = u.z*s; + Matrix3x3::Set( u.x*u.x*mc+c, xymc+zs, xzmc-ys, + xymc-zs, u.y*u.y*mc+c, yzmc+xs, + xzmc+ys, yzmc-xs, u.z*u.z*mc+c ); + return *this; +} + +// The rotation axis vector u MUST be a UNIT vector!!! +RotationMapR3& RotationMapR3::Set( const VectorR3& u, double s, double c ) +{ + assert ( fabs(u.NormSq()-1.0)<2.0e-6 ); + double mc = 1.0-c; + double xmc = u.x*mc; + double xymc = xmc*u.y; + double xzmc = xmc*u.z; + double yzmc = u.y*u.z*mc; + double xs = u.x*s; + double ys = u.y*s; + double zs = u.z*s; + Matrix3x3::Set( u.x*u.x*mc+c, xymc+zs, xzmc-ys, + xymc-zs, u.y*u.y*mc+c, yzmc+xs, + xzmc+ys, yzmc-xs, u.z*u.z*mc+c ); + return *this; +} + + +// ToAxisAndAngle - find a unit vector in the direction of the rotation axis, +// and the angle theta of rotation. Returns true if the rotation angle is non-zero, +// and false if it is zero. +bool RotationMapR3::ToAxisAndAngle( VectorR3* u, double *theta ) const +{ + double alpha = m11+m22+m33-1.0; + double beta = sqrt(Square(m32-m23)+Square(m13-m31)+Square(m21-m12)); + if ( beta==0.0 ) { + *u = VectorR3::UnitY; + *theta = 0.0; + return false; + } + else { + u->Set(m32-m23, m13-m31, m21-m12); + *u /= beta; + *theta = atan2( beta, alpha ); + return true; + } +} + +// VrRotate is similar to glRotate. Returns a matrix (LinearMapR3) +// that will perform the rotation when multiplied on the left. +// u is supposed to be a *unit* vector. Otherwise, the LinearMapR3 +// returned will be garbage! +RotationMapR3 VrRotate( double theta, const VectorR3& u ) +{ + RotationMapR3 ret; + ret.Set( u, theta ); + return ret; +} + +// This version of rotate takes the cosine and sine of theta +// instead of theta. u must still be a unit vector. + +RotationMapR3 VrRotate( double c, double s, const VectorR3& u ) +{ + RotationMapR3 ret; + ret.Set( u, s, c ); + return ret; +} + +// Returns a RotationMapR3 which rotates the fromVec to be colinear +// with toVec. + +RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec) +{ + VectorR3 crossVec = fromVec; + crossVec *= toVec; + double sinetheta = crossVec.Norm(); // Not yet normalized! + if ( sinetheta < 1.0e-40 ) { + return ( RotationMapR3( + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0) ); + } + crossVec /= sinetheta; // Normalize the vector + double scale = 1.0/sqrt(fromVec.NormSq()*toVec.NormSq()); + sinetheta *= scale; + double cosinetheta = (fromVec^toVec)*scale; + return ( VrRotate( cosinetheta, sinetheta, crossVec) ); +} + +// RotateToMap returns a rotation map which rotates fromVec to have the +// same direction as toVec. +// fromVec and toVec should be unit vectors +RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec) +{ + VectorR3 crossVec = fromVec; + crossVec *= toVec; + double sintheta = crossVec.Norm(); + double costheta = fromVec^toVec; + if ( sintheta <= 1.0e-40 ) { + if ( costheta>0.0 ) { + return ( RotationMapR3( + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0) ); + } + else { + GetOrtho ( toVec, crossVec ); // Get arbitrary orthonormal vector + return( VrRotate(costheta, sintheta, crossVec ) ); + } + } + else { + crossVec /= sintheta; // Normalize the vector + return ( VrRotate( costheta, sintheta, crossVec) ); + } +} + +// ************************************************************** +// * RigidMapR3 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +// The rotation axis vector u MUST be a UNIT vector!!! +RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double theta ) +{ + assert ( fabs(u.NormSq()-1.0)<2.0e-6 ); + register double c = cos(theta); + register double s = sin(theta); + register double mc = 1.0-c; + double xmc = u.x*mc; + double xymc = xmc*u.y; + double xzmc = xmc*u.z; + double yzmc = u.y*u.z*mc; + double xs = u.x*s; + double ys = u.y*s; + double zs = u.z*s; + Matrix3x4::Set3x3( u.x*u.x*mc+c, xymc+zs, xzmc-ys, + xymc-zs, u.y*u.y*mc+c, yzmc+xs, + xzmc+ys, yzmc-xs, u.z*u.z*mc+c ); + return *this; +} + +// The rotation axis vector u MUST be a UNIT vector!!! +RigidMapR3& RigidMapR3::SetRotationPart( const VectorR3& u, double s, double c ) +{ + assert ( fabs(u.NormSq()-1.0)<2.0e-6 ); + double mc = 1.0-c; + double xmc = u.x*mc; + double xymc = xmc*u.y; + double xzmc = xmc*u.z; + double yzmc = u.y*u.z*mc; + double xs = u.x*s; + double ys = u.y*s; + double zs = u.z*s; + Matrix3x4::Set3x3( u.x*u.x*mc+c, xymc+zs, xzmc-ys, + xymc-zs, u.y*u.y*mc+c, yzmc+xs, + xzmc+ys, yzmc-xs, u.z*u.z*mc+c ); + return *this; +} + + +// CalcGlideRotation - converts a rigid map into an equivalent +// glide rotation (screw motion). It returns the rotation axis +// as base point u, and a rotation axis v. The vectors u and v are +// always orthonormal. v will be a unit vector. +// It also returns the glide distance, which is the translation parallel +// to v. Further, it returns the signed rotation angle theta (right hand rule +// specifies the direction. +// The glide rotation means a rotation around the point u with axis direction v. +// Return code "true" if the rotation amount is non-zero. "false" if pure translation. +bool RigidMapR3::CalcGlideRotation( VectorR3* u, VectorR3* v, + double *glideDist, double *rotation ) const +{ + // Compare to the code for ToAxisAndAngle. + double alpha = m11+m22+m33-1.0; + double beta = sqrt(Square(m32-m23)+Square(m13-m31)+Square(m21-m12)); + if ( beta==0.0 ) { + double vN = m14*m14 + m24*m24 + m34*m34; + if ( vN>0.0 ) { + vN = sqrt(vN); + v->Set( m14, m24, m34 ); + *v /= vN; + *glideDist = vN; + } + else { + *v = VectorR3::UnitX; + *glideDist = 0.0; + } + u->SetZero(); + *rotation = 0; + return false; + } + else { + v->Set(m32-m23, m13-m31, m21-m12); + *v /= beta; // v - unit vector, rotation axis + *rotation = atan2( beta, alpha ); + u->Set( m14, m24, m34 ); + *glideDist = ((*u)^(*v)); + VectorR3 temp = *v; + temp *= *glideDist; + *u -= temp; // Subtract component in direction of rot. axis v + temp = *v; + temp *= *u; + temp /= tan((*rotation)/2); // temp = (v \times u) / tan(rotation/2) + *u += temp; + *u *= 0.5; + return true; + } + +} + +// *************************************************************** +// Linear Algebra Utilities * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns a righthanded orthonormal basis to complement vector u +void GetOrtho( const VectorR3& u, VectorR3& v, VectorR3& w) +{ + if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) { + v.Set ( u.y, -u.x, 0.0 ); + } + else { + v.Set ( 0.0, u.z, -u.y); + } + v.Normalize(); + w = u; + w *= v; + w.Normalize(); + // w.NormalizeFast(); + return; +} + +// Returns a vector v orthonormal to unit vector u +void GetOrtho( const VectorR3& u, VectorR3& v ) +{ + if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) { + v.Set ( u.y, -u.x, 0.0 ); + } + else { + v.Set ( 0.0, u.z, -u.y); + } + v.Normalize(); + return; +} + +// *************************************************************** +// Stream Output Routines * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +ostream& operator<< ( ostream& os, const VectorR3& u ) +{ + return (os << "<" << u.x << "," << u.y << "," << u.z << ">"); +} + +ostream& operator<< ( ostream& os, const Matrix3x3& A ) +{ + os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13 << ">\n" + << " <" << A.m21 << ", " << A.m22 << ", " << A.m23 << ">\n" + << " <" << A.m31 << ", " << A.m32 << ", " << A.m33 << ">\n" ; + return (os); +} + +ostream& operator<< ( ostream& os, const Matrix3x4& A ) +{ + os << " <" << A.m11 << ", " << A.m12 << ", " << A.m13 + << "; " << A.m14 << ">\n" + << " <" << A.m21 << ", " << A.m22 << ", " << A.m23 + << "; " << A.m24 << ">\n" + << " <" << A.m31 << ", " << A.m32 << ", " << A.m33 + << "; " << A.m34 << ">\n" ; + return (os); +} + diff --git a/examples/ThirdPartyLibs/BussIK/LinearR3.h b/examples/ThirdPartyLibs/BussIK/LinearR3.h new file mode 100644 index 000000000..4b79d4eed --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/LinearR3.h @@ -0,0 +1,2027 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +// +// Linear Algebra Classes over R3 +// +// +// A. Vector and Position classes +// +// A.1. VectorR3: a real column vector of length 3. +// +// A.2. VectorHgR3: a column vector of length 4 which is +// the homogenous representation of a vector in 3-space +// +// B. Matrix Classes +// +// B.1 LinearMapR3 - arbitrary linear map; 3x3 real matrix +// +// B.2 AffineMapR3 - arbitrary affine map; a 3x4 real matrix +// +// B.3 RotationMapR3 - orthonormal 3x3 matrix +// +// B.4 RigidMapR3 - RotationMapR3 plus displacement +// + +#ifndef LINEAR_R3_H +#define LINEAR_R3_H + +#include +#include +#include +#include "MathMisc.h" +using namespace std; + +class VectorR3; // Space Vector (length 3) +class VectorHgR3; // Homogenous Space Vector +class VectorR4; // Space Vector (length 4) + +class LinearMapR3; // Linear Map (3x3 Matrix) +class AffineMapR3; // Affine Map (3x4 Matrix) +class RotationMapR3; // Rotation (3x3 orthonormal matrix) +class RigidMapR3; // 3x4 matrix, first 3 columns orthonormal + +// Most for internal use: +class Matrix3x3; +class Matrix3x4; + +class Quaternion; + +// ************************************** +// VectorR3 class * +// * * * * * * * * * * * * * * * * * * ** + +class VectorR3 { + +public: + double x, y, z; // The x & y & z coordinates. + + static const VectorR3 Zero; + static const VectorR3 UnitX; + static const VectorR3 UnitY; + static const VectorR3 UnitZ; + static const VectorR3 NegUnitX; + static const VectorR3 NegUnitY; + static const VectorR3 NegUnitZ; + +public: + VectorR3( ) : x(0.0), y(0.0), z(0.0) {} + VectorR3( double xVal, double yVal, double zVal ) + : x(xVal), y(yVal), z(zVal) {} + VectorR3( const VectorHgR3& uH ); + + VectorR3& Set( const Quaternion& ); // Convert quat to rotation vector + VectorR3& Set( double xx, double yy, double zz ) + { x=xx; y=yy; z=zz; return *this; } + VectorR3& SetFromHg( const VectorR4& ); // Convert homogeneous VectorR4 to VectorR3 + VectorR3& SetZero() { x=0.0; y=0.0; z=0.0; return *this;} + VectorR3& Load( const double* v ); + VectorR3& Load( const float* v ); + void Dump( double* v ) const; + void Dump( float* v ) const; + + inline double operator[]( int i ); + + VectorR3& operator= ( const VectorR3& v ) + { x=v.x; y=v.y; z=v.z; return(*this);} + VectorR3& operator+= ( const VectorR3& v ) + { x+=v.x; y+=v.y; z+=v.z; return(*this); } + VectorR3& operator-= ( const VectorR3& v ) + { x-=v.x; y-=v.y; z-=v.z; return(*this); } + VectorR3& operator*= ( double m ) + { x*=m; y*=m; z*=m; return(*this); } + VectorR3& operator/= ( double m ) + { register double mInv = 1.0/m; + x*=mInv; y*=mInv; z*=mInv; + return(*this); } + VectorR3 operator- () const { return ( VectorR3(-x, -y, -z) ); } + VectorR3& operator*= (const VectorR3& v); // Cross Product + VectorR3& ArrayProd(const VectorR3&); // Component-wise product + + VectorR3& AddScaled( const VectorR3& u, double s ); + + bool IsZero() const { return ( x==0.0 && y==0.0 && z==0.0 ); } + double Norm() const { return ( (double)sqrt( x*x + y*y + z*z ) ); } + double NormSq() const { return ( x*x + y*y + z*z ); } + double MaxAbs() const; + double Dist( const VectorR3& u ) const; // Distance from u + double DistSq( const VectorR3& u ) const; // Distance from u squared + VectorR3& Negate() { x = -x; y = -y; z = -z; return *this;} + VectorR3& Normalize () { *this /= Norm(); return *this;} // No error checking + inline VectorR3& MakeUnit(); // Normalize() with error checking + inline VectorR3& ReNormalize(); + bool IsUnit( ) const + { register double norm = Norm(); + return ( 1.000001>=norm && norm>=0.999999 ); } + bool IsUnit( double tolerance ) const + { register double norm = Norm(); + return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); } + bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );} + // tolerance should be non-negative + + double YaxisDistSq() const { return (x*x+z*z); } + double YaxisDist() const { return sqrt(x*x+z*z); } + + VectorR3& Rotate( double theta, const VectorR3& u); // rotate around u. + VectorR3& RotateUnitInDirection ( const VectorR3& dir); // rotate in direction dir + VectorR3& Rotate( const Quaternion& ); // Rotate according to quaternion + + friend ostream& operator<< ( ostream& os, const VectorR3& u ); + +}; + +inline VectorR3 operator+( const VectorR3& u, const VectorR3& v ); +inline VectorR3 operator-( const VectorR3& u, const VectorR3& v ); +inline VectorR3 operator*( const VectorR3& u, double m); +inline VectorR3 operator*( double m, const VectorR3& u); +inline VectorR3 operator/( const VectorR3& u, double m); +inline int operator==( const VectorR3& u, const VectorR3& v ); + +inline double operator^ (const VectorR3& u, const VectorR3& v ); // Dot Product +inline VectorR3 operator* (const VectorR3& u, const VectorR3& v); // Cross Product +inline VectorR3 ArrayProd ( const VectorR3& u, const VectorR3& v ); + +inline double Mag(const VectorR3& u) { return u.Norm(); } +inline double Dist(const VectorR3& u, const VectorR3& v) { return u.Dist(v); } +inline double DistSq(const VectorR3& u, const VectorR3& v) { return u.DistSq(v); } +inline double NormalizeError (const VectorR3& u); + +extern const VectorR3 UnitVecIR3; +extern const VectorR3 UnitVecJR3; +extern const VectorR3 UnitVecKR3; + +inline VectorR3 ToVectorR3( const Quaternion& q ) + {return VectorR3().Set(q);} + + +// **************************************** +// VectorHgR3 class * +// * * * * * * * * * * * * * * * * * * * ** + +class VectorHgR3 { + +public: + double x, y, z, w; // The x & y & z & w coordinates. + +public: + VectorHgR3( ) : x(0.0), y(0.0), z(0.0), w(1.0) {} + VectorHgR3( double xVal, double yVal, double zVal ) + : x(xVal), y(yVal), z(zVal), w(1.0) {} + VectorHgR3( double xVal, double yVal, double zVal, double wVal ) + : x(xVal), y(yVal), z(zVal), w(wVal) {} + VectorHgR3 ( const VectorR3& u ) : x(u.x), y(u.y), z(u.z), w(1.0) {} +}; + +// +// Advanced vector and position functions (prototypes) +// + +VectorR3 Interpolate( const VectorR3& start, const VectorR3& end, double a); + +// ***************************************** +// Matrix3x3 class * +// * * * * * * * * * * * * * * * * * * * * * + +class Matrix3x3 { +public: + + double m11, m12, m13, m21, m22, m23, m31, m32, m33; + + // Implements a 3x3 matrix: m_i_j - row-i and column-j entry + + static const Matrix3x3 Identity; + +public: + inline Matrix3x3(); + inline Matrix3x3(const VectorR3&, const VectorR3&, const VectorR3&); // Sets by columns! + inline Matrix3x3(double, double, double, double, double, double, + double, double, double ); // Sets by columns + + inline void SetIdentity (); // Set to the identity map + inline void Set ( const Matrix3x3& ); // Set to the matrix. + inline void Set3x3 ( const Matrix3x4& ); // Set to the 3x3 part of the matrix. + inline void Set( const VectorR3&, const VectorR3&, const VectorR3& ); + inline void Set( double, double, double, + double, double, double, + double, double, double ); + inline void SetByRows( double, double, double, double, double, double, + double, double, double ); + inline void SetByRows( const VectorR3&, const VectorR3&, const VectorR3& ); + + inline void SetColumn1 ( double, double, double ); + inline void SetColumn2 ( double, double, double ); + inline void SetColumn3 ( double, double, double ); + inline void SetColumn1 ( const VectorR3& ); + inline void SetColumn2 ( const VectorR3& ); + inline void SetColumn3 ( const VectorR3& ); + inline VectorR3 Column1() const; + inline VectorR3 Column2() const; + inline VectorR3 Column3() const; + + inline void SetRow1 ( double, double, double ); + inline void SetRow2 ( double, double, double ); + inline void SetRow3 ( double, double, double ); + inline void SetRow1 ( const VectorR3& ); + inline void SetRow2 ( const VectorR3& ); + inline void SetRow3 ( const VectorR3& ); + inline VectorR3 Row1() const; + inline VectorR3 Row2() const; + inline VectorR3 Row3() const; + + inline void SetDiagonal( double, double, double ); + inline void SetDiagonal( const VectorR3& ); + inline double Diagonal( int ); + + inline void MakeTranspose(); // Transposes it. + Matrix3x3& ReNormalize(); + VectorR3 Solve(const VectorR3&) const; // Returns solution + + inline void Transform( VectorR3* ) const; + inline void Transform( const VectorR3& src, VectorR3* dest) const; + +protected: + void OperatorTimesEquals( const Matrix3x3& ); // Internal use only + void SetZero (); // Set to the zero map + +}; + +inline VectorR3 operator* ( const Matrix3x3&, const VectorR3& ); + +ostream& operator<< ( ostream& os, const Matrix3x3& A ); + + +// ***************************************** +// Matrix3x4 class * +// * * * * * * * * * * * * * * * * * * * * * + +class Matrix3x4 +{ +public: + double m11, m12, m13, m21, m22, m23, m31, m32, m33; + double m14; + double m24; + double m34; + + static const Matrix3x4 Identity; + +public: + // Constructors set by columns! + Matrix3x4() {} + Matrix3x4(const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& ); + Matrix3x4(double, double, double, double, double, double, + double, double, double, double, double, double ); // Sets by columns + Matrix3x4( const Matrix3x3&, const VectorR3& ); + + void SetIdentity (); // Set to the identity map + void Set ( const Matrix3x4& ); // Set to the matrix. + void Set3x3 ( const Matrix3x3& ); // Set linear part to the matrix. + void Set ( const Matrix3x3&, const VectorR3& ); // Set to the matrix plus 4th column + void Set( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& ); + void Set( double, double, double, + double, double, double, + double, double, double, + double, double, double ); // Sets by columns + void Set3x3( double, double, double, + double, double, double, + double, double, double ); // Sets by columns + void SetByRows( double, double, double, double, double, double, + double, double, double, double, double, double ); + + void SetColumn1 ( double, double, double ); + void SetColumn2 ( double, double, double ); + void SetColumn3 ( double, double, double ); + void SetColumn4 ( double, double, double ); + void SetColumn1 ( const VectorR3& ); + void SetColumn2 ( const VectorR3& ); + void SetColumn3 ( const VectorR3& ); + void SetColumn4 ( const VectorR3& ); + VectorR3 Column1() const; + VectorR3 Column2() const; + VectorR3 Column3() const; + VectorR3 Column4() const; + void SetRow1 ( double x, double y, double z, double w ); + void SetRow2 ( double x, double y, double z, double w ); + void SetRow3 ( double x, double y, double z, double w ); + void SetRow4 ( double x, double y, double z, double w ); + + Matrix3x4& ApplyTranslationLeft( const VectorR3& u ); + Matrix3x4& ApplyTranslationRight( const VectorR3& u ); + Matrix3x4& ApplyYRotationLeft( double theta ); + Matrix3x4& ApplyYRotationLeft( double costheta, double sintheta ); + + Matrix3x4& ReNormalize(); + VectorR3 Solve(const VectorR3&) const; // Returns solution + + inline void Transform( VectorR3* ) const; + inline void Transform3x3( VectorR3* ) const; + inline void Transform( const VectorR3& src, VectorR3* dest ) const; + inline void Transform3x3( const VectorR3& src, VectorR3* dest ) const; + inline void Transform3x3Transpose( VectorR3* dest ) const; + inline void Transform3x3Transpose( const VectorR3& src, VectorR3* dest ) const; + +protected: + void SetZero (); // Set to the zero map + void OperatorTimesEquals( const Matrix3x3& ); // Internal use only + void OperatorTimesEquals( const Matrix3x4& ); // Internal use only +}; + +inline VectorR3 operator* ( const Matrix3x4&, const VectorR3& ); + +ostream& operator<< ( ostream& os, const Matrix3x4& A ); + +// ***************************************** +// LinearMapR3 class * +// * * * * * * * * * * * * * * * * * * * * * + +class LinearMapR3 : public Matrix3x3 { + +public: + + LinearMapR3(); + LinearMapR3( const VectorR3&, const VectorR3&, const VectorR3& ); + LinearMapR3( double, double, double, double, double, double, + double, double, double ); // Sets by columns + LinearMapR3 ( const Matrix3x3& ); + + void SetZero (); // Set to the zero map + inline void Negate(); + + inline LinearMapR3& operator+= (const Matrix3x3& ); + inline LinearMapR3& operator-= (const Matrix3x3& ); + inline LinearMapR3& operator*= (double); + inline LinearMapR3& operator/= (double); + LinearMapR3& operator*= (const Matrix3x3& ); // Matrix product + + inline LinearMapR3 Transpose() const; // Returns the transpose + double Determinant () const; // Returns the determinant + LinearMapR3 Inverse() const; // Returns inverse + LinearMapR3& Invert(); // Converts into inverse. + VectorR3 Solve(const VectorR3&) const; // Returns solution + LinearMapR3 PseudoInverse() const; // Returns pseudo-inverse TO DO + VectorR3 PseudoSolve(const VectorR3&); // Finds least squares solution TO DO + +}; + +inline LinearMapR3 operator+ (const LinearMapR3&, const Matrix3x3&); +inline LinearMapR3 operator+ (const Matrix3x3&, const LinearMapR3&); +inline LinearMapR3 operator- (const LinearMapR3&); +inline LinearMapR3 operator- (const LinearMapR3&, const Matrix3x3&); +inline LinearMapR3 operator- (const Matrix3x3&, const LinearMapR3&); +inline LinearMapR3 operator* ( const LinearMapR3&, double); +inline LinearMapR3 operator* ( double, const LinearMapR3& ); +inline LinearMapR3 operator/ ( const LinearMapR3&, double ); +LinearMapR3 operator* ( const LinearMapR3&, const LinearMapR3& ); + // Matrix product (composition) + + +// ***************************************************** +// * AffineMapR3 class * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * + +class AffineMapR3 : public Matrix3x4 { + +public: + AffineMapR3(); + AffineMapR3( double, double, double, double, double, double, + double, double, double, double, double, double ); // Sets by columns + AffineMapR3 ( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3&); + AffineMapR3 ( const LinearMapR3&, const VectorR3& ); + + void SetIdentity (); // Set to the identity map + void SetZero (); // Set to the zero map + + AffineMapR3& operator+= (const Matrix3x4& ); + AffineMapR3& operator-= (const Matrix3x4& ); + AffineMapR3& operator*= (double); + AffineMapR3& operator/= (double); + AffineMapR3& operator*= (const Matrix3x3& ); // Composition + AffineMapR3& operator*= (const Matrix3x4& ); // Composition + + AffineMapR3& ApplyTranslationLeft( const VectorR3& u ) + { Matrix3x4::ApplyTranslationLeft( u ); return *this; } + AffineMapR3& ApplyTranslationRight( const VectorR3& u ) + { Matrix3x4::ApplyTranslationRight( u ); return *this; } + AffineMapR3& ApplyYRotationLeft( double theta ) + { Matrix3x4::ApplyYRotationLeft( theta ); return *this; } + AffineMapR3& ApplyYRotationLeft( double costheta, double sintheta ) + { Matrix3x4::ApplyYRotationLeft( costheta, sintheta ); return *this; } + + AffineMapR3 Inverse() const; // Returns inverse + AffineMapR3& Invert(); // Converts into inverse. + VectorR3 Solve(const VectorR3&) const; // Returns solution + AffineMapR3 PseudoInverse() const; // Returns pseudo-inverse // TO DO + VectorR3 PseudoSolve(const VectorR3&); // Least squares solution // TO DO +}; + +inline AffineMapR3 operator+ (const AffineMapR3&, const Matrix3x4&); +inline AffineMapR3 operator+ (const Matrix3x4&, const AffineMapR3&); +inline AffineMapR3 operator+ (const AffineMapR3&, const Matrix3x3&); +inline AffineMapR3 operator+ (const Matrix3x3&, const AffineMapR3&); +inline AffineMapR3 operator- (const AffineMapR3&, const Matrix3x4&); +inline AffineMapR3 operator- (const Matrix3x4&, const AffineMapR3&); +inline AffineMapR3 operator- (const AffineMapR3&, const Matrix3x3&); +inline AffineMapR3 operator- (const Matrix3x3&, const AffineMapR3&); +inline AffineMapR3 operator* (const AffineMapR3&, double); +inline AffineMapR3 operator* (double, const AffineMapR3& ); +inline AffineMapR3 operator/ (const AffineMapR3&, double ); + +// Composition operators +AffineMapR3 operator* ( const AffineMapR3&, const AffineMapR3& ); +AffineMapR3 operator* ( const LinearMapR3&, const AffineMapR3& ); +AffineMapR3 operator* ( const AffineMapR3&, const LinearMapR3& ); + + +// ******************************************* +// RotationMapR3 class * +// * * * * * * * * * * * * * * * * * * * * * * + +class RotationMapR3 : public Matrix3x3 { + +public: + + RotationMapR3(); + RotationMapR3( const VectorR3&, const VectorR3&, const VectorR3& ); + RotationMapR3( double, double, double, double, double, double, + double, double, double ); + + RotationMapR3& Set( const Quaternion& ); + RotationMapR3& Set( const VectorR3&, double theta ); // Set rotation axis and angle + RotationMapR3& Set( const VectorR3&, double sintheta, double costheta ); + + RotationMapR3& operator*= (const RotationMapR3& ); // Matrix product + + RotationMapR3 Transpose() const { return Inverse(); }; // Returns the transpose + RotationMapR3 Inverse() const; // Returns inverse + RotationMapR3& Invert(); // Converts into inverse. + VectorR3 Solve(const VectorR3&) const; // Returns solution // Was named Invert + + bool ToAxisAndAngle( VectorR3* u, double* theta ) const; // returns unit vector u and angle + +}; + +RotationMapR3 operator* ( const RotationMapR3&, const RotationMapR3& ); + // Matrix product (composition) + +inline RotationMapR3 ToRotationMapR3( const Quaternion& q ) + { return( RotationMapR3().Set(q) ); } + +ostream& operator<< ( ostream& os, const RotationMapR3& A ); + + + +// *************************************************************** +// * RigidMapR3 class - prototypes. * * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +class RigidMapR3 : public Matrix3x4 +{ + +public: + RigidMapR3(); + RigidMapR3( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& ); + RigidMapR3( double, double, double, double, double, double, + double, double, double, double, double, double ); + RigidMapR3( const Matrix3x3&, const VectorR3& ); + + RigidMapR3& Set( const Matrix3x3&, const VectorR3& ); // Set to RotationMap & Vector + RigidMapR3& SetTranslationPart( const VectorR3& ); // Set the translation part + RigidMapR3& SetTranslationPart( double, double, double ); // Set the translation part + RigidMapR3& SetRotationPart( const Matrix3x3& ); // Set the rotation part + RigidMapR3& SetRotationPart( const Quaternion& ); + RigidMapR3& SetRotationPart( const VectorR3&, double theta ); // Set rotation axis and angle + RigidMapR3& SetRotationPart( const VectorR3&, double sintheta, double costheta ); + + RigidMapR3& ApplyTranslationLeft( const VectorR3& u ) + {Matrix3x4::ApplyTranslationLeft( u ); return *this;} + RigidMapR3& ApplyTranslationRight( const VectorR3& u ) + {Matrix3x4::ApplyTranslationRight( u ); return *this;} + RigidMapR3& ApplyYRotationLeft( double theta ) + { Matrix3x4::ApplyYRotationLeft( theta ); return *this; } + RigidMapR3& ApplyYRotationLeft( double costheta, double sintheta ) + { Matrix3x4::ApplyYRotationLeft( costheta, sintheta ); return *this; } + + RigidMapR3& operator*=(const RotationMapR3& ); // Composition + RigidMapR3& operator*=(const RigidMapR3& ); // Composition + + RigidMapR3 Inverse() const; // Returns inverse + RigidMapR3& Invert(); // Converts into inverse. + + bool CalcGlideRotation( VectorR3* u, VectorR3* v, + double *glideDist, double *rotation ) const; + + void Transform3x3Inverse( VectorR3* dest ) const + { Matrix3x4::Transform3x3Transpose( dest ); } + void Transform3x3Inverse( const VectorR3& src, VectorR3* dest ) const + { Matrix3x4::Transform3x3Transpose( src, dest ); } + +}; + + +// *************************************************************** +// * 3-space vector and matrix utilities (prototypes) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns the solid angle between vectors v and w. +inline double SolidAngle( const VectorR3& v, const VectorR3& w); + +// Returns a righthanded orthonormal basis to complement unit vector x +void GetOrtho( const VectorR3& x, VectorR3& y, VectorR3& z); +// Returns a vector v orthonormal to unit vector x +void GetOrtho( const VectorR3& x, VectorR3& y ); + +// Projections + +// The next three functions are templated below. +//inline VectorR3 ProjectToUnit ( const VectorR3& u, const VectorR3& v); // Project u onto v +//inline VectorR3 ProjectPerpUnit ( const VectorR3& u, const VectorR3 & v); // Project perp to v +//inline VectorR3 ProjectPerpUnitDiff ( const VectorR3& u, const VectorR3& v) +// v must be a unit vector. + +// Projection maps (LinearMapR3s) + +inline LinearMapR3 VectorProjectMap( const VectorR3& u ); +inline LinearMapR3 PlaneProjectMap ( const VectorR3& w ); +inline LinearMapR3 PlaneProjectMap ( const VectorR3& u, const VectorR3 &v ); +// u,v,w - must be unit vector. u and v must be orthonormal and +// specify the plane they are parallel to. w specifies the plane +// it is orthogonal to. + +// VrRotate is similar to glRotate. Returns a matrix (RotationMapR3) +// that will perform the rotation. u should be a unit vector. +RotationMapR3 VrRotate( double theta, const VectorR3& u ); +RotationMapR3 VrRotate( double costheta, double sintheta, const VectorR3& u ); +RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec); +RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec); +// fromVec and toVec should be unit vectors for RotateToMap + +// *************************************************************** +// * Stream Output Routines (Prototypes) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +ostream& operator<< ( ostream& os, const VectorR3& u ); + + +// ***************************************************** +// * VectorR3 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * + +inline VectorR3& VectorR3::Load( const double* v ) +{ + x = *v; + y = *(v+1); + z = *(v+2); + return *this; +} + +inline VectorR3& VectorR3::Load( const float* v ) +{ + x = *v; + y = *(v+1); + z = *(v+2); + return *this; +} + +inline void VectorR3::Dump( double* v ) const +{ + *v = x; + *(v+1) = y; + *(v+2) = z; +} + +inline void VectorR3::Dump( float* v ) const +{ + *v = (float)x; + *(v+1) = (float)y; + *(v+2) = (float)z; +} + +inline double VectorR3::operator[]( int i ) +{ + switch (i) { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + assert(0); + return 0.0; + } +} + +inline VectorR3& VectorR3::MakeUnit () // Convert to unit vector (or leave zero). +{ + double nSq = NormSq(); + if (nSq != 0.0) { + *this /= sqrt(nSq); + } + return *this; +} + +inline VectorR3 operator+( const VectorR3& u, const VectorR3& v ) +{ + return VectorR3(u.x+v.x, u.y+v.y, u.z+v.z); +} +inline VectorR3 operator-( const VectorR3& u, const VectorR3& v ) +{ + return VectorR3(u.x-v.x, u.y-v.y, u.z-v.z); +} +inline VectorR3 operator*( const VectorR3& u, register double m) +{ + return VectorR3( u.x*m, u.y*m, u.z*m); +} +inline VectorR3 operator*( register double m, const VectorR3& u) +{ + return VectorR3( u.x*m, u.y*m, u.z*m); +} +inline VectorR3 operator/( const VectorR3& u, double m) +{ + register double mInv = 1.0/m; + return VectorR3( u.x*mInv, u.y*mInv, u.z*mInv); +} + +inline int operator==( const VectorR3& u, const VectorR3& v ) +{ + return ( u.x==v.x && u.y==v.y && u.z==v.z ); +} + +inline double operator^ ( const VectorR3& u, const VectorR3& v ) // Dot Product +{ + return ( u.x*v.x + u.y*v.y + u.z*v.z ); +} + +inline VectorR3 operator* (const VectorR3& u, const VectorR3& v) // Cross Product +{ + return (VectorR3( u.y*v.z - u.z*v.y, + u.z*v.x - u.x*v.z, + u.x*v.y - u.y*v.x ) ); +} + +inline VectorR3 ArrayProd ( const VectorR3& u, const VectorR3& v ) +{ + return ( VectorR3( u.x*v.x, u.y*v.y, u.z*v.z ) ); +} + +inline VectorR3& VectorR3::operator*= (const VectorR3& v) // Cross Product +{ + double tx=x, ty=y; + x = y*v.z - z*v.y; + y = z*v.x - tx*v.z; + z = tx*v.y - ty*v.x; + return ( *this ); +} + +inline VectorR3& VectorR3::ArrayProd (const VectorR3& v) // Component-wise Product +{ + x *= v.x; + y *= v.y; + z *= v.z; + return ( *this ); +} + +inline VectorR3& VectorR3::AddScaled( const VectorR3& u, double s ) +{ + x += s*u.x; + y += s*u.y; + z += s*u.z; + return(*this); +} + +inline VectorR3::VectorR3( const VectorHgR3& uH ) +: x(uH.x), y(uH.y), z(uH.z) +{ + *this /= uH.w; +} + +inline VectorR3& VectorR3::ReNormalize() // Convert near unit back to unit +{ + double nSq = NormSq(); + register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor + *this *= mFact; + return *this; +} + +inline double NormalizeError (const VectorR3& u) +{ + register double discrepancy; + discrepancy = u.x*u.x + u.y*u.y + u.z*u.z - 1.0; + if ( discrepancy < 0.0 ) { + discrepancy = -discrepancy; + } + return discrepancy; +} + +inline double VectorR3::Dist( const VectorR3& u ) const // Distance from u +{ + return sqrt( DistSq(u) ); +} + +inline double VectorR3::DistSq( const VectorR3& u ) const // Distance from u +{ + return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) + (z-u.z)*(z-u.z) ); +} + +// +// Interpolation routines (not just Spherical Interpolation) +// + +// Interpolate(start,end,frac) - linear interpolation +// - allows overshooting the end points +inline VectorR3 Interpolate( const VectorR3& start, const VectorR3& end, double a) +{ + VectorR3 ret; + Lerp( start, end, a, ret ); + return ret; +} + + +// ****************************************************** +// * Matrix3x3 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline Matrix3x3::Matrix3x3() {} + +inline Matrix3x3::Matrix3x3( const VectorR3& u, const VectorR3& v, + const VectorR3& s ) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m13 = s.x; // Column 3 + m23 = s.y; + m33 = s.z; +} + +inline Matrix3x3::Matrix3x3( double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; +} + +inline void Matrix3x3::SetIdentity ( ) +{ + m11 = m22 = m33 = 1.0; + m12 = m13 = m21 = m23 = m31 = m32 = 0.0; +} + +inline void Matrix3x3::SetZero( ) +{ + m11 = m12 = m13 = m21 = m22 = m23 = m31 = m32 = m33 = 0.0; +} + +inline void Matrix3x3::Set ( const Matrix3x3& A ) // Set to the matrix. +{ + m11 = A.m11; + m21 = A.m21; + m31 = A.m31; + m12 = A.m12; + m22 = A.m22; + m32 = A.m32; + m13 = A.m13; + m23 = A.m23; + m33 = A.m33; +} + +inline void Matrix3x3::Set3x3 ( const Matrix3x4& A ) // Set to the 3x3 part of the matrix. +{ + m11 = A.m11; + m21 = A.m21; + m31 = A.m31; + m12 = A.m12; + m22 = A.m22; + m32 = A.m32; + m13 = A.m13; + m23 = A.m23; + m33 = A.m33; +} + +inline void Matrix3x3::Set( const VectorR3& u, const VectorR3& v, + const VectorR3& w) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m13 = w.x; // Column 3 + m23 = w.y; + m33 = w.z; +} + +inline void Matrix3x3::Set( double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; +} + +inline void Matrix3x3::SetByRows( double a11, double a12, double a13, + double a21, double a22, double a23, + double a31, double a32, double a33) + // Values specified in row order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; +} + +inline void Matrix3x3::SetByRows( const VectorR3& u, const VectorR3& v, + const VectorR3& s ) +{ + m11 = u.x; // Row 1 + m12 = u.y; + m13 = u.z; + m21 = v.x; // Row 2 + m22 = v.y; + m23 = v.z; + m31 = s.x; // Row 3 + m32 = s.y; + m33 = s.z; +} + +inline void Matrix3x3::SetColumn1 ( double x, double y, double z) +{ + m11 = x; m21 = y; m31= z; +} + +inline void Matrix3x3::SetColumn2 ( double x, double y, double z) +{ + m12 = x; m22 = y; m32= z; +} + +inline void Matrix3x3::SetColumn3 ( double x, double y, double z) +{ + m13 = x; m23 = y; m33= z; +} + +inline void Matrix3x3::SetColumn1 ( const VectorR3& u ) +{ + m11 = u.x; m21 = u.y; m31 = u.z; +} + +inline void Matrix3x3::SetColumn2 ( const VectorR3& u ) +{ + m12 = u.x; m22 = u.y; m32 = u.z; +} + +inline void Matrix3x3::SetColumn3 ( const VectorR3& u ) +{ + m13 = u.x; m23 = u.y; m33 = u.z; +} + +inline void Matrix3x3::SetRow1 ( double x, double y, double z ) +{ + m11 = x; + m12 = y; + m13 = z; +} + +inline void Matrix3x3::SetRow2 ( double x, double y, double z ) +{ + m21 = x; + m22 = y; + m23 = z; +} + +inline void Matrix3x3::SetRow3 ( double x, double y, double z ) +{ + m31 = x; + m32 = y; + m33 = z; +} + + + +inline VectorR3 Matrix3x3::Column1() const +{ + return ( VectorR3(m11, m21, m31) ); +} + +inline VectorR3 Matrix3x3::Column2() const +{ + return ( VectorR3(m12, m22, m32) ); +} + +inline VectorR3 Matrix3x3::Column3() const +{ + return ( VectorR3(m13, m23, m33) ); +} + +inline VectorR3 Matrix3x3::Row1() const +{ + return ( VectorR3(m11, m12, m13) ); +} + +inline VectorR3 Matrix3x3::Row2() const +{ + return ( VectorR3(m21, m22, m23) ); +} + +inline VectorR3 Matrix3x3::Row3() const +{ + return ( VectorR3(m31, m32, m33) ); +} + +inline void Matrix3x3::SetDiagonal( double x, double y, double z ) +{ + m11 = x; + m22 = y; + m33 = z; +} + +inline void Matrix3x3::SetDiagonal( const VectorR3& u ) +{ + SetDiagonal ( u.x, u.y, u.z ); +} + +inline double Matrix3x3::Diagonal( int i ) +{ + switch (i) { + case 0: + return m11; + case 1: + return m22; + case 2: + return m33; + default: + assert(0); + return 0.0; + } +} + +inline void Matrix3x3::MakeTranspose() // Transposes it. +{ + register double temp; + temp = m12; + m12 = m21; + m21=temp; + temp = m13; + m13 = m31; + m31 = temp; + temp = m23; + m23 = m32; + m32 = temp; +} + +inline VectorR3 operator* ( const Matrix3x3& A, const VectorR3& u) +{ + return( VectorR3( A.m11*u.x + A.m12*u.y + A.m13*u.z, + A.m21*u.x + A.m22*u.y + A.m23*u.z, + A.m31*u.x + A.m32*u.y + A.m33*u.z ) ); +} + +inline void Matrix3x3::Transform( VectorR3* u ) const { + double newX, newY; + newX = m11*u->x + m12*u->y + m13*u->z; + newY = m21*u->x + m22*u->y + m23*u->z; + u->z = m31*u->x + m32*u->y + m33*u->z; + u->x = newX; + u->y = newY; +} + +inline void Matrix3x3::Transform( const VectorR3& src, VectorR3* dest ) const { + dest->x = m11*src.x + m12*src.y + m13*src.z; + dest->y = m21*src.x + m22*src.y + m23*src.z; + dest->z = m31*src.x + m32*src.y + m33*src.z; +} + + +// ****************************************************** +// * Matrix3x4 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline Matrix3x4::Matrix3x4(const VectorR3& u, const VectorR3& v, + const VectorR3& s, const VectorR3& t) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m13 = s.x; // Column 3 + m23 = s.y; + m33 = s.z; + m14 = t.x; + m24 = t.y; + m34 = t.z; +} + +inline Matrix3x4::Matrix3x4(double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33, + double a14, double a24, double a34) +{ // Values in COLUMN order! + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; +} + +inline Matrix3x4::Matrix3x4( const Matrix3x3& A, const VectorR3& u ) +{ + Set(A, u); +} + +inline void Matrix3x4::SetIdentity () // Set to the identity map +{ + m11 = m22 = m33 = 1.0; + m12 = m13 = m21 = m23 = m31 = m32 = 0.0; + m14 = m24 = m34 = 0.0; +} + +inline void Matrix3x4::SetZero () // Set to the zero map +{ + m11 = m22 = m33 = 0.0; + m12 = m13 = m21 = m23 = m31 = m32 = 0.0; + m14 = m24 = m34 = 0.0; +} + +inline void Matrix3x4::Set ( const Matrix3x4& A ) // Set to the matrix. +{ + m11 = A.m11; + m21 = A.m21; + m31 = A.m31; + m12 = A.m12; + m22 = A.m22; + m32 = A.m32; + m13 = A.m13; + m23 = A.m23; + m33 = A.m33; + m14 = A.m14; + m24 = A.m24; + m34 = A.m34; +} + +inline void Matrix3x4::Set ( const Matrix3x3& A, const VectorR3& t ) // Set to the matrix plus 4th column +{ + m11 = A.m11; + m21 = A.m21; + m31 = A.m31; + m12 = A.m12; + m22 = A.m22; + m32 = A.m32; + m13 = A.m13; + m23 = A.m23; + m33 = A.m33; + m14 = t.x; // Column 4 + m24 = t.y; + m34 = t.z; +} + +// Set linear part to the matrix +inline void Matrix3x4::Set3x3 ( const Matrix3x3& A ) +{ + m11 = A.m11; + m21 = A.m21; + m31 = A.m31; + m12 = A.m12; + m22 = A.m22; + m32 = A.m32; + m13 = A.m13; + m23 = A.m23; + m33 = A.m33; +} + +inline void Matrix3x4::Set( const VectorR3& u, const VectorR3& v, + const VectorR3& w, const VectorR3& t ) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m13 = w.x; // Column 3 + m23 = w.y; + m33 = w.z; + m14 = t.x; // Column 4 + m24 = t.y; + m34 = t.z; +} + +inline void Matrix3x4::Set( double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33, + double a14, double a24, double a34 ) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; +} + +inline void Matrix3x4::Set3x3( double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33 ) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; +} + +inline void Matrix3x4::SetByRows( double a11, double a12, double a13, double a14, + double a21, double a22, double a23, double a24, + double a31, double a32, double a33, double a34 ) + // Values specified in row order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; +} + +inline void Matrix3x4::SetColumn1 ( double x, double y, double z) +{ + m11 = x; m21 = y; m31= z; +} + +inline void Matrix3x4::SetColumn2 ( double x, double y, double z) +{ + m12 = x; m22 = y; m32= z; +} + +inline void Matrix3x4::SetColumn3 ( double x, double y, double z) +{ + m13 = x; m23 = y; m33= z; +} + +inline void Matrix3x4::SetColumn4 ( double x, double y, double z ) +{ + m14 = x; m24 = y; m34= z; +} + +inline void Matrix3x4::SetColumn1 ( const VectorR3& u ) +{ + m11 = u.x; m21 = u.y; m31 = u.z; +} + +inline void Matrix3x4::SetColumn2 ( const VectorR3& u ) +{ + m12 = u.x; m22 = u.y; m32 = u.z; +} + +inline void Matrix3x4::SetColumn3 ( const VectorR3& u ) +{ + m13 = u.x; m23 = u.y; m33 = u.z; +} + +inline void Matrix3x4::SetColumn4 ( const VectorR3& u ) +{ + m14 = u.x; m24 = u.y; m34 = u.z; +} + +inline VectorR3 Matrix3x4::Column1() const +{ + return ( VectorR3(m11, m21, m31) ); +} + +inline VectorR3 Matrix3x4::Column2() const +{ + return ( VectorR3(m12, m22, m32) ); +} + +inline VectorR3 Matrix3x4::Column3() const +{ + return ( VectorR3(m13, m23, m33) ); +} + +inline VectorR3 Matrix3x4::Column4() const +{ + return ( VectorR3(m14, m24, m34) ); +} + +inline void Matrix3x4::SetRow1 ( double x, double y, double z, double w ) +{ + m11 = x; + m12 = y; + m13 = z; + m14 = w; +} + +inline void Matrix3x4::SetRow2 ( double x, double y, double z, double w ) +{ + m21 = x; + m22 = y; + m23 = z; + m24 = w; +} + +inline void Matrix3x4::SetRow3 ( double x, double y, double z, double w ) +{ + m31 = x; + m32 = y; + m33 = z; + m34 = w; +} + +// Left multiply with a translation (so the translation is applied afterwards). +inline Matrix3x4& Matrix3x4::ApplyTranslationLeft( const VectorR3& u ) +{ + m14 += u.x; + m24 += u.y; + m34 += u.z; + return *this; +} + +// Right multiply with a translation (so the translation is applied first). +inline Matrix3x4& Matrix3x4::ApplyTranslationRight( const VectorR3& u ) +{ + double new14 = m14 + m11*u.x + m12*u.y + m13*u.z; + double new24 = m24 + m21*u.x + m22*u.y + m23*u.z; + m34 = m34 + m31*u.x + m32*u.y + m33*u.z; + m14 = new14; + m24 = new24; + return *this; +} + +// Left-multiply with a rotation around the y-axis. +inline Matrix3x4& Matrix3x4::ApplyYRotationLeft( double theta ) +{ + double costheta = cos(theta); + double sintheta = sin(theta); + return ApplyYRotationLeft( costheta, sintheta ); +} + +inline Matrix3x4& Matrix3x4::ApplyYRotationLeft( double costheta, double sintheta ) +{ + double tmp; + tmp = costheta*m11+sintheta*m31; + m31 = costheta*m31-sintheta*m11; + m11 = tmp; + + tmp = costheta*m12+sintheta*m32; + m32 = costheta*m32-sintheta*m12; + m12 = tmp; + + tmp = costheta*m13+sintheta*m33; + m33 = costheta*m33-sintheta*m13; + m13 = tmp; + + tmp = costheta*m14+sintheta*m34; + m34 = costheta*m34-sintheta*m14; + m14 = tmp; + + return *this; +} + +inline VectorR3 Matrix3x4::Solve(const VectorR3& u) const // Returns solution +{ + Matrix3x3 A; + A.Set3x3(*this); + return ( A.Solve( VectorR3(m14-u.x, m24-u.y, m34-u.z) ) ); +} + +inline void Matrix3x4::Transform( VectorR3* u ) const { + double newX, newY; + newX = m11*u->x + m12*u->y + m13*u->z + m14; + newY = m21*u->x + m22*u->y + m23*u->z + m24; + u->z = m31*u->x + m32*u->y + m33*u->z + m34; + u->x = newX; + u->y = newY; +} + +inline void Matrix3x4::Transform3x3( VectorR3* u ) const { + double newX, newY; + newX = m11*u->x + m12*u->y + m13*u->z; + newY = m21*u->x + m22*u->y + m23*u->z; + u->z = m31*u->x + m32*u->y + m33*u->z; + u->x = newX; + u->y = newY; +} + +inline void Matrix3x4::Transform( const VectorR3& src, VectorR3* dest ) const { + dest->x = m11*src.x + m12*src.y + m13*src.z + m14; + dest->y = m21*src.x + m22*src.y + m23*src.z + m24; + dest->z = m31*src.x + m32*src.y + m33*src.z + m34; +} + +inline void Matrix3x4::Transform3x3( const VectorR3& src, VectorR3* dest ) const { + dest->x = m11*src.x + m12*src.y + m13*src.z; + dest->y = m21*src.x + m22*src.y + m23*src.z; + dest->z = m31*src.x + m32*src.y + m33*src.z; +} + +inline void Matrix3x4::Transform3x3Transpose( VectorR3* u ) const { + double newX, newY; + newX = m11*u->x + m21*u->y + m31*u->z; + newY = m12*u->x + m22*u->y + m32*u->z; + u->z = m13*u->x + m23*u->y + m33*u->z; + u->x = newX; + u->y = newY; +} + +inline void Matrix3x4::Transform3x3Transpose( const VectorR3& src, VectorR3* dest ) const { + dest->x = m11*src.x + m21*src.y + m31*src.z; + dest->y = m12*src.x + m22*src.y + m32*src.z; + dest->z = m13*src.x + m23*src.y + m33*src.z; +} + +inline VectorR3 operator* ( const Matrix3x4& A, const VectorR3& u ) +{ + return( VectorR3( A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14, + A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24, + A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34) ); +} + + +// ****************************************************** +// * LinearMapR3 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline LinearMapR3::LinearMapR3() +{ + SetZero(); + return; +} + +inline LinearMapR3::LinearMapR3( const VectorR3& u, const VectorR3& v, + const VectorR3& s ) +:Matrix3x3 ( u, v, s ) +{ } + +inline LinearMapR3::LinearMapR3( + double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33) + // Values specified in column order!!! +:Matrix3x3 ( a11, a21, a31, a12, a22, a32, a13, a23, a33) +{ } + +inline LinearMapR3::LinearMapR3 ( const Matrix3x3& A ) +: Matrix3x3 (A) +{} + +inline void LinearMapR3::SetZero( ) +{ + Matrix3x3::SetZero(); +} + +inline void LinearMapR3::Negate() +{ + m11 = -m11; // Row 1 + m12 = -m12; + m13 = -m13; + m21 = -m21; // Row 2 + m22 = -m22; + m23 = -m23; + m31 = -m31; // Row 3 + m32 = -m32; + m33 = -m33; +} + + +inline LinearMapR3& LinearMapR3::operator+= (const Matrix3x3& B) +{ + m11 += B.m11; + m12 += B.m12; + m13 += B.m13; + m21 += B.m21; + m22 += B.m22; + m23 += B.m23; + m31 += B.m31; + m32 += B.m32; + m33 += B.m33; + return ( *this ); +} + +inline LinearMapR3& LinearMapR3::operator-= (const Matrix3x3& B) +{ + m11 -= B.m11; + m12 -= B.m12; + m13 -= B.m13; + m21 -= B.m21; + m22 -= B.m22; + m23 -= B.m23; + m31 -= B.m31; + m32 -= B.m32; + m33 -= B.m33; + return( *this ); +} + +inline LinearMapR3 operator+ (const LinearMapR3& A, const Matrix3x3& B) +{ + return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, + A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, + A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) ); +} + +inline LinearMapR3 operator+ (const Matrix3x3& A, const LinearMapR3& B) +{ + return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, + A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, + A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) ); +} + +inline LinearMapR3 operator- (const LinearMapR3& A) +{ + return( LinearMapR3( -A.m11, -A.m21, -A.m31, + -A.m12, -A.m22, -A.m32, + -A.m13, -A.m23, -A.m33 ) ); +} + +inline LinearMapR3 operator- (const Matrix3x3& A, const LinearMapR3& B) +{ + return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, + A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, + A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) ); +} + +inline LinearMapR3 operator- (const LinearMapR3& A, const Matrix3x3& B) +{ + return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, + A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, + A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) ); +} + +inline LinearMapR3& LinearMapR3::operator*= (register double b) +{ + m11 *= b; + m12 *= b; + m13 *= b; + m21 *= b; + m22 *= b; + m23 *= b; + m31 *= b; + m32 *= b; + m33 *= b; + return ( *this); +} + +inline LinearMapR3 operator* ( const LinearMapR3& A, register double b) +{ + return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b, + A.m12*b, A.m22*b, A.m32*b, + A.m13*b, A.m23*b, A.m33*b ) ); +} + +inline LinearMapR3 operator* ( register double b, const LinearMapR3& A) +{ + return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b, + A.m12*b, A.m22*b, A.m32*b, + A.m13*b, A.m23*b, A.m33*b ) ); +} + +inline LinearMapR3 operator/ ( const LinearMapR3& A, double b) +{ + register double bInv = 1.0/b; + return( LinearMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv, + A.m12*bInv, A.m22*bInv, A.m32*bInv, + A.m13*bInv, A.m23*bInv, A.m33*bInv ) ); +} + +inline LinearMapR3& LinearMapR3::operator/= (register double b) +{ + register double bInv = 1.0/b; + return ( *this *= bInv ); +} + +inline LinearMapR3& LinearMapR3::operator*= (const Matrix3x3& B) // Matrix product +{ + OperatorTimesEquals( B ); + return( *this ); +} + +inline VectorR3 LinearMapR3::Solve(const VectorR3& u) const // Returns solution +{ + return ( Matrix3x3::Solve( u ) ); +} + +inline LinearMapR3 LinearMapR3::Transpose() const // Returns the transpose +{ + return ( LinearMapR3 ( m11, m12, m13, m21, m22, m23, m31, m32, m33) ); +} + +// ****************************************************** +// * AffineMapR3 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline AffineMapR3::AffineMapR3( double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33, + double a14, double a24, double a34) +{ // Values in COLUMN order! + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; +} + +inline AffineMapR3::AffineMapR3 (const VectorR3& u, const VectorR3& v, + const VectorR3& w, const VectorR3& t) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m13 = w.x; // Column 3 + m23 = w.y; + m33 = w.z; + m14 = t.x; // Column 4 + m24 = t.y; + m34 = t.z; +} + +inline AffineMapR3::AffineMapR3 (const LinearMapR3& A, const VectorR3& t) +{ + m11 = A.m11; + m12 = A.m12; + m13 = A.m13; + m14 = t.x; + m21 = A.m21; + m22 = A.m22; + m23 = A.m23; + m24 = t.y; + m31 = A.m31; + m32 = A.m32; + m33 = A.m33; + m34 = t.z; + +} + +inline void AffineMapR3::SetIdentity ( ) +{ + Matrix3x4::SetIdentity(); +} + +inline void AffineMapR3::SetZero( ) +{ + Matrix3x4::SetZero(); +} + +inline VectorR3 AffineMapR3::Solve(const VectorR3& u) const // Returns solution +{ + return ( Matrix3x4::Solve( u ) ); +} + + +inline AffineMapR3& AffineMapR3::operator+= (const Matrix3x4& B) +{ + m11 += B.m11; + m21 += B.m21; + m31 += B.m31; + m12 += B.m12; + m22 += B.m22; + m32 += B.m32; + m13 += B.m13; + m23 += B.m23; + m33 += B.m33; + m14 += B.m14; + m24 += B.m24; + m34 += B.m34; + return (*this); +} + +inline AffineMapR3& AffineMapR3::operator-= (const Matrix3x4& B) +{ + m11 -= B.m11; + m21 -= B.m21; + m31 -= B.m31; + m12 -= B.m12; + m22 -= B.m22; + m32 -= B.m32; + m13 -= B.m13; + m23 -= B.m23; + m33 -= B.m33; + m14 -= B.m14; + m24 -= B.m24; + m34 -= B.m34; + return (*this); + +} + +inline AffineMapR3 operator+ (const AffineMapR3& A, const AffineMapR3& B) +{ + return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, + A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, + A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, + A.m14+B.m14, A.m23+B.m24, A.m34+B.m34) ); +} + +inline AffineMapR3 operator+ (const AffineMapR3& A, const Matrix3x3& B) +{ + return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, + A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, + A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, + A.m14, A.m23, A.m34 ) ); +} + +inline AffineMapR3 operator+ (const Matrix3x3& B, const AffineMapR3& A) +{ + return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, + A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, + A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, + A.m14, A.m23, A.m34 ) ); +} + +inline AffineMapR3 operator- (const AffineMapR3& A, const AffineMapR3& B) +{ + return( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, + A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, + A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, + A.m14-B.m14, A.m23-B.m24, A.m34-B.m34) ); +} + +inline AffineMapR3 operator- (const AffineMapR3& A, const LinearMapR3& B) +{ + return ( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, + A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, + A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, + A.m14, A.m23, A.m34 ) ); +} + +inline AffineMapR3 operator- (const LinearMapR3& B, const AffineMapR3& A) +{ + return( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, + A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, + A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, + A.m14, A.m23, A.m34 ) ); +} + + +inline AffineMapR3& AffineMapR3::operator*= (register double b) +{ + m11 *= b; + m12 *= b; + m13 *= b; + m21 *= b; + m22 *= b; + m23 *= b; + m31 *= b; + m32 *= b; + m33 *= b; + m14 *= b; + m24 *= b; + m34 *= b; + return (*this); +} + +inline AffineMapR3 operator* (const AffineMapR3& A, register double b) +{ + return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b, + A.m12*b, A.m22*b, A.m32*b, + A.m13*b, A.m23*b, A.m33*b, + A.m14*b, A.m24*b, A.m34*b ) ); +} + +inline AffineMapR3 operator* (register double b, const AffineMapR3& A) +{ + return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b, + A.m12*b, A.m22*b, A.m32*b, + A.m13*b, A.m23*b, A.m33*b, + A.m14*b, A.m24*b, A.m34*b ) ); +} + +inline AffineMapR3& AffineMapR3::operator/= (double b) +{ + register double bInv = 1.0/b; + *this *= bInv; + return( *this ); +} + +inline AffineMapR3 operator/ (const AffineMapR3& A, double b) +{ + register double bInv = 1.0/b; + return(AffineMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv, + A.m12*bInv, A.m22*bInv, A.m32*bInv, + A.m13*bInv, A.m23*bInv, A.m33*bInv, + A.m14*bInv, A.m24*bInv, A.m34*bInv ) ); +} + +inline AffineMapR3& AffineMapR3::operator*= (const Matrix3x3& B) // Composition +{ + OperatorTimesEquals( B ); + return( *this ); +} + +inline AffineMapR3& AffineMapR3::operator*= (const Matrix3x4& B) // Composition +{ + OperatorTimesEquals( B ); + return( *this ); +} + +// ************************************************************** +// RotationMapR3 class (inlined functions) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline RotationMapR3::RotationMapR3() +{ + SetIdentity(); + return; +} + +inline RotationMapR3::RotationMapR3( const VectorR3& u, const VectorR3& v, + const VectorR3& s ) +:Matrix3x3 ( u, v, s ) +{ } + +inline RotationMapR3::RotationMapR3( + double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33) + // Values specified in column order!!! +:Matrix3x3 ( a11, a21, a31, a12, a22, a32, a13, a23, a33) +{ } + +inline RotationMapR3 RotationMapR3::Inverse() const // Returns inverse +{ + return ( RotationMapR3 ( m11, m12, m13, // In column order! + m21, m22, m23, + m31, m32, m33 ) ); +} + +inline RotationMapR3& RotationMapR3::Invert() // Converts into inverse. +{ + register double temp; + temp = m12; + m12 = m21; + m21 = temp; + temp = m13; + m13 = m31; + m31 = temp; + temp = m23; + m23 = m32; + m32 = temp; + return ( *this ); +} + +inline VectorR3 RotationMapR3::Solve(const VectorR3& u) const // Returns solution +{ + return( VectorR3( m11*u.x + m21*u.y + m31*u.z, + m12*u.x + m22*u.y + m32*u.z, + m13*u.x + m23*u.y + m33*u.z ) ); +} + +inline RotationMapR3& RotationMapR3::operator*= (const RotationMapR3& B) // Matrix product +{ + OperatorTimesEquals( B ); + return( *this ); +} + + +// ************************************************************** +// RigidMapR3 class (inlined functions) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline RigidMapR3::RigidMapR3() +{ + SetIdentity(); + return; +} + +inline RigidMapR3::RigidMapR3( const VectorR3& u, const VectorR3& v, + const VectorR3& s, const VectorR3& t ) +:Matrix3x4 ( u, v, s, t ) +{} + +inline RigidMapR3::RigidMapR3( + double a11, double a21, double a31, + double a12, double a22, double a32, + double a13, double a23, double a33, + double a14, double a24, double a34) + // Values specified in column order!!! +:Matrix3x4 ( a11, a21, a31, a12, a22, a32, a13, a23, a33, a14, a24, a34 ) +{} + +inline RigidMapR3::RigidMapR3( const Matrix3x3& A, const VectorR3& u ) // Set to RotationMap & Vector +: Matrix3x4( A, u ) +{} + + +inline RigidMapR3& RigidMapR3::Set( const Matrix3x3& A, const VectorR3& u ) // Set to RotationMap & Vector +{ + Matrix3x4::Set( A, u ); + return *this; +} + +inline RigidMapR3& RigidMapR3::SetTranslationPart( const VectorR3& u) // Set the translation part +{ + SetColumn4( u ); + return *this; +} + +inline RigidMapR3& RigidMapR3::SetTranslationPart( double x, double y, double z) // Set the translation part +{ + SetColumn4( x, y, z ); + return *this; +} + +inline RigidMapR3& RigidMapR3::SetRotationPart( const Matrix3x3& A) // Set the rotation part +{ + Matrix3x4::Set3x3( A ); + return *this; +} + +inline RigidMapR3& RigidMapR3::operator*= (const RotationMapR3& B) // Composition +{ + OperatorTimesEquals( B ); + return( *this ); +} + +inline RigidMapR3& RigidMapR3::operator*= (const RigidMapR3& B) // Composition +{ + OperatorTimesEquals( B ); + return( *this ); +} + +inline RigidMapR3 RigidMapR3::Inverse() const // Returns inverse +{ + double new14 = -(m11*m14 + m21*m24 + m31*m34); + double new24 = -(m12*m14 + m22*m24 + m32*m34); + double new34 = -(m13*m14 + m23*m24 + m33*m34); + return ( RigidMapR3 ( m11, m12, m13, // In column order! + m21, m22, m23, + m31, m32, m33, + new14, new24, new34 ) ); +} + +inline RigidMapR3& RigidMapR3::Invert() // Converts into inverse. +{ + double new14 = -(m11*m14 + m21*m24 + m31*m34); + double new24 = -(m12*m14 + m22*m24 + m32*m34); + m34 = -(m13*m14 + m23*m24 + m33*m34); + m14 = new14; + m24 = new24; + + register double temp; + temp = m12; + m12 = m21; + m21 = temp; + temp = m13; + m13 = m31; + m31 = temp; + temp = m23; + m23 = m32; + m32 = temp; + return ( *this ); +} + +// *************************************************************** +// * 3-space vector and matrix utilities (inlined functions) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns the projection of u onto unit v +inline VectorR3 ProjectToUnit ( const VectorR3& u, const VectorR3& v) +{ + return (u^v)*v; +} + +// Returns the projection of u onto the plane perpindicular to the unit vector v +inline VectorR3 ProjectPerpUnit ( const VectorR3& u, const VectorR3& v) +{ + return ( u - ((u^v)*v) ); +} + +// Returns the projection of u onto the plane perpindicular to the unit vector v +// This one is more stable when u and v are nearly equal. +inline VectorR3 ProjectPerpUnitDiff ( const VectorR3& u, const VectorR3& v) +{ + VectorR3 ans = u; + ans -= v; + ans -= ((ans^v)*v); + return ans; // ans = (u-v) - ((u-v)^v)*v +} + +// VectorProjectMap returns map projecting onto a given vector u. +// u should be a unit vector (otherwise the returned map is +// scaled according to the magnitude of u. +inline LinearMapR3 VectorProjectMap( const VectorR3& u ) +{ + double a = u.x*u.y; + double b = u.x*u.z; + double c = u.y*u.z; + return( LinearMapR3( u.x*u.x, a, b, + a, u.y*u.y, c, + b, c, u.z*u.z ) ); +} + +// PlaneProjectMap returns map projecting onto a given plane. +// The plane is the plane orthognal to w. +// w must be a unit vector (otherwise the returned map is +// garbage). +inline LinearMapR3 PlaneProjectMap ( const VectorR3& w ) +{ + double a = -w.x*w.y; + double b = -w.x*w.z; + double c = -w.y*w.z; + return( LinearMapR3( 1.0-w.x*w.x, a, b, + a, 1.0-w.y*w.y, c, + b, c, 1.0-w.z*w.z ) ); +} + +// PlaneProjectMap returns map projecting onto a given plane. +// The plane is the plane containing the two orthonormal vectors u,v. +// If u, v are orthonormal, this is a projection with scaling. +// If they are not orthonormal, the results are more difficult +// to interpret. +inline LinearMapR3 PlaneProjectMap ( const VectorR3& u, const VectorR3 &v ) +{ + double a = u.x*u.y + v.x*v.y; + double b = u.x*u.z + v.x*v.z; + double c = u.y*u.z + v.y*v.z; + return( LinearMapR3( u.x*u.x+v.x*v.x, a, b, + a, u.y*u.y+u.y*u.y, c, + b, c, u.z*u.z+v.z*v.z ) ); +} + +// Returns the solid angle between unit vectors v and w. +inline double SolidAngle( const VectorR3& v, const VectorR3& w) +{ + return atan2 ( (v*w).Norm(), v^w ); +} + + +#endif + +// ******************* End of header material ******************** diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.cpp b/examples/ThirdPartyLibs/BussIK/LinearR4.cpp new file mode 100644 index 000000000..f662a7d90 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.cpp @@ -0,0 +1,467 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +#include "LinearR4.h" + +#include + +const VectorR4 VectorR4::Zero(0.0, 0.0, 0.0, 0.0); +const VectorR4 VectorR4::UnitX( 1.0, 0.0, 0.0, 0.0); +const VectorR4 VectorR4::UnitY( 0.0, 1.0, 0.0, 0.0); +const VectorR4 VectorR4::UnitZ( 0.0, 0.0, 1.0, 0.0); +const VectorR4 VectorR4::UnitW( 0.0, 0.0, 0.0, 1.0); +const VectorR4 VectorR4::NegUnitX(-1.0, 0.0, 0.0, 0.0); +const VectorR4 VectorR4::NegUnitY( 0.0,-1.0, 0.0, 0.0); +const VectorR4 VectorR4::NegUnitZ( 0.0, 0.0,-1.0, 0.0); +const VectorR4 VectorR4::NegUnitW( 0.0, 0.0, 0.0,-1.0); + +const Matrix4x4 Matrix4x4::Identity(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0); + + +// ****************************************************** +// * VectorR4 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +double VectorR4::MaxAbs() const +{ + register double m; + m = (x>0.0) ? x : -x; + if ( y>m ) m=y; + else if ( -y >m ) m = -y; + if ( z>m ) m=z; + else if ( -z>m ) m = -z; + if ( w>m ) m=w; + else if ( -w>m ) m = -w; + return m; +} + +// ****************************************************** +// * Matrix4x4 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + +void Matrix4x4::operator*= (const Matrix4x4& B) // Matrix product +{ + double t1, t2, t3; // temporary values + t1 = m11*B.m11 + m12*B.m21 + m13*B.m31 + m14*B.m41; + t2 = m11*B.m12 + m12*B.m22 + m13*B.m32 + m14*B.m42; + t3 = m11*B.m13 + m12*B.m23 + m13*B.m33 + m14*B.m43; + m14 = m11*B.m14 + m12*B.m24 + m13*B.m34 + m14*B.m44; + m11 = t1; + m12 = t2; + m13 = t3; + + t1 = m21*B.m11 + m22*B.m21 + m23*B.m31 + m24*B.m41; + t2 = m21*B.m12 + m22*B.m22 + m23*B.m32 + m24*B.m42; + t3 = m21*B.m13 + m22*B.m23 + m23*B.m33 + m24*B.m43; + m24 = m21*B.m14 + m22*B.m24 + m23*B.m34 + m24*B.m44; + m21 = t1; + m22 = t2; + m23 = t3; + + t1 = m31*B.m11 + m32*B.m21 + m33*B.m31 + m34*B.m41; + t2 = m31*B.m12 + m32*B.m22 + m33*B.m32 + m34*B.m42; + t3 = m31*B.m13 + m32*B.m23 + m33*B.m33 + m34*B.m43; + m34 = m31*B.m14 + m32*B.m24 + m33*B.m34 + m34*B.m44; + m31 = t1; + m32 = t2; + m33 = t3; + + t1 = m41*B.m11 + m42*B.m21 + m43*B.m31 + m44*B.m41; + t2 = m41*B.m12 + m42*B.m22 + m43*B.m32 + m44*B.m42; + t3 = m41*B.m13 + m42*B.m23 + m43*B.m33 + m44*B.m43; + m44 = m41*B.m14 + m42*B.m24 + m43*B.m34 + m44*B.m44; + m41 = t1; + m42 = t2; + m43 = t3; +} + +inline void ReNormalizeHelper ( double &a, double &b, double &c, double &d ) +{ + register double scaleF = a*a+b*b+c*c+d*d; // Inner product of Vector-R4 + scaleF = 1.0-0.5*(scaleF-1.0); + a *= scaleF; + b *= scaleF; + c *= scaleF; + d *= scaleF; +} + +Matrix4x4& Matrix4x4::ReNormalize() { + ReNormalizeHelper( m11, m21, m31, m41 ); // Renormalize first column + ReNormalizeHelper( m12, m22, m32, m42 ); // Renormalize second column + ReNormalizeHelper( m13, m23, m33, m43 ); // Renormalize third column + ReNormalizeHelper( m14, m24, m34, m44 ); // Renormalize fourth column + double alpha = 0.5*(m11*m12 + m21*m22 + m31*m32 + m41*m42); //1st and 2nd cols + double beta = 0.5*(m11*m13 + m21*m23 + m31*m33 + m41*m43); //1st and 3rd cols + double gamma = 0.5*(m11*m14 + m21*m24 + m31*m34 + m41*m44); //1st and 4nd cols + double delta = 0.5*(m12*m13 + m22*m23 + m32*m33 + m42*m43); //2nd and 3rd cols + double eps = 0.5*(m12*m14 + m22*m24 + m32*m34 + m42*m44); //2nd and 4nd cols + double phi = 0.5*(m13*m14 + m23*m24 + m33*m34 + m43*m44); //3rd and 4nd cols + double temp1, temp2, temp3; + temp1 = m11 - alpha*m12 - beta*m13 - gamma*m14; + temp2 = m12 - alpha*m11 - delta*m13 - eps*m14; + temp3 = m13 - beta*m11 - delta*m12 - phi*m14; + m14 -= (gamma*m11 + eps*m12 + phi*m13); + m11 = temp1; + m12 = temp2; + m13 = temp3; + temp1 = m21 - alpha*m22 - beta*m23 - gamma*m24; + temp2 = m22 - alpha*m21 - delta*m23 - eps*m24; + temp3 = m23 - beta*m21 - delta*m22 - phi*m24; + m24 -= (gamma*m21 + eps*m22 + phi*m23); + m21 = temp1; + m22 = temp2; + m23 = temp3; + temp1 = m31 - alpha*m32 - beta*m33 - gamma*m34; + temp2 = m32 - alpha*m31 - delta*m33 - eps*m34; + temp3 = m33 - beta*m31 - delta*m32 - phi*m34; + m34 -= (gamma*m31 + eps*m32 + phi*m33); + m31 = temp1; + m32 = temp2; + m33 = temp3; + temp1 = m41 - alpha*m42 - beta*m43 - gamma*m44; + temp2 = m42 - alpha*m41 - delta*m43 - eps*m44; + temp3 = m43 - beta*m41 - delta*m42 - phi*m44; + m44 -= (gamma*m41 + eps*m42 + phi*m43); + m41 = temp1; + m42 = temp2; + m43 = temp3; + return *this; +} + +// ****************************************************** +// * LinearMapR4 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + +double LinearMapR4::Determinant () const // Returns the determinant +{ + double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants + double Tbt34C13 = m31*m43-m33*m41; + double Tbt34C14 = m31*m44-m34*m41; + double Tbt34C23 = m32*m43-m33*m42; + double Tbt34C24 = m32*m44-m34*m42; + double Tbt34C34 = m33*m44-m34*m43; + + double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants + double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13; + double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12; + double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12; + + return ( m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14 ); +} + +LinearMapR4 LinearMapR4::Inverse() const // Returns inverse +{ + + double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants + double Tbt34C13 = m31*m43-m33*m41; + double Tbt34C14 = m31*m44-m34*m41; + double Tbt34C23 = m32*m43-m33*m42; + double Tbt34C24 = m32*m44-m34*m42; + double Tbt34C34 = m33*m44-m34*m43; + double Tbt24C12 = m21*m42-m22*m41; // 2x2 subdeterminants + double Tbt24C13 = m21*m43-m23*m41; + double Tbt24C14 = m21*m44-m24*m41; + double Tbt24C23 = m22*m43-m23*m42; + double Tbt24C24 = m22*m44-m24*m42; + double Tbt24C34 = m23*m44-m24*m43; + double Tbt23C12 = m21*m32-m22*m31; // 2x2 subdeterminants + double Tbt23C13 = m21*m33-m23*m31; + double Tbt23C14 = m21*m34-m24*m31; + double Tbt23C23 = m22*m33-m23*m32; + double Tbt23C24 = m22*m34-m24*m32; + double Tbt23C34 = m23*m34-m24*m33; + + double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants + double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13; + double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12; + double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12; + double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23; // 3x3 subdeterminants + double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13; + double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12; + double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12; + double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23; // 3x3 subdeterminants + double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13; + double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12; + double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12; + double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23; // 3x3 subdeterminants + double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13; + double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12; + double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12; + + + register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14); + + return( LinearMapR4( sd11*detInv, -sd12*detInv, sd13*detInv, -sd14*detInv, + -sd21*detInv, sd22*detInv, -sd23*detInv, sd24*detInv, + sd31*detInv, -sd32*detInv, sd33*detInv, -sd34*detInv, + -sd41*detInv, sd42*detInv, -sd43*detInv, sd44*detInv ) ); +} + +LinearMapR4& LinearMapR4::Invert() // Converts into inverse. +{ + double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants + double Tbt34C13 = m31*m43-m33*m41; + double Tbt34C14 = m31*m44-m34*m41; + double Tbt34C23 = m32*m43-m33*m42; + double Tbt34C24 = m32*m44-m34*m42; + double Tbt34C34 = m33*m44-m34*m43; + double Tbt24C12 = m21*m42-m22*m41; // 2x2 subdeterminants + double Tbt24C13 = m21*m43-m23*m41; + double Tbt24C14 = m21*m44-m24*m41; + double Tbt24C23 = m22*m43-m23*m42; + double Tbt24C24 = m22*m44-m24*m42; + double Tbt24C34 = m23*m44-m24*m43; + double Tbt23C12 = m21*m32-m22*m31; // 2x2 subdeterminants + double Tbt23C13 = m21*m33-m23*m31; + double Tbt23C14 = m21*m34-m24*m31; + double Tbt23C23 = m22*m33-m23*m32; + double Tbt23C24 = m22*m34-m24*m32; + double Tbt23C34 = m23*m34-m24*m33; + + double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants + double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13; + double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12; + double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12; + double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23; // 3x3 subdeterminants + double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13; + double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12; + double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12; + double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23; // 3x3 subdeterminants + double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13; + double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12; + double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12; + double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23; // 3x3 subdeterminants + double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13; + double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12; + double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12; + + register double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14); + + m11 = sd11*detInv; + m12 = -sd21*detInv; + m13 = sd31*detInv; + m14 = -sd41*detInv; + m21 = -sd12*detInv; + m22 = sd22*detInv; + m23 = -sd32*detInv; + m24 = sd42*detInv; + m31 = sd13*detInv; + m32 = -sd23*detInv; + m33 = sd33*detInv; + m34 = -sd43*detInv; + m41 = -sd14*detInv; + m42 = sd24*detInv; + m43 = -sd34*detInv; + m44 = sd44*detInv; + + return ( *this ); +} + +VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution +{ + // Just uses Inverse() for now. + return ( Inverse()*u ); +} + +// ****************************************************** +// * RotationMapR4 class - math library functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + + +// *************************************************************** +// * 4-space vector and matrix utilities * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns u * v^T +LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v) +{ + LinearMapR4 result; + TimesTranspose( u, v, result ); + return result; +} + +// The following routines are use to obtain +// a righthanded orthonormal basis to complement vectors u,v,w. +// The vectors u,v,w must be unit and orthonormal. +// The value is returned in "rotmat" with the first column(s) of +// rotmat equal to u,v,w as appropriate. + +void GetOrtho( const VectorR4& u, RotationMapR4& rotmat ) +{ + rotmat.SetColumn1(u); + GetOrtho( 1, rotmat ); +} + +void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat ) +{ + rotmat.SetColumn1(u); + rotmat.SetColumn2(v); + GetOrtho( 2, rotmat ); +} + +void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& s, + RotationMapR4& rotmat ) +{ + rotmat.SetColumn1(u); + rotmat.SetColumn2(v); + rotmat.SetColumn3(s); + GetOrtho( 3, rotmat ); +} + +// This final version of GetOrtho is mainly for internal use. +// It uses a Gram-Schmidt procedure to extend a partial orthonormal +// basis to a complete orthonormal basis. +// j = number of columns of rotmat that have already been set. +void GetOrtho( int j, RotationMapR4& rotmat) +{ + if ( j==0 ) { + rotmat.SetIdentity(); + return; + } + if ( j==1 ) { + rotmat.SetColumn2( -rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31 ); + j = 2; + } + + assert ( rotmat.Column1().Norm()<1.0001 && 0.9999 -0.001 ); + + // 2x2 subdeterminants in first 2 columns + + double d12 = rotmat.m11*rotmat.m22-rotmat.m12*rotmat.m21; + double d13 = rotmat.m11*rotmat.m32-rotmat.m12*rotmat.m31; + double d14 = rotmat.m11*rotmat.m42-rotmat.m12*rotmat.m41; + double d23 = rotmat.m21*rotmat.m32-rotmat.m22*rotmat.m31; + double d24 = rotmat.m21*rotmat.m42-rotmat.m22*rotmat.m41; + double d34 = rotmat.m31*rotmat.m42-rotmat.m32*rotmat.m41; + VectorR4 vec3; + + if ( j==2 ) { + if ( d12>0.4 || d12<-0.4 || d13>0.4 || d13<-0.4 + || d23>0.4 || d23<-0.4 ) { + vec3.Set( d23, -d13, d12, 0.0); + } + else if ( d24>0.4 || d24<-0.4 || d14>0.4 || d14<-0.4 ) { + vec3.Set( d24, -d14, 0.0, d12 ); + } + else { + vec3.Set( d34, 0.0, -d14, d13 ); + } + vec3.Normalize(); + rotmat.SetColumn3(vec3); + } + + // Do the final column + + rotmat.SetColumn4 ( + -rotmat.m23*d34 + rotmat.m33*d24 - rotmat.m43*d23, + rotmat.m13*d34 - rotmat.m33*d14 + rotmat.m43*d13, + -rotmat.m13*d24 + rotmat.m23*d14 - rotmat.m43*d12, + rotmat.m13*d23 - rotmat.m23*d13 + rotmat.m33*d12 ); + + assert ( 0.99 < (((LinearMapR4)rotmat)).Determinant() + && (((LinearMapR4)rotmat)).Determinant() < 1.01 ); + +} + + + +// ********************************************************************* +// Rotation routines * +// ********************************************************************* + +// Rotate unit vector x in the direction of "dir": length of dir is rotation angle. +// x must be a unit vector. dir must be perpindicular to x. +VectorR4& VectorR4::RotateUnitInDirection ( const VectorR4& dir) +{ + assert ( this->Norm()<1.0001 && this->Norm()>0.9999 && + (dir^(*this))<0.0001 && (dir^(*this))>-0.0001 ); + + double theta = dir.NormSq(); + if ( theta==0.0 ) { + return *this; + } + else { + theta = sqrt(theta); + double costheta = cos(theta); + double sintheta = sin(theta); + VectorR4 dirUnit = dir/theta; + *this = costheta*(*this) + sintheta*dirUnit; + // this->NormalizeFast(); + return ( *this ); + } +} + +// RotateToMap returns a RotationMapR4 that rotates fromVec to toVec, +// leaving the orthogonal subspace fixed. +// fromVec and toVec should be unit vectors +RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec) +{ + LinearMapR4 result; + result.SetIdentity(); + LinearMapR4 temp; + VectorR4 vPerp = ProjectPerpUnitDiff( toVec, fromVec ); + double sintheta = vPerp.Norm(); // theta = angle between toVec and fromVec + VectorR4 vProj = toVec-vPerp; + double costheta = vProj^fromVec; + if ( sintheta == 0.0 ) { + // The vectors either coincide (return identity) or directly oppose + if ( costheta < 0.0 ) { + result = -result; // Vectors directly oppose: return -identity. + } + } + else { + vPerp /= sintheta; // Normalize + VectorProjectMap ( fromVec, temp ); // project in fromVec direction + temp *= (costheta-1.0); + result += temp; + VectorProjectMap ( vPerp, temp ); // Project in vPerp direction + temp *= (costheta-1.0); + result += temp; + TimesTranspose ( vPerp, fromVec, temp ); // temp = (vPerp)*(fromVec^T) + temp *= sintheta; + result += temp; + temp.MakeTranspose(); + result -= temp; // (-sintheta)*(fromVec)*(vPerp^T) + } + RotationMapR4 rotationResult; + rotationResult.Set(result); // Make explicitly a RotationMapR4 + return rotationResult; +} + + +// *************************************************************** +// Stream Output Routines * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +ostream& operator<< ( ostream& os, const VectorR4& u ) +{ + return (os << "<" << u.x << "," << u.y << "," << u.z << "," << u.w << ">"); +} + + diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h new file mode 100644 index 000000000..ee995326c --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h @@ -0,0 +1,1099 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +// +// Linear Algebra Classes over R4 +// +// +// A. Vector and Position classes +// +// A.1. VectorR4: a column vector of length 4 +// +// B. Matrix Classes +// +// B.1 LinearMapR4 - arbitrary linear map; 4x4 real matrix +// +// B.2 RotationMapR4 - orthonormal 4x4 matrix +// + +#ifndef LINEAR_R4_H +#define LINEAR_R4_H + +#include +#include +#include +#include "LinearR3.h" +using namespace std; + +class VectorR4; // R4 Vector +class LinearMapR4; // 4x4 real matrix +class RotationMapR4; // 4x4 rotation map + +// ************************************** +// VectorR4 class * +// * * * * * * * * * * * * * * * * * * ** + +class VectorR4 { + +public: + double x, y, z, w; // The x & y & z & w coordinates. + + static const VectorR4 Zero; + static const VectorR4 UnitX; + static const VectorR4 UnitY; + static const VectorR4 UnitZ; + static const VectorR4 UnitW; + static const VectorR4 NegUnitX; + static const VectorR4 NegUnitY; + static const VectorR4 NegUnitZ; + static const VectorR4 NegUnitW; + +public: + VectorR4( ) : x(0.0), y(0.0), z(0.0), w(0.0) {} + VectorR4( double xVal, double yVal, double zVal, double wVal ) + : x(xVal), y(yVal), z(zVal), w(wVal) {} + VectorR4( const Quaternion& q); // Definition with Quaternion routines + + VectorR4& SetZero() { x=0.0; y=0.0; z=0.0; w=0.0; return *this;} + VectorR4& Set( double xx, double yy, double zz, double ww ) + { x=xx; y=yy; z=zz; w=ww; return *this;} + VectorR4& Set ( const Quaternion& ); // Defined with Quaternion + VectorR4& Set ( const VectorHgR3& h ) {x=h.x; y=h.y; z=h.z; w=h.w; return *this; } + VectorR4& Load( const double* v ); + VectorR4& Load( const float* v ); + void Dump( double* v ) const; + void Dump( float* v ) const; + + VectorR4& operator+= ( const VectorR4& v ) + { x+=v.x; y+=v.y; z+=v.z; w+=v.w; return(*this); } + VectorR4& operator-= ( const VectorR4& v ) + { x-=v.x; y-=v.y; z-=v.z; w-=v.w; return(*this); } + VectorR4& operator*= ( double m ) + { x*=m; y*=m; z*=m; w*=m; return(*this); } + VectorR4& operator/= ( double m ) + { register double mInv = 1.0/m; + x*=mInv; y*=mInv; z*=mInv; w*=mInv; + return(*this); } + VectorR4 operator- () const { return ( VectorR4(-x, -y, -z, -w) ); } + VectorR4& ArrayProd(const VectorR4&); // Component-wise product + VectorR4& ArrayProd3(const VectorR3&); // Component-wise product + + VectorR4& AddScaled( const VectorR4& u, double s ); + + double Norm() const { return ( (double)sqrt( x*x + y*y + z*z +w*w) ); } + double NormSq() const { return ( x*x + y*y + z*z + w*w ); } + double Dist( const VectorR4& u ) const; // Distance from u + double DistSq( const VectorR4& u ) const; // Distance from u + double MaxAbs() const; + VectorR4& Normalize () { *this /= Norm(); return *this; } // No error checking + inline VectorR4& MakeUnit(); // Normalize() with error checking + inline VectorR4& ReNormalize(); + bool IsUnit( ) const + { register double norm = Norm(); + return ( 1.000001>=norm && norm>=0.999999 ); } + bool IsUnit( double tolerance ) const + { register double norm = Norm(); + return ( 1.0+tolerance>=norm && norm>=1.0-tolerance ); } + bool IsZero() const { return ( x==0.0 && y==0.0 && z==0.0 && w==0.0); } + bool NearZero(double tolerance) const { return( MaxAbs()<=tolerance );} + // tolerance should be non-negative + + VectorR4& RotateUnitInDirection ( const VectorR4& dir); // rotate in direction dir + +}; + +inline VectorR4 operator+( const VectorR4& u, const VectorR4& v ); +inline VectorR4 operator-( const VectorR4& u, const VectorR4& v ); +inline VectorR4 operator*( const VectorR4& u, double m); +inline VectorR4 operator*( double m, const VectorR4& u); +inline VectorR4 operator/( const VectorR4& u, double m); +inline int operator==( const VectorR4& u, const VectorR4& v ); + +inline double operator^ (const VectorR4& u, const VectorR4& v ); // Dot Product +inline VectorR4 ArrayProd(const VectorR4& u, const VectorR4& v ); + +inline double Mag(const VectorR4& u) { return u.Norm(); } +inline double Dist(const VectorR4& u, const VectorR4& v) { return u.Dist(v); } +inline double DistSq(const VectorR4& u, const VectorR4& v) { return u.DistSq(v); } +inline double NormalizeError (const VectorR4& u); + +// ******************************************************************** +// Matrix4x4 - base class for 4x4 matrices * +// * * * * * * * * * * * * * * * * * * * * * ************************** + +class Matrix4x4 { + +public: + double m11, m12, m13, m14, m21, m22, m23, m24, + m31, m32, m33, m34, m41, m42, m43, m44; + + // Implements a 4x4 matrix: m_i_j - row-i and column-j entry + + static const Matrix4x4 Identity; + + +public: + + Matrix4x4(); + Matrix4x4( const VectorR4&, const VectorR4&, + const VectorR4&, const VectorR4& ); // Sets by columns! + Matrix4x4( double, double, double, double, + double, double, double, double, + double, double, double, double, + double, double, double, double ); // Sets by columns + + inline void SetIdentity (); // Set to the identity map + inline void SetZero (); // Set to the zero map + inline void Set ( const Matrix4x4& ); // Set to the matrix. + inline void Set( const VectorR4&, const VectorR4&, + const VectorR4&, const VectorR4& ); + inline void Set( double, double, double, double, + double, double, double, double, + double, double, double, double, + double, double, double, double ); + inline void SetByRows( const VectorR4&, const VectorR4&, + const VectorR4&, const VectorR4& ); + inline void SetByRows( double, double, double, double, + double, double, double, double, + double, double, double, double, + double, double, double, double ); + inline void SetColumn1 ( double, double, double, double ); + inline void SetColumn2 ( double, double, double, double ); + inline void SetColumn3 ( double, double, double, double ); + inline void SetColumn4 ( double, double, double, double ); + inline void SetColumn1 ( const VectorR4& ); + inline void SetColumn2 ( const VectorR4& ); + inline void SetColumn3 ( const VectorR4& ); + inline void SetColumn4 ( const VectorR4& ); + inline VectorR4 Column1() const; + inline VectorR4 Column2() const; + inline VectorR4 Column3() const; + inline VectorR4 Column4() const; + + inline void SetDiagonal( double, double, double, double ); + inline void SetDiagonal( const VectorR4& ); + inline double Diagonal( int ); + + inline void MakeTranspose(); // Transposes it. + void Matrix4x4::operator*= (const Matrix4x4& B); // Matrix product + + Matrix4x4& ReNormalize(); +}; + +inline VectorR4 operator* ( const Matrix4x4&, const VectorR4& ); + +ostream& operator<< ( ostream& os, const Matrix4x4& A ); + + +// ***************************************** +// LinearMapR4 class * +// * * * * * * * * * * * * * * * * * * * * * + +class LinearMapR4 : public Matrix4x4 { + +public: + + LinearMapR4(); + LinearMapR4( const VectorR4&, const VectorR4&, + const VectorR4&, const VectorR4& ); // Sets by columns! + LinearMapR4( double, double, double, double, + double, double, double, double, + double, double, double, double, + double, double, double, double ); // Sets by columns + LinearMapR4 ( const Matrix4x4& ); + + inline LinearMapR4& operator+= (const LinearMapR4& ); + inline LinearMapR4& operator-= (const LinearMapR4& ); + inline LinearMapR4& operator*= (double); + inline LinearMapR4& operator/= (double); + inline LinearMapR4& operator*= (const Matrix4x4& ); // Matrix product + + inline LinearMapR4 Transpose() const; + double Determinant () const; // Returns the determinant + LinearMapR4 Inverse() const; // Returns inverse + LinearMapR4& Invert(); // Converts into inverse. + VectorR4 Solve(const VectorR4&) const; // Returns solution + LinearMapR4 PseudoInverse() const; // Returns pseudo-inverse TO DO + VectorR4 PseudoSolve(const VectorR4&); // Finds least squares solution TO DO +}; + +inline LinearMapR4 operator+ (const LinearMapR4&, const LinearMapR4&); +inline LinearMapR4 operator- (const LinearMapR4&); +inline LinearMapR4 operator- (const LinearMapR4&, const LinearMapR4&); +inline LinearMapR4 operator* ( const LinearMapR4&, double); +inline LinearMapR4 operator* ( double, const LinearMapR4& ); +inline LinearMapR4 operator/ ( const LinearMapR4&, double ); +inline LinearMapR4 operator* ( const Matrix4x4&, const LinearMapR4& ); +inline LinearMapR4 operator* ( const LinearMapR4&, const Matrix4x4& ); + // Matrix product (composition) + + +// ******************************************* +// RotationMapR4 class * +// * * * * * * * * * * * * * * * * * * * * * * + +class RotationMapR4 : public Matrix4x4 { + +public: + + RotationMapR4(); + RotationMapR4( const VectorR4&, const VectorR4&, + const VectorR4&, const VectorR4& ); // Sets by columns! + RotationMapR4( double, double, double, double, + double, double, double, double, + double, double, double, double, + double, double, double, double ); // Sets by columns! + + RotationMapR4& SetZero(); // IT IS AN ERROR TO USE THIS FUNCTION! + + inline RotationMapR4& operator*= (const RotationMapR4& ); // Matrix product + + inline RotationMapR4 Transpose() const; + inline RotationMapR4 Inverse() const { return Transpose(); }; // Returns the transpose + inline RotationMapR4& Invert() { MakeTranspose(); return *this; }; // Transposes it. + inline VectorR4 Invert(const VectorR4&) const; // Returns solution +}; + +inline RotationMapR4 operator* ( const RotationMapR4&, const RotationMapR4& ); + // Matrix product (composition) + + +// *************************************************************** +// * 4-space vector and matrix utilities (prototypes) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +// Returns the angle between vectors u and v. +// Use SolidAngleUnit if both vectors are unit vectors +inline double SolidAngle( const VectorR4& u, const VectorR4& v); +inline double SolidAngleUnit( const VectorR4 u, const VectorR4 v ); + +// Returns a righthanded orthonormal basis to complement vectors u,v,w. +// The vectors u,v,w must be unit and orthonormal. +void GetOrtho( const VectorR4& u, RotationMapR4& rotmap ); +void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmap ); +void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& w, + RotationMapR4& rotmap ); +void GetOrtho( int j, RotationMapR4& rotmap); // Mainly for internal use + +// Projections + +inline VectorR4 ProjectToUnit ( const VectorR4& u, const VectorR4& v); + // Project u onto v +inline VectorR4 ProjectPerpUnit ( const VectorR4& u, const VectorR4 & v); + // Project perp to v +inline VectorR4 ProjectPerpUnitDiff ( const VectorR4& u, const VectorR4& v); +// v must be a unit vector. + +// Returns the projection of u onto unit v +inline VectorR4 ProjectToUnit ( const VectorR4& u, const VectorR4& v) +{ + return (u^v)*v; +} + +// Returns the projection of u onto the plane perpindicular to the unit vector v +inline VectorR4 ProjectPerpUnit ( const VectorR4& u, const VectorR4& v) +{ + return ( u - ((u^v)*v) ); +} + +// Returns the projection of u onto the plane perpindicular to the unit vector v +// This one is more stable when u and v are nearly equal. +inline VectorR4 ProjectPerpUnitDiff ( const VectorR4& u, const VectorR4& v) +{ + VectorR4 ans = u; + ans -= v; + ans -= ((ans^v)*v); + return ans; // ans = (u-v) - ((u-v)^v)*v +} + + +// Projection maps (LinearMapR4's) + +// VectorProjectMap returns map projecting onto a given vector u. +// u should be a unit vector (otherwise the returned map is +// scaled according to the magnitude of u. +inline void VectorProjectMap( const VectorR4& u, LinearMapR4& M ) +{ + double a = u.x*u.y; + double b = u.x*u.z; + double c = u.x*u.w; + double d = u.y*u.z; + double e = u.y*u.w; + double f = u.z*u.w; + M.Set( u.x*u.x, a, b, c, + a, u.y*u.y, d, e, + b, d, u.z*u.z, f, + c, e, f, u.w*u.w ); +} + +inline LinearMapR4 VectorProjectMap( const VectorR4& u ) +{ + LinearMapR4 result; + VectorProjectMap( u, result ); + return result; +} + +inline LinearMapR4 PerpProjectMap ( const VectorR4& u ); +// u - must be unit vector. + +LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v); // u * v^T. +inline void TimesTranspose( const VectorR4& u, const VectorR4& v, LinearMapR4& M); + +// Rotation Maps + +RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec); +// fromVec and toVec should be unit vectors + + + +// *************************************************************** +// * Stream Output Routines (Prototypes) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +ostream& operator<< ( ostream& os, const VectorR4& u ); + + +// ***************************************************** +// * VectorR4 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * + +inline VectorR4& VectorR4::Load( const double* v ) +{ + x = *v; + y = *(v+1); + z = *(v+2); + w = *(v+3); + return *this; +} + +inline VectorR4& VectorR4::Load( const float* v ) +{ + x = *v; + y = *(v+1); + z = *(v+2); + w = *(v+3); + return *this; +} + +inline void VectorR4::Dump( double* v ) const +{ + *v = x; + *(v+1) = y; + *(v+2) = z; + *(v+3) = w; +} + +inline void VectorR4::Dump( float* v ) const +{ + *v = (float)x; + *(v+1) = (float)y; + *(v+2) = (float)z; + *(v+3) = (float)w; +} + +inline VectorR4& VectorR4::MakeUnit () // Convert to unit vector (or leave zero). +{ + double nSq = NormSq(); + if (nSq != 0.0) { + *this /= sqrt(nSq); + } + return *this; +} + +inline VectorR4 operator+( const VectorR4& u, const VectorR4& v ) +{ + return VectorR4(u.x+v.x, u.y+v.y, u.z+v.z, u.w+v.w ); +} +inline VectorR4 operator-( const VectorR4& u, const VectorR4& v ) +{ + return VectorR4(u.x-v.x, u.y-v.y, u.z-v.z, u.w-v.w); +} +inline VectorR4 operator*( const VectorR4& u, register double m) +{ + return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m ); +} +inline VectorR4 operator*( register double m, const VectorR4& u) +{ + return VectorR4( u.x*m, u.y*m, u.z*m, u.w*m ); +} +inline VectorR4 operator/( const VectorR4& u, double m) +{ + register double mInv = 1.0/m; + return VectorR4( u.x*mInv, u.y*mInv, u.z*mInv, u.w*mInv ); +} + +inline int operator==( const VectorR4& u, const VectorR4& v ) +{ + return ( u.x==v.x && u.y==v.y && u.z==v.z && u.w==v.w ); +} + +inline double operator^ ( const VectorR4& u, const VectorR4& v ) // Dot Product +{ + return ( u.x*v.x + u.y*v.y + u.z*v.z + u.w*v.w ); +} + +inline VectorR4 ArrayProd ( const VectorR4& u, const VectorR4& v ) +{ + return ( VectorR4( u.x*v.x, u.y*v.y, u.z*v.z, u.w*v.w ) ); +} + +inline VectorR4& VectorR4::ArrayProd (const VectorR4& v) // Component-wise Product +{ + x *= v.x; + y *= v.y; + z *= v.z; + w *= v.w; + return ( *this ); +} + +inline VectorR4& VectorR4::ArrayProd3 (const VectorR3& v) // Component-wise Product +{ + x *= v.x; + y *= v.y; + z *= v.z; + return ( *this ); +} + +inline VectorR4& VectorR4::AddScaled( const VectorR4& u, double s ) +{ + x += s*u.x; + y += s*u.y; + z += s*u.z; + w += s*u.w; + return(*this); +} + +inline VectorR4& VectorR4::ReNormalize() // Convert near unit back to unit +{ + double nSq = NormSq(); + register double mFact = 1.0-0.5*(nSq-1.0); // Multiplicative factor + *this *= mFact; + return *this; +} + +inline double NormalizeError (const VectorR4& u) +{ + register double discrepancy; + discrepancy = u.x*u.x + u.y*u.y + u.z*u.z + u.w*u.w - 1.0; + if ( discrepancy < 0.0 ) { + discrepancy = -discrepancy; + } + return discrepancy; +} + +inline VectorR3& VectorR3::SetFromHg(const VectorR4& v) { + double wInv = 1.0/v.w; + x = v.x*wInv; + y = v.y*wInv; + z = v.z*wInv; + return *this; +} + +inline double VectorR4::Dist( const VectorR4& u ) const // Distance from u +{ + return sqrt( DistSq(u) ); +} + +inline double VectorR4::DistSq( const VectorR4& u ) const // Distance from u +{ + return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) + (z-u.z)*(z-u.z) + (w-u.w)*(w-u.w) ); +} + + +// ********************************************************* +// * Matrix4x4 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ***** + +inline Matrix4x4::Matrix4x4() {} + +inline Matrix4x4::Matrix4x4( const VectorR4& u, const VectorR4& v, + const VectorR4& s, const VectorR4& t) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m41 = u.w; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m42 = v.w; + m13 = s.x; // Column 3 + m23 = s.y; + m33 = s.z; + m43 = s.w; + m14 = t.x; // Column 4 + m24 = t.y; + m34 = t.z; + m44 = t.w; +} + +inline Matrix4x4::Matrix4x4( double a11, double a21, double a31, double a41, + double a12, double a22, double a32, double a42, + double a13, double a23, double a33, double a43, + double a14, double a24, double a34, double a44) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; + m41 = a41; // Row 4 + m42 = a42; + m43 = a43; + m44 = a44; +} + +/* +inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A) + : m11(A.m11), m12(A.m12), m13(A.m13), m14(A.m14), + m21(A.m21), m22(A.m22), m23(A.m23), m24(A.m24), + m31(A.m31), m32(A.m32), m33(A.m33), m34(A.m34), + m41(A.m41), m42(A.m42), m43(A.m43), m44(A.m44) {} */ + +inline void Matrix4x4::SetIdentity ( ) +{ + m11 = m22 = m33 = m44 = 1.0; + m12 = m13 = m14 = m21 = m23 = m24 = m13 = m23 = m41= m42 = m43 = 0.0; +} + +inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v, + const VectorR4& s, const VectorR4& t ) +{ + m11 = u.x; // Column 1 + m21 = u.y; + m31 = u.z; + m41 = u.w; + m12 = v.x; // Column 2 + m22 = v.y; + m32 = v.z; + m42 = v.w; + m13 = s.x; // Column 3 + m23 = s.y; + m33 = s.z; + m43 = s.w; + m14 = t.x; // Column 4 + m24 = t.y; + m34 = t.z; + m44 = t.w; +} + +inline void Matrix4x4::Set( double a11, double a21, double a31, double a41, + double a12, double a22, double a32, double a42, + double a13, double a23, double a33, double a43, + double a14, double a24, double a34, double a44) + // Values specified in column order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; + m41 = a41; // Row 4 + m42 = a42; + m43 = a43; + m44 = a44; +} + +inline void Matrix4x4::Set ( const Matrix4x4& M ) // Set to the matrix. +{ + m11 = M.m11; + m12 = M.m12; + m13 = M.m13; + m14 = M.m14; + m21 = M.m21; + m22 = M.m22; + m23 = M.m23; + m24 = M.m24; + m31 = M.m31; + m32 = M.m32; + m33 = M.m33; + m34 = M.m34; + m41 = M.m41; + m42 = M.m42; + m43 = M.m43; + m44 = M.m44; +} + +inline void Matrix4x4::SetZero( ) +{ + m11 = m12 = m13 = m14 = m21 = m22 = m23 = m24 + = m31 = m32 = m33 = m34 = m41 = m42 = m43 = m44 = 0.0; +} + +inline void Matrix4x4::SetByRows( const VectorR4& u, const VectorR4& v, + const VectorR4& s, const VectorR4& t ) +{ + m11 = u.x; // Row 1 + m12 = u.y; + m13 = u.z; + m14 = u.w; + m21 = v.x; // Row 2 + m22 = v.y; + m23 = v.z; + m24 = v.w; + m31 = s.x; // Row 3 + m32 = s.y; + m33 = s.z; + m34 = s.w; + m41 = t.x; // Row 4 + m42 = t.y; + m43 = t.z; + m44 = t.w; +} + +inline void Matrix4x4::SetByRows( double a11, double a12, double a13, double a14, + double a21, double a22, double a23, double a24, + double a31, double a32, double a33, double a34, + double a41, double a42, double a43, double a44 ) + // Values specified in row order!!! +{ + m11 = a11; // Row 1 + m12 = a12; + m13 = a13; + m14 = a14; + m21 = a21; // Row 2 + m22 = a22; + m23 = a23; + m24 = a24; + m31 = a31; // Row 3 + m32 = a32; + m33 = a33; + m34 = a34; + m41 = a41; // Row 4 + m42 = a42; + m43 = a43; + m44 = a44; +} + +inline void Matrix4x4::SetColumn1 ( double x, double y, double z, double w) +{ + m11 = x; m21 = y; m31= z; m41 = w; +} + +inline void Matrix4x4::SetColumn2 ( double x, double y, double z, double w) +{ + m12 = x; m22 = y; m32= z; m42 = w; +} + +inline void Matrix4x4::SetColumn3 ( double x, double y, double z, double w) +{ + m13 = x; m23 = y; m33= z; m43 = w; +} + +inline void Matrix4x4::SetColumn4 ( double x, double y, double z, double w) +{ + m14 = x; m24 = y; m34= z; m44 = w; +} + +inline void Matrix4x4::SetColumn1 ( const VectorR4& u ) +{ + m11 = u.x; m21 = u.y; m31 = u.z; m41 = u.w; +} + +inline void Matrix4x4::SetColumn2 ( const VectorR4& u ) +{ + m12 = u.x; m22 = u.y; m32 = u.z; m42 = u.w; +} + +inline void Matrix4x4::SetColumn3 ( const VectorR4& u ) +{ + m13 = u.x; m23 = u.y; m33 = u.z; m43 = u.w; +} + +inline void Matrix4x4::SetColumn4 ( const VectorR4& u ) +{ + m14 = u.x; m24 = u.y; m34 = u.z; m44 = u.w; +} + +VectorR4 Matrix4x4::Column1() const +{ + return ( VectorR4(m11, m21, m31, m41) ); +} + +VectorR4 Matrix4x4::Column2() const +{ + return ( VectorR4(m12, m22, m32, m42) ); +} + +VectorR4 Matrix4x4::Column3() const +{ + return ( VectorR4(m13, m23, m33, m43) ); +} + +VectorR4 Matrix4x4::Column4() const +{ + return ( VectorR4(m14, m24, m34, m44) ); +} + +inline void Matrix4x4::SetDiagonal( double x, double y, + double z, double w) +{ + m11 = x; + m22 = y; + m33 = z; + m44 = w; +} + +inline void Matrix4x4::SetDiagonal( const VectorR4& u ) +{ + SetDiagonal ( u.x, u.y, u.z, u.w ); +} + +inline double Matrix4x4::Diagonal( int i ) +{ + switch (i) { + case 0: + return m11; + case 1: + return m22; + case 2: + return m33; + case 3: + return m44; + default: + assert(0); + return 0.0; + } +} + +inline void Matrix4x4::MakeTranspose() // Transposes it. +{ + register double temp; + temp = m12; + m12 = m21; + m21=temp; + temp = m13; + m13 = m31; + m31 = temp; + temp = m14; + m14 = m41; + m41 = temp; + temp = m23; + m23 = m32; + m32 = temp; + temp = m24; + m24 = m42; + m42 = temp; + temp = m34; + m34 = m43; + m43 = temp; +} + +inline VectorR4 operator* ( const Matrix4x4& A, const VectorR4& u) +{ + VectorR4 ret; + ret.x = A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w; + ret.y = A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w; + ret.z = A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w; + ret.w = A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w; + return ret; +} + + +// ****************************************************** +// * LinearMapR4 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline LinearMapR4::LinearMapR4() +{ + SetZero(); + return; +} + +inline LinearMapR4::LinearMapR4( const VectorR4& u, const VectorR4& v, + const VectorR4& s, const VectorR4& t) +:Matrix4x4 ( u, v, s ,t ) +{ } + +inline LinearMapR4::LinearMapR4( + double a11, double a21, double a31, double a41, + double a12, double a22, double a32, double a42, + double a13, double a23, double a33, double a43, + double a14, double a24, double a34, double a44) + // Values specified in column order!!! +:Matrix4x4 ( a11, a21, a31, a41, a12, a22, a32, a42, + a13, a23, a33, a43, a14, a24, a34, a44 ) +{ } + +inline LinearMapR4::LinearMapR4 ( const Matrix4x4& A ) +: Matrix4x4 (A) +{} + + +inline LinearMapR4& LinearMapR4::operator+= (const LinearMapR4& B) +{ + m11 += B.m11; + m12 += B.m12; + m13 += B.m13; + m14 += B.m14; + m21 += B.m21; + m22 += B.m22; + m23 += B.m23; + m24 += B.m24; + m31 += B.m31; + m32 += B.m32; + m33 += B.m33; + m34 += B.m34; + m41 += B.m41; + m42 += B.m42; + m43 += B.m43; + m44 += B.m44; + return ( *this ); +} + +inline LinearMapR4& LinearMapR4::operator-= (const LinearMapR4& B) +{ + m11 -= B.m11; + m12 -= B.m12; + m13 -= B.m13; + m14 -= B.m14; + m21 -= B.m21; + m22 -= B.m22; + m23 -= B.m23; + m24 -= B.m24; + m31 -= B.m31; + m32 -= B.m32; + m33 -= B.m33; + m34 -= B.m34; + m41 -= B.m41; + m42 -= B.m42; + m43 -= B.m43; + m44 -= B.m44; + return( *this ); +} + +inline LinearMapR4 operator+ (const LinearMapR4& A, const LinearMapR4& B) +{ + return( LinearMapR4( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31, A.m41+B.m41, + A.m12+B.m12, A.m22+B.m22, A.m32+B.m32, A.m42+B.m42, + A.m13+B.m13, A.m23+B.m23, A.m33+B.m33, A.m43+B.m43, + A.m14+B.m14, A.m24+B.m24, A.m34+B.m34, A.m44+B.m44) ); +} + +inline LinearMapR4 operator- (const LinearMapR4& A) +{ + return( LinearMapR4( -A.m11, -A.m21, -A.m31, -A.m41, + -A.m12, -A.m22, -A.m32, -A.m42, + -A.m13, -A.m23, -A.m33, -A.m43, + -A.m14, -A.m24, -A.m34, -A.m44 ) ); +} + +inline LinearMapR4 operator- (const LinearMapR4& A, const LinearMapR4& B) +{ + return( LinearMapR4( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31, A.m41-B.m41, + A.m12-B.m12, A.m22-B.m22, A.m32-B.m32, A.m42-B.m42, + A.m13-B.m13, A.m23-B.m23, A.m33-B.m33, A.m43-B.m43, + A.m14-B.m14, A.m24-B.m24, A.m34-B.m34, A.m44-B.m44 ) ); +} + +inline LinearMapR4& LinearMapR4::operator*= (register double b) +{ + m11 *= b; + m12 *= b; + m13 *= b; + m14 *= b; + m21 *= b; + m22 *= b; + m23 *= b; + m24 *= b; + m31 *= b; + m32 *= b; + m33 *= b; + m34 *= b; + m41 *= b; + m42 *= b; + m43 *= b; + m44 *= b; + return ( *this); +} + +inline LinearMapR4 operator* ( const LinearMapR4& A, register double b) +{ + return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b, + A.m12*b, A.m22*b, A.m32*b, A.m42*b, + A.m13*b, A.m23*b, A.m33*b, A.m43*b, + A.m14*b, A.m24*b, A.m34*b, A.m44*b) ); +} + +inline LinearMapR4 operator* ( register double b, const LinearMapR4& A) +{ + return( LinearMapR4( A.m11*b, A.m21*b, A.m31*b, A.m41*b, + A.m12*b, A.m22*b, A.m32*b, A.m42*b, + A.m13*b, A.m23*b, A.m33*b, A.m43*b, + A.m14*b, A.m24*b, A.m34*b, A.m44*b ) ); +} + +inline LinearMapR4 operator/ ( const LinearMapR4& A, double b) +{ + register double bInv = 1.0/b; + return ( A*bInv ); +} + +inline LinearMapR4& LinearMapR4::operator/= (register double b) +{ + register double bInv = 1.0/b; + return ( *this *= bInv ); +} + +inline VectorR4 operator* ( const LinearMapR4& A, const VectorR4& u) +{ + return(VectorR4 ( A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14*u.w, + A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24*u.w, + A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34*u.w, + A.m41*u.x + A.m42*u.y + A.m43*u.z + A.m44*u.w ) ); +} + +inline LinearMapR4 LinearMapR4::Transpose() const // Returns the transpose +{ + return (LinearMapR4( m11, m12, m13, m14, + m21, m22, m23, m24, + m31, m32, m33, m34, + m41, m42, m43, m44 ) ); +} + +inline LinearMapR4& LinearMapR4::operator*= (const Matrix4x4& B) // Matrix product +{ + (*this).Matrix4x4::operator*=(B); + + return( *this ); +} + +inline LinearMapR4 operator* ( const LinearMapR4& A, const Matrix4x4& B) +{ + LinearMapR4 AA(A); + AA.Matrix4x4::operator*=(B); + return AA; +} + +inline LinearMapR4 operator* ( const Matrix4x4& A, const LinearMapR4& B) +{ + LinearMapR4 AA(A); + AA.Matrix4x4::operator*=(B); + return AA; +} + + + +// ****************************************************** +// * RotationMapR4 class - inlined functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * ** + +inline RotationMapR4::RotationMapR4() +{ + SetIdentity(); + return; +} + +inline RotationMapR4::RotationMapR4( const VectorR4& u, const VectorR4& v, + const VectorR4& s, const VectorR4& t) +:Matrix4x4 ( u, v, s ,t ) +{ } + +inline RotationMapR4::RotationMapR4( + double a11, double a21, double a31, double a41, + double a12, double a22, double a32, double a42, + double a13, double a23, double a33, double a43, + double a14, double a24, double a34, double a44) + // Values specified in column order!!! +:Matrix4x4 ( a11, a21, a31, a41, a12, a22, a32, a42, + a13, a23, a33, a43, a14, a24, a34, a44 ) +{ } + +inline RotationMapR4 RotationMapR4::Transpose() const // Returns the transpose +{ + return ( RotationMapR4( m11, m12, m13, m14, + m21, m22, m23, m24, + m31, m32, m33, m34, + m41, m42, m43, m44 ) ); +} + +inline VectorR4 RotationMapR4::Invert(const VectorR4& u) const // Returns solution +{ + return (VectorR4( m11*u.x + m21*u.y + m31*u.z + m41*u.w, + m12*u.x + m22*u.y + m32*u.z + m42*u.w, + m13*u.x + m23*u.y + m33*u.z + m43*u.w, + m14*u.x + m24*u.y + m34*u.z + m44*u.w ) ); +} + +inline RotationMapR4& RotationMapR4::operator*= (const RotationMapR4& B) // Matrix product +{ + (*this).Matrix4x4::operator*=(B); + + return( *this ); +} + +inline RotationMapR4 operator* ( const RotationMapR4& A, const RotationMapR4& B) +{ + RotationMapR4 AA(A); + AA.Matrix4x4::operator*=(B); + return AA; +} + + +// *************************************************************** +// * 4-space vector and matrix utilities (inlined functions) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +inline void TimesTranspose( const VectorR4& u, const VectorR4& v, LinearMapR4& M) +{ + M.Set ( v.x*u.x, v.x*u.y, v.x*u.z, v.x*u.w, // Set by columns! + v.y*u.x, v.y*u.y, v.y*u.z, v.y*u.w, + v.z*u.x, v.z*u.y, v.z*u.z, v.z*u.w, + v.w*u.x, v.w*u.y, v.w*u.z, v.w*u.w ) ; +} + +// Returns the solid angle between vectors u and v (not necessarily unit vectors) +inline double SolidAngle( const VectorR4& u, const VectorR4& v) +{ + double nSqU = u.NormSq(); + double nSqV = v.NormSq(); + if ( nSqU==0.0 && nSqV==0.0 ) { + return (0.0); + } + else { + return ( SolidAngleUnit( u/sqrt(nSqU), v/sqrt(nSqV) ) ); + } +} + +inline double SolidAngleUnit( const VectorR4 u, const VectorR4 v ) +{ + return ( atan2 ( ProjectPerpUnit(v,u).Norm(), u^v ) ); +} + + +#endif // LINEAR_R4_H diff --git a/examples/ThirdPartyLibs/BussIK/MathMisc.h b/examples/ThirdPartyLibs/BussIK/MathMisc.h new file mode 100644 index 000000000..1738753f8 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/MathMisc.h @@ -0,0 +1,384 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +#ifndef MATH_MISC_H +#define MATH_MISC_H + +#include + +// +// Commonly used constants +// + +const double PI = 3.1415926535897932384626433832795028841972; +const double PI2 = 2.0*PI; +const double PI4 = 4.0*PI; +const double PISq = PI*PI; +const double PIhalves = 0.5*PI; +const double PIthirds = PI/3.0; +const double PItwothirds = PI2/3.0; +const double PIfourths = 0.25*PI; +const double PIsixths = PI/6.0; +const double PIsixthsSq = PIsixths*PIsixths; +const double PItwelfths = PI/12.0; +const double PItwelfthsSq = PItwelfths*PItwelfths; +const double PIinv = 1.0/PI; +const double PI2inv = 0.5/PI; +const double PIhalfinv = 2.0/PI; + +const double RadiansToDegrees = 180.0/PI; +const double DegreesToRadians = PI/180; + +const double OneThird = 1.0/3.0; +const double TwoThirds = 2.0/3.0; +const double OneSixth = 1.0/6.0; +const double OneEighth = 1.0/8.0; +const double OneTwelfth = 1.0/12.0; + +const double Root2 = sqrt(2.0); +const double Root3 = sqrt(3.0); +const double Root2Inv = 1.0/Root2; // sqrt(2)/2 +const double HalfRoot3 = sqrtf(3)/2.0; + +const double LnTwo = log(2.0); +const double LnTwoInv = 1.0/log(2.0); + +// Special purpose constants +const double OnePlusEpsilon15 = 1.0+1.0e-15; +const double OneMinusEpsilon15 = 1.0-1.0e-15; + +inline double ZeroValue(const double& x) +{ + return 0.0; +} + +// +// Comparisons +// + +template inline T Min ( T x, T y ) +{ + return (x inline T Max ( T x, T y ) +{ + return (y inline T ClampRange ( T x, T min, T max) +{ + if ( xmax ) { + return max; + } + return x; +} + +template inline bool ClampRange ( T *x, T min, T max) +{ + if ( (*x)max ) { + (*x) = max; + return false; + } + else { + return true; + } +} + +template inline bool ClampMin ( T *x, T min) +{ + if ( (*x) inline bool ClampMax ( T *x, T max) +{ + if ( (*x)>max ) { + (*x) = max; + return false; + } + return true; +} + +template inline T& UpdateMin ( const T& x, T& y ) +{ + if ( x inline T& UpdateMax ( const T& x, T& y ) +{ + if ( x>y ) { + y = x; + } + return y; +} + + +template inline bool SameSignNonzero( T x, T y ) +{ + if ( x<0 ) { + return (y<0); + } + else if ( 0 +inline bool NearEqual( T a, T b, double tolerance ) { + a -= b; + return ( Mag(a)<=tolerance ); +} + +inline bool EqualZeroFuzzy( double x ) { + return ( fabs(x)<=1.0e-14 ); +} + +inline bool NearZero( double x, double tolerance ) { + return ( fabs(x)<=tolerance ); +} + +inline bool LessOrEqualFuzzy( double x, double y ) +{ + if ( x <= y ) { + return true; + } + + if ( y > 0.0 ) { + if ( x>0.0 ) { + return ( x*OneMinusEpsilon15 < y*OnePlusEpsilon15 ); + } + else { + return ( y<1.0e-15 ); // x==0 in this case + } + } + else if ( y < 0.0 ) { + if ( x<0.0 ) { + return ( x*OnePlusEpsilon15 < y*OneMinusEpsilon15 ); + } + else { + return ( y>-1.0e-15 ); // x==0 in this case + } + } + else { + return ( -1.0e-15 *maxabs ) { + *maxabs = updateval; + return true; + } + else if ( -updateval > *maxabs ) { + *maxabs = -updateval; + return true; + } + else { + return false; + } +} + +// ********************************************************** +// Combinations and averages. * +// ********************************************************** + +template +void averageOf ( const T& a, const T &b, T&c ) { + c = a; + c += b; + c *= 0.5; +} + +template +void Lerp( const T& a, const T&b, double alpha, T&c ) { + double beta = 1.0-alpha; + if ( beta>alpha ) { + c = b; + c *= alpha/beta; + c += a; + c *= beta; + } + else { + c = a; + c *= beta/alpha; + c += b; + c *= alpha; + } +} + +template +T Lerp( const T& a, const T&b, double alpha ) { + T ret; + Lerp( a, b, alpha, ret ); + return ret; +} + +// ********************************************************** +// Trigonometry * +// ********************************************************** + +// TimesCot(x) returns x*cot(x) +inline double TimesCot ( double x ) { + if ( -1.0e-5 < x && x < 1.0e-5 ) { + return 1.0+x*OneThird; + } + else { + return ( x*cos(x)/sin(x) ); + } +} + +// SineOver(x) returns sin(x)/x. +inline double SineOver( double x ) { + if ( -1.0e-5 < x && x < 1.0e-5 ) { + return 1.0-x*x*OneSixth; + } + else { + return sin(x)/x; + } +} +// OverSine(x) returns x/sin(x). +inline double OverSine( double x ) { + if ( -1.0e-5 < x && x < 1.0e-5 ) { + return 1.0+x*x*OneSixth; + } + else { + return x/sin(x); + } +} + +inline double SafeAsin( double x ) { + if ( x <= -1.0 ) { + return -PIhalves; + } + else if ( x >= 1.0 ) { + return PIhalves; + } + else { + return asin(x); + } +} + +inline double SafeAcos( double x ) { + if ( x <= -1.0 ) { + return PI; + } + else if ( x >= 1.0 ) { + return 0.0; + } + else { + return acos(x); + } +} + + +// ********************************************************************** +// Roots and powers * +// ********************************************************************** + +// Square(x) returns x*x, of course! + +template inline T Square ( T x ) +{ + return (x*x); +} + +// Cube(x) returns x*x*x, of course! + +template inline T Cube ( T x ) +{ + return (x*x*x); +} + +// SafeSqrt(x) = returns sqrt(max(x, 0.0)); + +inline double SafeSqrt( double x ) { + if ( x<=0.0 ) { + return 0.0; + } + else { + return sqrt(x); + } +} + + +// SignedSqrt(a, s) returns (sign(s)*sqrt(a)). +inline double SignedSqrt( double a, double sgn ) +{ + if ( sgn==0.0 ) { + return 0.0; + } + else { + return ( sgn>0.0 ? sqrt(a) : -sqrt(a) ); + } +} + + +// Template version of Sign function + +template inline int Sign( T x) +{ + if ( x<0 ) { + return -1; + } + else if ( x==0 ) { + return 0; + } + else { + return 1; + } +} + + +#endif // #ifndef MATH_MISC_H diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp new file mode 100644 index 000000000..365d90f6c --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -0,0 +1,984 @@ + +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +// +// MatrixRmn.cpp: Matrix over reals (Variable dimensional vector) +// +// Not very sophisticated yet. Needs more functionality +// To do: better handling of resizing. +// + +#include "MatrixRmn.h" + +MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix + +// Fill the diagonal entries with the value d. The rest of the matrix is unchanged. +void MatrixRmn::SetDiagonalEntries( double d ) +{ + long diagLen = Min( NumRows, NumCols ); + double* dPtr = x; + for ( ; diagLen>0; diagLen-- ) { + *dPtr = d; + dPtr += NumRows+1; + } +} + +// Fill the diagonal entries with values in vector d. The rest of the matrix is unchanged. +void MatrixRmn::SetDiagonalEntries( const VectorRn& d ) +{ + long diagLen = Min( NumRows, NumCols ); + assert ( d.length == diagLen ); + double* dPtr = x; + double* from = d.x; + for ( ; diagLen>0; diagLen-- ) { + *dPtr = *(from++); + dPtr += NumRows+1; + } +} + +// Fill the superdiagonal entries with the value d. The rest of the matrix is unchanged. +void MatrixRmn::SetSuperDiagonalEntries( double d ) +{ + long sDiagLen = Min( NumRows, (long)(NumCols-1) ); + double* to = x + NumRows; + for ( ; sDiagLen>0; sDiagLen-- ) { + *to = d; + to += NumRows+1; + } +} + +// Fill the superdiagonal entries with values in vector d. The rest of the matrix is unchanged. +void MatrixRmn::SetSuperDiagonalEntries( const VectorRn& d ) +{ + long sDiagLen = Min( (long)(NumRows-1), NumCols ); + assert ( sDiagLen == d.length ); + double* to = x + NumRows; + double* from = d.x; + for ( ; sDiagLen>0; sDiagLen-- ) { + *to = *(from++); + to += NumRows+1; + } +} + +// Fill the subdiagonal entries with the value d. The rest of the matrix is unchanged. +void MatrixRmn::SetSubDiagonalEntries( double d ) +{ + long sDiagLen = Min( NumRows, NumCols ) - 1; + double* to = x + 1; + for ( ; sDiagLen>0; sDiagLen-- ) { + *to = d; + to += NumRows+1; + } +} + +// Fill the subdiagonal entries with values in vector d. The rest of the matrix is unchanged. +void MatrixRmn::SetSubDiagonalEntries( const VectorRn& d ) +{ + long sDiagLen = Min( NumRows, NumCols ) - 1; + assert ( sDiagLen == d.length ); + double* to = x + 1; + double* from = d.x; + for ( ; sDiagLen>0; sDiagLen-- ) { + *to = *(from++); + to += NumRows+1; + } +} + +// Set the i-th column equal to d. +void MatrixRmn::SetColumn(long i, const VectorRn& d ) +{ + assert ( NumRows==d.GetLength() ); + double* to = x+i*NumRows; + const double* from = d.x; + for ( i=NumRows; i>0; i-- ) { + *(to++) = *(from++); + } +} + +// Set the i-th column equal to d. +void MatrixRmn::SetRow(long i, const VectorRn& d ) +{ + assert ( NumCols==d.GetLength() ); + double* to = x+i; + const double* from = d.x; + for ( i=NumRows; i>0; i-- ) { + *to = *(from++); + to += NumRows; + } +} + +// Sets a "linear" portion of the array with the values from a vector d +// The first row and column position are given by startRow, startCol. +// Successive positions are found by using the deltaRow, deltaCol values +// to increment the row and column indices. There is no wrapping around. +void MatrixRmn::SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol ) +{ + long length = d.length; + assert( startRow>=0 && startRow=0 && startCol=0 && startRow+(length-1)*deltaRow=0 && startCol+(length-1)*deltaCol0; length-- ) { + *to = *(from++); + to += stride; + } +} + +// The matrix A is loaded, in into "this" matrix, based at (0,0). +// The size of "this" matrix must be large enough to accomodate A. +// The rest of "this" matrix is left unchanged. It is not filled with zeroes! + +void MatrixRmn::LoadAsSubmatrix( const MatrixRmn& A ) +{ + assert( A.NumRows<=NumRows && A.NumCols<=NumCols ); + int extraColStep = NumRows - A.NumRows; + double *to = x; + double *from = A.x; + for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, one per time thru loop + for ( long j=A.NumRows; j>0; j-- ) { // Copy all elements of this column of A + *(to++) = *(from++); + } + to += extraColStep; + } +} + +// The matrix A is loaded, in transposed order into "this" matrix, based at (0,0). +// The size of "this" matrix must be large enough to accomodate A. +// The rest of "this" matrix is left unchanged. It is not filled with zeroes! +void MatrixRmn::LoadAsSubmatrixTranspose( const MatrixRmn& A ) +{ + assert( A.NumRows<=NumCols && A.NumCols<=NumRows ); + double* rowPtr = x; + double* from = A.x; + for ( long i=A.NumCols; i>0; i-- ) { // Copy columns of A, once per loop + double* to = rowPtr; + for ( long j=A.NumRows; j>0; j-- ) { // Loop copying values from the column of A + *to = *(from++); + to += NumRows; + } + rowPtr ++; + } +} + +// Calculate the Frobenius Norm (square root of sum of squares of entries of the matrix) +double MatrixRmn::FrobeniusNorm() const +{ + return sqrt( FrobeniusNormSq() ); +} + +// Multiply this matrix by column vector v. +// Result is column vector "result" +void MatrixRmn::Multiply( const VectorRn& v, VectorRn& result ) const +{ + assert ( v.GetLength()==NumCols && result.GetLength()==NumRows ); + double* out = result.GetPtr(); // Points to entry in result vector + const double* rowPtr = x; // Points to beginning of next row in matrix + for ( long j = NumRows; j>0; j-- ) { + const double* in = v.GetPtr(); + const double* m = rowPtr++; + *out = 0.0f; + for ( long i = NumCols; i>0; i-- ) { + *out += (*(in++)) * (*m); + m += NumRows; + } + out++; + } +} + +// Multiply transpose of this matrix by column vector v. +// Result is column vector "result" +// Equivalent to mult by row vector on left +void MatrixRmn::MultiplyTranspose( const VectorRn& v, VectorRn& result ) const +{ + assert ( v.GetLength()==NumRows && result.GetLength()==NumCols ); + double* out = result.GetPtr(); // Points to entry in result vector + const double* colPtr = x; // Points to beginning of next column in matrix + for ( long i=NumCols; i>0; i-- ) { + const double* in=v.GetPtr(); + *out = 0.0f; + for ( long j = NumRows; j>0; j-- ) { + *out += (*(in++)) * (*(colPtr++)); + } + out++; + } +} + +// Form the dot product of a vector v with the i-th column of the array +double MatrixRmn::DotProductColumn( const VectorRn& v, long colNum ) const +{ + assert ( v.GetLength()==NumRows ); + double* ptrC = x+colNum*NumRows; + double* ptrV = v.x; + double ret = 0.0; + for ( long i = NumRows; i>0; i-- ) { + ret += (*(ptrC++))*(*(ptrV++)); + } + return ret; +} + +// Add a constant to each entry on the diagonal +MatrixRmn& MatrixRmn::AddToDiagonal( double d ) // Adds d to each diagonal entry +{ + long diagLen = Min( NumRows, NumCols ); + double* dPtr = x; + for ( ; diagLen>0; diagLen-- ) { + *dPtr += d; + dPtr += NumRows+1; + } + return *this; +} + +// Multiply two MatrixRmn's +MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ) +{ + assert( A.NumCols == B.NumRows && A.NumRows == dst.NumRows && B.NumCols == dst.NumCols ); + long length = A.NumCols; + + double *bPtr = B.x; // Points to beginning of column in B + double *dPtr = dst.x; + for ( long i = dst.NumCols; i>0; i-- ) { + double *aPtr = A.x; // Points to beginning of row in A + for ( long j = dst.NumRows; j>0; j-- ) { + *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, 1 ); + dPtr++; + aPtr++; + } + bPtr += B.NumRows; + } + + return dst; +} + +// Multiply two MatrixRmn's, Transpose the first matrix before multiplying +MatrixRmn& MatrixRmn::TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ) +{ + assert( A.NumRows == B.NumRows && A.NumCols == dst.NumRows && B.NumCols == dst.NumCols ); + long length = A.NumRows; + + double *bPtr = B.x; // bPtr Points to beginning of column in B + double *dPtr = dst.x; + for ( long i = dst.NumCols; i>0; i-- ) { // Loop over all columns of dst + double *aPtr = A.x; // aPtr Points to beginning of column in A + for ( long j = dst.NumRows; j>0; j-- ) { // Loop over all rows of dst + *dPtr = DotArray( length, aPtr, 1, bPtr, 1 ); + dPtr ++; + aPtr += A.NumRows; + } + bPtr += B.NumRows; + } + + return dst; +} + +// Multiply two MatrixRmn's. Transpose the second matrix before multiplying +MatrixRmn& MatrixRmn::MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ) +{ + assert( A.NumCols == B.NumCols && A.NumRows == dst.NumRows && B.NumRows == dst.NumCols ); + long length = A.NumCols; + + double *bPtr = B.x; // Points to beginning of row in B + double *dPtr = dst.x; + for ( long i = dst.NumCols; i>0; i-- ) { + double *aPtr = A.x; // Points to beginning of row in A + for ( long j = dst.NumRows; j>0; j-- ) { + *dPtr = DotArray( length, aPtr, A.NumRows, bPtr, B.NumRows ); + dPtr++; + aPtr++; + } + bPtr++; + } + + return dst; +} + +// Solves the equation (*this)*xVec = b; +// Uses row operations. Assumes *this is square and invertible. +// No error checking for divide by zero or instability (except with asserts) +void MatrixRmn::Solve( const VectorRn& b, VectorRn* xVec ) const +{ + assert ( NumRows==NumCols && NumCols==xVec->GetLength() && NumRows==b.GetLength() ); + + // Copy this matrix and b into an Augmented Matrix + MatrixRmn& AugMat = GetWorkMatrix( NumRows, NumCols+1 ); + AugMat.LoadAsSubmatrix( *this ); + AugMat.SetColumn( NumRows, b ); + + // Put into row echelon form with row operations + AugMat.ConvertToRefNoFree(); + + // Solve for x vector values using back substitution + double* xLast = xVec->x+NumRows-1; // Last entry in xVec + double* endRow = AugMat.x+NumRows*NumCols-1; // Last entry in the current row of the coefficient part of Augmented Matrix + double* bPtr = endRow+NumRows; // Last entry in augmented matrix (end of last column, in augmented part) + for ( long i = NumRows; i>0; i-- ) { + double accum = *(bPtr--); + // Next loop computes back substitution terms + double* rowPtr = endRow; // Points to entries of the current row for back substitution. + double* xPtr = xLast; // Points to entries in the x vector (also for back substitution) + for ( long j=NumRows-i; j>0; j-- ) { + accum -= (*rowPtr)*(*(xPtr--)); + rowPtr -= NumCols; // Previous entry in the row + } + assert( *rowPtr != 0.0 ); // Are not supposed to be any free variables in this matrix + *xPtr = accum/(*rowPtr); + endRow--; + } +} + +// ConvertToRefNoFree +// Converts the matrix (in place) to row echelon form +// For us, row echelon form allows any non-zero values, not just 1's, in the +// position for a lead variable. +// The "NoFree" version operates on the assumption that no free variable will be found. +// Algorithm uses row operations and row pivoting (only). +// Augmented matrix is correctly accomodated. Only the first square part participates +// in the main work of row operations. +void MatrixRmn::ConvertToRefNoFree() +{ + // Loop over all columns (variables) + // Find row with most non-zero entry. + // Swap to the highest active row + // Subtract appropriately from all the lower rows (row op of type 3) + long numIters = Min(NumRows,NumCols); + double* rowPtr1 = x; + const long diagStep = NumRows+1; + long lenRowLeft = NumCols; + for ( ; numIters>1; numIters-- ) { + // Find row with most non-zero entry. + double* rowPtr2 = rowPtr1; + double maxAbs = fabs(*rowPtr1); + double *rowPivot = rowPtr1; + long i; + for ( i=numIters-1; i>0; i-- ) { + const double& newMax = *(++rowPivot); + if ( newMax > maxAbs ) { + maxAbs = *rowPivot; + rowPtr2 = rowPivot; + } + else if ( -newMax > maxAbs ) { + maxAbs = -newMax; + rowPtr2 = rowPivot; + } + } + // Pivot step: Swap the row with highest entry to the current row + if ( rowPtr1 != rowPtr2 ) { + double *to = rowPtr1; + for ( long i=lenRowLeft; i>0; i-- ) { + double temp = *to; + *to = *rowPtr2; + *rowPtr2 = temp; + to += NumRows; + rowPtr2 += NumRows; + } + } + // Subtract this row appropriately from all the lower rows (row operation of type 3) + rowPtr2 = rowPtr1; + for ( i=numIters-1; i>0; i-- ) { + rowPtr2++; + double* to = rowPtr2; + double* from = rowPtr1; + assert( *from != 0.0 ); + double alpha = (*to)/(*from); + *to = 0.0; + for ( long j=lenRowLeft-1; j>0; j-- ) { + to += NumRows; + from += NumRows; + *to -= (*from)*alpha; + } + } + // Update for next iteration of loop + rowPtr1 += diagStep; + lenRowLeft--; + } + +} + +// Calculate the c=cosine and s=sine values for a Givens transformation. +// The matrix M = ( (c, -s), (s, c) ) in row order transforms the +// column vector (a, b)^T to have y-coordinate zero. +void MatrixRmn::CalcGivensValues( double a, double b, double *c, double *s ) +{ + double denomInv = sqrt(a*a + b*b); + if ( denomInv==0.0 ) { + *c = 1.0; + *s = 0.0; + } + else { + denomInv = 1.0/denomInv; + *c = a*denomInv; + *s = -b*denomInv; + } +} + +// Applies Givens transform to columns i and i+1. +// Equivalent to postmultiplying by the matrix +// ( c -s ) +// ( s c ) +// with non-zero entries in rows i and i+1 and columns i and i+1 +void MatrixRmn::PostApplyGivens( double c, double s, long idx ) +{ + assert ( 0<=idx && idx0; i-- ) { + double temp = *colA; + *colA = (*colA)*c + (*colB)*s; + *colB = (*colB)*c - temp*s; + colA++; + colB++; + } +} + +// Applies Givens transform to columns idx1 and idx2. +// Equivalent to postmultiplying by the matrix +// ( c -s ) +// ( s c ) +// with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2 +void MatrixRmn::PostApplyGivens( double c, double s, long idx1, long idx2 ) +{ + assert ( idx1!=idx2 && 0<=idx1 && idx10; i-- ) { + double temp = *colA; + *colA = (*colA)*c + (*colB)*s; + *colB = (*colB)*c - temp*s; + colA++; + colB++; + } +} + + +// ******************************************************************************************** +// Singular value decomposition. +// Return othogonal matrices U and V and diagonal matrix with diagonal w such that +// (this) = U * Diag(w) * V^T (V^T is V-transpose.) +// Diagonal entries have all non-zero entries before all zero entries, but are not +// necessarily sorted. (Someday, I will write ComputedSortedSVD that handles +// sorting the eigenvalues by magnitude.) +// ******************************************************************************************** +void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const +{ + assert ( U.NumRows==NumRows && V.NumCols==NumCols + && U.NumRows==U.NumCols && V.NumRows==V.NumCols + && w.GetLength()==Min(NumRows,NumCols) ); + + double temp=0.0; + VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 ); // Some extra work space. Will get passed around. + + // Choose larger of U, V to hold intermediate results + // If U is larger than V, use U to store intermediate results + // Otherwise use V. In the latter case, we form the SVD of A transpose, + // (which is essentially identical to the SVD of A). + MatrixRmn* leftMatrix; + MatrixRmn* rightMatrix; + if ( NumRows >= NumCols ) { + U.LoadAsSubmatrix( *this ); // Copy A into U + leftMatrix = &U; + rightMatrix = &V; + } + else { + V.LoadAsSubmatrixTranspose( *this ); // Copy A-transpose into V + leftMatrix = &V; + rightMatrix = &U; + } + + // Do the actual work to calculate the SVD + // Now matrix has at least as many rows as columns + CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag ); + ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag ); + +} + +// ************************************************ CalcBidiagonal ************************** +// Helper routine for SVD computation +// U is a matrix to be bidiagonalized. +// On return, U and V are orthonormal and w holds the new diagonal +// elements and superDiag holds the super diagonal elements. + +void MatrixRmn::CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) +{ + assert ( U.NumRows>=V.NumRows ); + + // The diagonal and superdiagonal entries of the bidiagonalized + // version of the U matrix + // are stored in the vectors w and superDiag (temporarily). + + // Apply Householder transformations to U. + // Householder transformations come in pairs. + // First, on the left, we map a portion of a column to zeros + // Second, on the right, we map a portion of a row to zeros + const long rowStep = U.NumCols; + const long diagStep = U.NumCols+1; + double *diagPtr = U.x; + double* wPtr = w.x; + double* superDiagPtr = superDiag.x; + long colLengthLeft = U.NumRows; + long rowLengthLeft = V.NumCols; + while (true) { + // Apply a Householder xform on left to zero part of a column + SvdHouseholder( diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr ); + + if ( rowLengthLeft==2 ) { + *superDiagPtr = *(diagPtr+rowStep); + break; + } + // Apply a Householder xform on the right to zero part of a row + SvdHouseholder( diagPtr+rowStep, rowLengthLeft-1, colLengthLeft, rowStep, 1, superDiagPtr ); + + rowLengthLeft--; + colLengthLeft--; + diagPtr += diagStep; + wPtr++; + superDiagPtr++; + } + + int extra = 0; + diagPtr += diagStep; + wPtr++; + if ( colLengthLeft > 2 ) { + extra = 1; + // Do one last Householder transformation when the matrix is not square + colLengthLeft--; + SvdHouseholder( diagPtr, colLengthLeft, 1, 1, 0, wPtr ); + } + else { + *wPtr = *diagPtr; + } + + // Form U and V from the Householder transformations + V.ExpandHouseholders( V.NumCols-2, 1, U.x+U.NumRows, U.NumRows, 1 ); + U.ExpandHouseholders( V.NumCols-1+extra, 0, U.x, 1, U.NumRows ); + + // Done with bidiagonalization + return; +} + +// Helper routine for CalcBidiagonal +// Performs a series of Householder transformations on a matrix +// Stores results compactly into the matrix: The Householder vector u (normalized) +// is stored into the first row/column being transformed. +// The leading term of that row (= plus/minus its magnitude is returned +// separately into "retFirstEntry" +void MatrixRmn::SvdHouseholder( double* basePt, + long colLength, long numCols, long colStride, long rowStride, + double* retFirstEntry ) +{ + + // Calc norm of vector u + double* cPtr = basePt; + double norm = 0.0; + long i; + for ( i=colLength; i>0 ; i-- ) { + norm += Square( *cPtr ); + cPtr += colStride; + } + norm = sqrt(norm); // Norm of vector to reflect to axis e_1 + + // Handle sign issues + double imageVal; // Choose sign to maximize distance + if ( (*basePt) < 0.0 ) { + imageVal = norm; + norm = 2.0*norm*(norm-(*basePt)); + } + else { + imageVal = -norm; + norm = 2.0*norm*(norm+(*basePt)); + } + norm = sqrt(norm); // Norm is norm of reflection vector + + if ( norm==0.0 ) { // If the vector being transformed is equal to zero + // Force to zero in case of roundoff errors + cPtr = basePt; + for ( i=colLength; i>0; i-- ) { + *cPtr = 0.0; + cPtr += colStride; + } + *retFirstEntry = 0.0; + return; + } + + *retFirstEntry = imageVal; + + // Set up the normalized Householder vector + *basePt -= imageVal; // First component changes. Rest stay the same. + // Normalize the vector + norm = 1.0/norm; // Now it is the inverse norm + cPtr = basePt; + for ( i=colLength; i>0 ; i-- ) { + *cPtr *= norm; + cPtr += colStride; + } + + // Transform the rest of the U matrix with the Householder transformation + double *rPtr = basePt; + for ( long j=numCols-1; j>0; j-- ) { + rPtr += rowStride; + // Calc dot product with Householder transformation vector + double dotP = DotArray( colLength, basePt, colStride, rPtr, colStride ); + // Transform with I - 2*dotP*(Householder vector) + AddArrayScale( colLength, basePt, colStride, rPtr, colStride, -2.0*dotP ); + } +} + +// ********************************* ExpandHouseholders ******************************************** +// The matrix will be square. +// numXforms = number of Householder transformations to concatenate +// Each Householder transformation is represented by a unit vector +// Each successive Householder transformation starts one position later +// and has one more implied leading zero +// basePt = beginning of the first Householder transform +// colStride, rowStride: Householder xforms are stored in "columns" +// numZerosSkipped is the number of implicit zeros on the front each +// Householder transformation vector (only values supported are 0 and 1). +void MatrixRmn::ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride ) +{ + // Number of applications of the last Householder transform + // (That are not trivial!) + long numToTransform = NumCols-numXforms+1-numZerosSkipped; + assert( numToTransform>0 ); + + if ( numXforms==0 ) { + SetIdentity(); + return; + } + + // Handle the first one separately as a special case, + // "this" matrix will be treated to simulate being preloaded with the identity + long hDiagStride = rowStride+colStride; + const double* hBase = basePt + hDiagStride*(numXforms-1); // Pointer to the last Householder vector + const double* hDiagPtr = hBase + colStride*(numToTransform-1); // Pointer to last entry in that vector + long i; + double* diagPtr = x+NumCols*NumRows-1; // Last entry in matrix (points to diagonal entry) + double* colPtr = diagPtr-(numToTransform-1); // Pointer to column in matrix + for ( i=numToTransform; i>0; i-- ) { + CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hDiagPtr) ); + *diagPtr += 1.0; // Add back in 1 to the diagonal entry (since xforming the identity) + diagPtr -= (NumRows+1); // Next diagonal entry in this matrix + colPtr -= NumRows; // Next column in this matrix + hDiagPtr -= colStride; + } + + // Now handle the general case + // A row of zeros must be in effect added to the top of each old column (in each loop) + double* colLastPtr = x + NumRows*NumCols - numToTransform - 1; + for ( i = numXforms-1; i>0; i-- ) { + numToTransform++; // Number of non-trivial applications of this Householder transformation + hBase -= hDiagStride; // Pointer to the beginning of the Householder transformation + colPtr = colLastPtr; + for ( long j = numToTransform-1; j>0; j-- ) { + // Get dot product + double dotProd2N = -2.0*DotArray( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1 ); + *colPtr = dotProd2N*(*hBase); // Adding onto zero at initial point + AddArrayScale( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1, dotProd2N ); + colPtr -= NumRows; + } + // Do last one as a special case (may overwrite the Householder vector) + CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hBase) ); + *colPtr += 1.0; // Add back one one as identity + // Done with this Householder transformation + colLastPtr --; + } + + if ( numZerosSkipped!=0 ) { + assert( numZerosSkipped==1 ); + // Fill first row and column with identity (More generally: first numZerosSkipped many rows and columns) + double* d = x; + *d = 1; + double* d2 = d; + for ( i=NumRows-1; i>0; i-- ) { + *(++d) = 0; + *(d2+=NumRows) = 0; + } + } +} + +// **************** ConvertBidiagToDiagonal *********************************************** +// Do the iterative transformation from bidiagonal form to diagonal form using +// Givens transformation. (Golub-Reinsch) +// U and V are square. Size of U less than or equal to that of U. +void MatrixRmn::ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const +{ + // These two index into the last bidiagonal block (last in the matrix, it will be + // first one handled. + long lastBidiagIdx = V.NumRows-1; + long firstBidiagIdx = 0; + double eps = 1.0e-15 * Max(w.MaxAbs(), superDiag.MaxAbs()); + + while ( true ) { + bool workLeft = UpdateBidiagIndices( &firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps ); + if ( !workLeft ) { + break; + } + + // Get ready for first Givens rotation + // Push non-zero to M[2,1] with Givens transformation + double* wPtr = w.x+firstBidiagIdx; + double* sdPtr = superDiag.x+firstBidiagIdx; + double extraOffDiag=0.0; + if ( (*wPtr)==0.0 ) { + ClearRowWithDiagonalZero( firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps ); + if ( firstBidiagIdx>0 ) { + if ( NearZero( *(--sdPtr), eps ) ) { + *sdPtr = 0.0; + } + else { + ClearColumnWithDiagonalZero( firstBidiagIdx, V, wPtr, sdPtr, eps ); + } + } + continue; + } + + // Estimate an eigenvalue from bottom four entries of M + // This gives a lambda value which will shift the Givens rotations + // Last four entries of M^T * M are ( ( A, B ), ( B, C ) ). + double A; + A = (firstBidiagIdx C ) { + lambda = -lambda; + } + lambda += (A+C)*0.5; // Now lambda equals the estimate for the last eigenvalue + double t11 = Square(w[firstBidiagIdx]); + double t12 = w[firstBidiagIdx]*superDiag[firstBidiagIdx]; + + double c, s; + CalcGivensValues( t11-lambda, t12, &c, &s ); + ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, wPtr+1 ); + V.PostApplyGivens( c, -s, firstBidiagIdx ); + long i; + for ( i=firstBidiagIdx; i 0 ) { + if ( NearZero( *wPtr, eps ) ) { // If this diagonal entry (near) zero + *wPtr = 0.0; + break; + } + if ( NearZero(*(--sdPtr), eps) ) { // If the entry above the diagonal entry is (near) zero + *sdPtr = 0.0; + break; + } + wPtr--; + firstIdx--; + } + *firstBidiagIdx = firstIdx; + return true; +} + + +// ******************************************DEBUG STUFFF + +bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const +{ + // Special SVD test code + + MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() ); + IV.SetIdentity(); + MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() ); + MatrixRmn::TransposeMultiply( V, V, VTV ); + IV -= VTV; + double error = IV.FrobeniusNorm(); + + MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() ); + IU.SetIdentity(); + MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() ); + MatrixRmn::TransposeMultiply( U, U, UTU ); + IU -= UTU; + error += IU.FrobeniusNorm(); + + MatrixRmn Diag( U.GetNumRows(), V.GetNumRows() ); + Diag.SetZero(); + Diag.SetDiagonalEntries( w ); + MatrixRmn B(U.GetNumRows(), V.GetNumRows() ); + MatrixRmn C(U.GetNumRows(), V.GetNumRows() ); + MatrixRmn::Multiply( U, Diag, B ); + MatrixRmn::MultiplyTranspose( B, V, C ); + C -= *this; + error += C.FrobeniusNorm(); + + bool ret = ( fabs(error)<=1.0e-13*w.MaxAbs() ); + assert ( ret ); + return ret; +} + +bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const +{ + // Special SVD test code + + MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() ); + IV.SetIdentity(); + MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() ); + MatrixRmn::TransposeMultiply( V, V, VTV ); + IV -= VTV; + double error = IV.FrobeniusNorm(); + + MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() ); + IU.SetIdentity(); + MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() ); + MatrixRmn::TransposeMultiply( U, U, UTU ); + IU -= UTU; + error += IU.FrobeniusNorm(); + + MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() ); + DiagAndSuper.SetZero(); + DiagAndSuper.SetDiagonalEntries( w ); + if ( this->GetNumRows()>=this->GetNumColumns() ) { + DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 ); + } + else { + DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 ); + } + MatrixRmn B(U.GetNumRows(), V.GetNumRows() ); + MatrixRmn C(U.GetNumRows(), V.GetNumRows() ); + MatrixRmn::Multiply( U, DiagAndSuper, B ); + MatrixRmn::MultiplyTranspose( B, V, C ); + C -= *this; + error += C.FrobeniusNorm(); + + bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) ); + assert ( ret ); + return ret; +} + diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h new file mode 100644 index 000000000..544dd921e --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -0,0 +1,402 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +// +// MatrixRmn: Matrix over reals (Variable dimensional vector) +// +// Not very sophisticated yet. Needs more functionality +// To do: better handling of resizing. +// + +#ifndef MATRIX_RMN_H +#define MATRIX_RMN_H + +#include +#include +#include "LinearR3.h" +#include "VectorRn.h" + +class MatrixRmn { + +public: + MatrixRmn(); // Null constructor + MatrixRmn( long numRows, long numCols ); // Constructor with length + ~MatrixRmn(); // Destructor + + void SetSize( long numRows, long numCols ); + long GetNumRows() const { return NumRows; } + long GetNumColumns() const { return NumCols; } + void SetZero(); + + // Return entry in row i and column j. + double Get( long i, long j ) const; + void GetTriple( long i, long j, VectorR3 *retValue ) const; + + // Use GetPtr to get pointer into the array (efficient) + // Is friendly in that anyone can change the array contents (be careful!) + // The entries are in column order!!! + // Use this with care. You may call GetRowStride and GetColStride to navigate + // within the matrix. I do not expect these values to ever change. + const double* GetPtr() const; + double* GetPtr(); + const double* GetPtr( long i, long j ) const; + double* GetPtr( long i, long j ); + const double* GetColumnPtr( long j ) const; + double* GetColumnPtr( long j ); + const double* GetRowPtr( long i ) const; + double* GetRowPtr( long i ); + long GetRowStride() const { return NumRows; } // Step size (stride) along a row + long GetColStride() const { return 1; } // Step size (stide) along a column + + void Set( long i, long j, double val ); + void SetTriple( long i, long c, const VectorR3& u ); + + void SetIdentity(); + void SetDiagonalEntries( double d ); + void SetDiagonalEntries( const VectorRn& d ); + void SetSuperDiagonalEntries( double d ); + void SetSuperDiagonalEntries( const VectorRn& d ); + void SetSubDiagonalEntries( double d ); + void SetSubDiagonalEntries( const VectorRn& d ); + void SetColumn(long i, const VectorRn& d ); + void SetRow(long i, const VectorRn& d ); + void SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol ); + + // Loads matrix in as a sub-matrix. (i,j) is the base point. Defaults to (0,0). + // The "Tranpose" versions load the transpose of A. + void LoadAsSubmatrix( const MatrixRmn& A ); + void LoadAsSubmatrix( long i, long j, const MatrixRmn& A ); + void LoadAsSubmatrixTranspose( const MatrixRmn& A ); + void LoadAsSubmatrixTranspose( long i, long j, const MatrixRmn& A ); + + // Norms + double FrobeniusNormSq() const; + double FrobeniusNorm() const; + + // Operations on VectorRn's + void Multiply( const VectorRn& v, VectorRn& result ) const; // result = (this)*(v) + void MultiplyTranspose( const VectorRn& v, VectorRn& result ) const; // Equivalent to mult by row vector on left + double DotProductColumn( const VectorRn& v, long colNum ) const; // Returns dot product of v with i-th column + + // Operations on MatrixRmn's + MatrixRmn& operator*=( double ); + MatrixRmn& operator/=( double d ) { assert(d!=0.0); *this *= (1.0/d); return *this; } + MatrixRmn& AddScaled( const MatrixRmn& B, double factor ); + MatrixRmn& operator+=( const MatrixRmn& B ); + MatrixRmn& operator-=( const MatrixRmn& B ); + static MatrixRmn& Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ); // Sets dst = A*B. + static MatrixRmn& MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ); // Sets dst = A*(B-tranpose). + static MatrixRmn& TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ); // Sets dst = (A-transpose)*B. + + // Miscellaneous operation + MatrixRmn& AddToDiagonal( double d ); // Adds d to each diagonal + + // Solving systems of linear equations + void Solve( const VectorRn& b, VectorRn* x ) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible. + + // Row Echelon Form and Reduced Row Echelon Form routines + // Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables. + void ConvertToRefNoFree(); // Converts the matrix in place to row echelon form -- assumption is no free variables will be found + void ConvertToRef( int numVars); // Converts the matrix in place to row echelon form -- numVars is number of columns to work with. + void ConvertToRef( int numVars, double eps); // Same, but eps is the measure of closeness to zero + + // Givens transformation + static void CalcGivensValues( double a, double b, double *c, double *s ); + void PostApplyGivens( double c, double s, long idx ); // Applies Givens transform to columns idx and idx+1. + void PostApplyGivens( double c, double s, long idx1, long idx2 ); // Applies Givens transform to columns idx1 and idx2. + + // Singular value decomposition + void ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const; + // Good for debugging SVD computations (I recommend this be used for any new application to check for bugs/instability). + bool DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const; + + // Some useful routines for experts who understand the inner workings of these classes. + inline static double DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB ); + inline static void CopyArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale ); + inline static void AddArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale ); + +private: + long NumRows; // Number of rows + long NumCols; // Number of columns + double *x; // Array of vector entries - stored in column order + long AllocSize; // Allocated size of the x array + + static MatrixRmn WorkMatrix; // Temporary work matrix + static MatrixRmn& GetWorkMatrix() { return WorkMatrix; } + static MatrixRmn& GetWorkMatrix(long numRows, long numCols) { WorkMatrix.SetSize( numRows, numCols ); return WorkMatrix; } + + // Internal helper routines for SVD calculations + static void CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ); + void ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const; + static void SvdHouseholder( double* basePt, + long colLength, long numCols, long colStride, long rowStride, + double* retFirstEntry ); + void ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride ); + static bool UpdateBidiagIndices( long *firstDiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps ); + static void ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d ); + static void ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, + double d, double *e, double *f ); + static void ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx, + MatrixRmn& U, double *wPtr, double *sdPtr, double eps ); + static void ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps ); + bool DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const; +}; + +inline MatrixRmn::MatrixRmn() +{ + NumRows = 0; + NumCols = 0; + x = 0; + AllocSize = 0; +} + +inline MatrixRmn::MatrixRmn( long numRows, long numCols ) +{ + NumRows = 0; + NumCols = 0; + x = 0; + AllocSize = 0; + SetSize( numRows, numCols ); +} + +inline MatrixRmn::~MatrixRmn() +{ + delete x; +} + +// Resize. +// If the array space is decreased, the information about the allocated length is lost. +inline void MatrixRmn::SetSize( long numRows, long numCols ) +{ + assert ( numRows>0 && numCols>0 ); + long newLength = numRows*numCols; + if ( newLength>AllocSize ) { + delete x; + AllocSize = Max(newLength, AllocSize<<1); + x = new double[AllocSize]; + } + NumRows = numRows; + NumCols = numCols; +} + +// Zero out the entire vector +inline void MatrixRmn::SetZero() +{ + double* target = x; + for ( long i=NumRows*NumCols; i>0; i-- ) { + *(target++) = 0.0; + } +} + +// Return entry in row i and column j. +inline double MatrixRmn::Get( long i, long j ) const { + assert ( iLoad( x+j*NumRows + ii ); +} + +// Get a pointer to the (0,0) entry. +// The entries are in column order. +// This version gives read-only pointer +inline const double* MatrixRmn::GetPtr( ) const +{ + return x; +} + +// Get a pointer to the (0,0) entry. +// The entries are in column order. +inline double* MatrixRmn::GetPtr( ) +{ + return x; +} + +// Get a pointer to the (i,j) entry. +// The entries are in column order. +// This version gives read-only pointer +inline const double* MatrixRmn::GetPtr( long i, long j ) const +{ + assert ( 0<=i && i0; i-- ) { + (*(aPtr++)) *= mult; + } + return (*this); +} + +inline MatrixRmn& MatrixRmn::AddScaled( const MatrixRmn& B, double factor ) +{ + assert (NumRows==B.NumRows && NumCols==B.NumCols); + double* aPtr = x; + double* bPtr = B.x; + for ( long i=NumRows*NumCols; i>0; i-- ) { + (*(aPtr++)) += (*(bPtr++))*factor; + } + return (*this); +} + +inline MatrixRmn& MatrixRmn::operator+=( const MatrixRmn& B ) +{ + assert (NumRows==B.NumRows && NumCols==B.NumCols); + double* aPtr = x; + double* bPtr = B.x; + for ( long i=NumRows*NumCols; i>0; i-- ) { + (*(aPtr++)) += *(bPtr++); + } + return (*this); +} + +inline MatrixRmn& MatrixRmn::operator-=( const MatrixRmn& B ) +{ + assert (NumRows==B.NumRows && NumCols==B.NumCols); + double* aPtr = x; + double* bPtr = B.x; + for ( long i=NumRows*NumCols; i>0; i-- ) { + (*(aPtr++)) -= *(bPtr++); + } + return (*this); +} + +inline double MatrixRmn::FrobeniusNormSq() const +{ + double* aPtr = x; + double result = 0.0; + for ( long i=NumRows*NumCols; i>0; i-- ) { + result += Square( *(aPtr++) ); + } + return result; +} + +// Helper routine to calculate dot product +inline double MatrixRmn::DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB ) +{ + double result = 0.0; + for ( ; length>0 ; length-- ) { + result += (*ptrA)*(*ptrB); + ptrA += strideA; + ptrB += strideB; + } + return result; +} + +// Helper routine: copies and scales an array (src and dest may be equal, or overlap) +inline void MatrixRmn::CopyArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale ) +{ + for ( ; length>0; length-- ) { + *to = (*from)*scale; + from += fromStride; + to += toStride; + } +} + +// Helper routine: adds a scaled array +// fromArray = toArray*scale. +inline void MatrixRmn::AddArrayScale( long length, const double* from, long fromStride, double *to, long toStride, double scale ) +{ + for ( ; length>0; length-- ) { + *to += (*from)*scale; + from += fromStride; + to += toStride; + } +} + + + +#endif MATRIX_RMN_H \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp new file mode 100644 index 000000000..03f34812b --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp @@ -0,0 +1,346 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +#include +#include "LinearR3.h" + +#include "../OpenGLWindow/OpenGLInclude.h" + +/**************************************************************** + Axes +*****************************************************************/ +static float xx[] = { + 0., 1., 0., 1. + }; + +static float xy[] = { + -.5, .5, .5, -.5 + }; + +static int xorder[] = { + 1, 2, -3, 4 + }; + + +static float yx[] = { + 0., 0., -.5, .5 + }; + +static float yy[] = { + 0.f, .6f, 1.f, 1.f + }; + +static int yorder[] = { + 1, 2, 3, -2, 4 + }; + + +static float zx[] = { + 1., 0., 1., 0., .25, .75 + }; + +static float zy[] = { + .5, .5, -.5, -.5, 0., 0. + }; + +static int zorder[] = { + 1, 2, 3, 4, -5, 6 + }; + +#define LENFRAC 0.10 +#define BASEFRAC 1.10 + +void Axes( float length ) +{ + int i, j; /* counters */ + float fact; /* character scale factor */ + float base; /* character start location */ + + glBegin( GL_LINE_STRIP ); + glVertex3f( length, 0., 0. ); + glVertex3f( 0., 0., 0. ); + glVertex3f( 0., length, 0. ); + glEnd(); + glBegin( GL_LINE_STRIP ); + glVertex3f( 0., 0., 0. ); + glVertex3f( 0., 0., length ); + glEnd(); + + fact = LENFRAC * length; + base = BASEFRAC * length; + + glBegin( GL_LINE_STRIP ); + for( i = 0; i < 4; i++ ) + { + j = xorder[i]; + if( j < 0 ) + { + + glEnd(); + glBegin( GL_LINE_STRIP ); + j = -j; + } + j--; + glVertex3f( base + fact*xx[j], fact*xy[j], 0.0 ); + } + glEnd(); + + glBegin( GL_LINE_STRIP ); + for( i = 0; i < 5; i++ ) + { + j = yorder[i]; + if( j < 0 ) + { + + glEnd(); + glBegin( GL_LINE_STRIP ); + j = -j; + } + j--; + glVertex3f( fact*yx[j], base + fact*yy[j], 0.0 ); + } + glEnd(); + + glBegin( GL_LINE_STRIP ); + for( i = 0; i < 6; i++ ) + { + j = zorder[i]; + if( j < 0 ) + { + + glEnd(); + glBegin( GL_LINE_STRIP ); + j = -j; + } + j--; + glVertex3f( 0.0, fact*zy[j], base + fact*zx[j] ); + } + glEnd(); + +} + + +/**************************************************************** + Arrow +*****************************************************************/ + +/* size of wings as fraction of length: */ + +#define WINGS 0.10 + + +/* axes: */ + +#define X 1 +#define Y 2 +#define Z 3 + + +/* x, y, z, axes: */ + +static float axx[3] = { 1., 0., 0. }; +static float ayy[3] = { 0., 1., 0. }; +static float azz[3] = { 0., 0., 1. }; + + + +/* function declarations: */ +void Arrow( float tail[3], float head[3] ); +void Arrow( const VectorR3& tail, const VectorR3& head ); +void cross( float [3], float [3], float [3] ); +float dot( float [3], float [3] ); +float unit( float [3], float [3] ); + + +void Arrow( const VectorR3& tail, const VectorR3& head ) +{ + float t[3]; + float h[3]; + tail.Dump( t ); + head.Dump( h ); + Arrow( t, h ); +} + + +void Arrow( float tail[3], float head[3] ) +{ + float u[3], v[3], w[3]; /* arrow coordinate system */ + float d; /* wing distance */ + float x, y, z; /* point to plot */ + float mag; /* magnitude of major direction */ + float f; /* fabs of magnitude */ + int axis; /* which axis is the major */ + + + /* set w direction in u-v-w coordinate system: */ + + w[0] = head[0] - tail[0]; + w[1] = head[1] - tail[1]; + w[2] = head[2] - tail[2]; + + + /* determine major direction: */ + + axis = X; + mag = fabs( w[0] ); + if( (f=fabs(w[1])) > mag ) + { + axis = Y; + mag = f; + } + if( (f=fabs(w[2])) > mag ) + { + axis = Z; + mag = f; + } + + + /* set size of wings and turn w into a unit vector: */ + + d = WINGS * unit( w, w ); + + + /* draw the shaft of the arrow: */ + + glBegin( GL_LINE_STRIP ); + glVertex3fv( tail ); + glVertex3fv( head ); + glEnd(); + + /* draw two sets of wings in the non-major directions: */ + + if( axis != X ) + { + cross( w, axx, v ); + (void) unit( v, v ); + cross( v, w, u ); + x = head[0] + d * ( u[0] - w[0] ); + y = head[1] + d * ( u[1] - w[1] ); + z = head[2] + d * ( u[2] - w[2] ); + glBegin( GL_LINE_STRIP ); + glVertex3fv( head ); + glVertex3f( x, y, z ); + glEnd(); + x = head[0] + d * ( -u[0] - w[0] ); + y = head[1] + d * ( -u[1] - w[1] ); + z = head[2] + d * ( -u[2] - w[2] ); + glBegin( GL_LINE_STRIP ); + glVertex3fv( head ); + glVertex3f( x, y, z ); + glEnd(); + } + + + if( axis != Y ) + { + cross( w, ayy, v ); + (void) unit( v, v ); + cross( v, w, u ); + x = head[0] + d * ( u[0] - w[0] ); + y = head[1] + d * ( u[1] - w[1] ); + z = head[2] + d * ( u[2] - w[2] ); + glBegin( GL_LINE_STRIP ); + glVertex3fv( head ); + glVertex3f( x, y, z ); + glEnd(); + x = head[0] + d * ( -u[0] - w[0] ); + y = head[1] + d * ( -u[1] - w[1] ); + z = head[2] + d * ( -u[2] - w[2] ); + glBegin( GL_LINE_STRIP ); + glVertex3fv( head ); + glVertex3f( x, y, z ); + glEnd(); + } + + if( axis != Z ) + { + cross( w, azz, v ); + (void) unit( v, v ); + cross( v, w, u ); + x = head[0] + d * ( u[0] - w[0] ); + y = head[1] + d * ( u[1] - w[1] ); + z = head[2] + d * ( u[2] - w[2] ); + glBegin( GL_LINE_STRIP ); + glVertex3fv( head ); + glVertex3f( x, y, z ); + glEnd(); + x = head[0] + d * ( -u[0] - w[0] ); + y = head[1] + d * ( -u[1] - w[1] ); + z = head[2] + d * ( -u[2] - w[2] ); + glBegin( GL_LINE_STRIP ); + glVertex3fv( head ); + glVertex3f( x, y, z ); + glEnd(); + } + + + /* done: */ + +} + +float dot( float v1[3], float v2[3] ) +{ + return( v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] ); +} + + + +void +cross( float v1[3], float v2[3], float vout[3] ) +{ + float tmp[3]; + + tmp[0] = v1[1]*v2[2] - v2[1]*v1[2]; + tmp[1] = v2[0]*v1[2] - v1[0]*v2[2]; + tmp[2] = v1[0]*v2[1] - v2[0]*v1[1]; + + vout[0] = tmp[0]; + vout[1] = tmp[1]; + vout[2] = tmp[2]; +} + + + +float +unit( float vin[3], float vout[3] ) +{ + float dist, f ; + + dist = vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2]; + + if( dist > 0.0 ) + { + dist = sqrt( dist ); + f = 1. / dist; + vout[0] = f * vin[0]; + vout[1] = f * vin[1]; + vout[2] = f * vin[2]; + } + else + { + vout[0] = vin[0]; + vout[1] = vin[1]; + vout[2] = vin[2]; + } + + return( dist ); +} diff --git a/examples/ThirdPartyLibs/BussIK/Node.cpp b/examples/ThirdPartyLibs/BussIK/Node.cpp new file mode 100644 index 000000000..612cc42d7 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Node.cpp @@ -0,0 +1,90 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +#include + + +#include "LinearR3.h" +#include "MathMisc.h" +#include "Node.h" + +extern int RotAxesOn; + +Node::Node(const VectorR3& attach, const VectorR3& v, double size, Purpose purpose, double minTheta, double maxTheta, double restAngle) +{ + Node::freezed = false; + Node::size = size; + Node::purpose = purpose; + seqNumJoint = -1; + seqNumEffector = -1; + Node::attach = attach; // Global attachment point when joints are at zero angle + r.Set(0.0, 0.0, 0.0); // r will be updated when this node is inserted into tree + Node::v = v; // Rotation axis when joints at zero angles + theta = 0.0; + Node::minTheta = minTheta; + Node::maxTheta = maxTheta; + Node::restAngle = restAngle; + left = right = realparent = 0; +} + +// Compute the global position of a single node +void Node::ComputeS(void) +{ + Node* y = this->realparent; + Node* w = this; + s = r; // Initialize to local (relative) position + while ( y ) { + s.Rotate( y->theta, y->v ); + y = y->realparent; + w = w->realparent; + s += w->r; + } +} + +// Compute the global rotation axis of a single node +void Node::ComputeW(void) +{ + Node* y = this->realparent; + w = v; // Initialize to local rotation axis + while (y) { + w.Rotate(y->theta, y->v); + y = y->realparent; + } +} + + + +void Node::PrintNode() +{ + cerr << "Attach : (" << attach << ")\n"; + cerr << "r : (" << r << ")\n"; + cerr << "s : (" << s << ")\n"; + cerr << "w : (" << w << ")\n"; + cerr << "realparent : " << realparent->seqNumJoint << "\n"; +} + + +void Node::InitNode() +{ + theta = 0.0; +} \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/Node.h b/examples/ThirdPartyLibs/BussIK/Node.h new file mode 100644 index 000000000..5a49d278a --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Node.h @@ -0,0 +1,101 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +#ifndef _CLASS_NODE +#define _CLASS_NODE + +#include "LinearR3.h" + +enum Purpose {JOINT, EFFECTOR}; + +class VectorR3; + +class Node { + + friend class Tree; + +public: + Node(const VectorR3&, const VectorR3&, double, Purpose, double minTheta=-PI, double maxTheta=PI, double restAngle=0.); + + + void PrintNode(); + void InitNode(); + + const VectorR3& GetAttach() const { return attach; } + + double GetTheta() const { return theta; } + double AddToTheta( double& delta ) { + double orgTheta = theta; + theta += delta; +#if 0 + if (theta < minTheta) + theta = minTheta; + if (theta > maxTheta) + theta = maxTheta; + double actualDelta = theta - orgTheta; + delta = actualDelta; +#endif + return theta; } + + const VectorR3& GetS() const { return s; } + const VectorR3& GetW() const { return w; } + + double GetMinTheta() const { return minTheta; } + double GetMaxTheta() const { return maxTheta; } + double GetRestAngle() const { return restAngle; } ; + void SetTheta(double newTheta) { theta = newTheta; } + void ComputeS(void); + void ComputeW(void); + + bool IsEffector() const { return purpose==EFFECTOR; } + bool IsJoint() const { return purpose==JOINT; } + int GetEffectorNum() const { return seqNumEffector; } + int GetJointNum() const { return seqNumJoint; } + + bool IsFrozen() const { return freezed; } + void Freeze() { freezed = true; } + void UnFreeze() { freezed = false; } + +//private: + bool freezed; // Is this node frozen? + int seqNumJoint; // sequence number if this node is a joint + int seqNumEffector; // sequence number if this node is an effector + double size; // size + Purpose purpose; // joint / effector / both + VectorR3 attach; // attachment point + VectorR3 r; // relative position vector + VectorR3 v; // rotation axis + double theta; // joint angle (radian) + double minTheta; // lower limit of joint angle + double maxTheta; // upper limit of joint angle + double restAngle; // rest position angle + VectorR3 s; // GLobal Position + VectorR3 w; // Global rotation axis + Node* left; // left child + Node* right; // right sibling + Node* realparent; // pointer to real parent + + +}; + +#endif \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/Spherical.h b/examples/ThirdPartyLibs/BussIK/Spherical.h new file mode 100644 index 000000000..5db1862d3 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Spherical.h @@ -0,0 +1,298 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + + +// +// Spherical Operations Classes +// +// +// B. SphericalInterpolator +// +// OrientationR3 +// +// A. Quaternion +// +// B. RotationMapR3 // Elsewhere +// +// C. EulerAnglesR3 // TO DO +// +// +// Functions for spherical operations +// A. Many routines for rotation and averaging on a sphere +// + +#ifndef SPHERICAL_H +#define SPHERICAL_H + +#include "LinearR3.h" +#include "LinearR4.h" +#include "MathMisc.h" + +class SphericalInterpolator; // Spherical linear interpolation of vectors +class SphericalBSpInterpolator; // Spherical Bspline interpolation of vector +class Quaternion; // Quaternion (x,y,z,w) values. +class EulerAnglesR3; // Euler Angles + +// ***************************************************** +// SphericalInterpolator class * +// - Does linear interpolation (slerp-ing) * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * + + +class SphericalInterpolator { + +private: + VectorR3 startDir, endDir; // Unit vectors (starting and ending) + double startLen, endLen; // Magnitudes of the vectors + double rotRate; // Angle between start and end vectors + +public: + SphericalInterpolator( const VectorR3& u, const VectorR3& v ); + + VectorR3 InterValue ( double frac ) const; +}; + + +// *************************************************************** +// * Quaternion class - prototypes * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +class Quaternion { + +public: + double x, y, z, w; + +public: + Quaternion() :x(0.0), y(0.0), z(0.0), w(1.0) {}; + Quaternion( double, double, double, double ); + + inline Quaternion& Set( double xx, double yy, double zz, double ww ); + inline Quaternion& Set( const VectorR4& ); + Quaternion& Set( const EulerAnglesR3& ); + Quaternion& Set( const RotationMapR3& ); + Quaternion& SetRotate( const VectorR3& ); + + Quaternion& SetIdentity(); // Set to the identity map + Quaternion Inverse() const; // Return the Inverse + Quaternion& Invert(); // Invert this quaternion + + double Angle(); // Angle of rotation + double Norm(); // Norm of x,y,z component + + Quaternion& operator*=(const Quaternion&); + +}; + +Quaternion operator*(const Quaternion&, const Quaternion&); + +inline Quaternion ToQuat( const VectorR4& v) +{ + return Quaternion(v.x,v.y,v.z,v.w); +} + +inline double Quaternion::Norm() +{ + return sqrt( x*x + y*y + z*z ); +} + +inline double Quaternion::Angle () +{ + double halfAngle = asin(Norm()); + return halfAngle+halfAngle; +} + + +// **************************************************************** +// Solid Geometry Routines * +// **************************************************************** + +// Compute the angle formed by two geodesics on the unit sphere. +// Three unit vectors u,v,w specify the geodesics u-v and v-w which +// meet at vertex uv. The angle from v-w to v-u is returned. This +// is always in the range [0, 2PI). +double SphereAngle( const VectorR3& u, const VectorR3& v, const VectorR3& w ); + +// Compute the area of a triangle on the unit sphere. Three unit vectors +// specify the corners of the triangle in COUNTERCLOCKWISE order. +inline double SphericalTriangleArea( + const VectorR3& u, const VectorR3& v, const VectorR3& w ) +{ + double AngleA = SphereAngle( u,v,w ); + double AngleB = SphereAngle( v,w,u ); + double AngleC = SphereAngle( w,u,v ); + return ( AngleA+AngleB+AngleC - PI ); +} + + +// **************************************************************** +// Spherical Mean routines * +// **************************************************************** + +// Weighted sum of vectors +VectorR3 WeightedSum( long Num, const VectorR3 vv[], const double weights[] ); +VectorR4 WeightedSum( long Num, const VectorR4 vv[], const double weights[] ); + +// Weighted average of vectors on the sphere. +// Sum of weights should equal one (but no checking is done) +VectorR3 ComputeMeanSphere( long Num, const VectorR3 vv[], const double weights[], + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); +VectorR3 ComputeMeanSphere( long Num, const VectorR3 vv[], const double weights[], + const VectorR3& InitialVec, + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); +VectorR4 ComputeMeanSphere( long Num, const VectorR4 vv[], const double weights[], + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); +Quaternion ComputeMeanQuat( long Num, const Quaternion qq[], const double weights[], + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); + +// Next functions mostly for internal use. +// It takes an initial estimate InitialVec (and a flag for +// indicating quaternions). +VectorR4 ComputeMeanSphere( long Num, const VectorR4 vv[], const double weights[], + const VectorR4& InitialVec, int QuatFlag=0, + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); +const int SPHERICAL_NOTQUAT=0; +const int SPHERICAL_QUAT=1; + +// Slow version, mostly for testing +VectorR3 ComputeMeanSphereSlow( long Num, const VectorR3 vv[], const double weights[], + double tolerance = 1.0e-16, double bkuptolerance = 5.0e-16 ); +VectorR4 ComputeMeanSphereSlow( long Num, const VectorR4 vv[], const double weights[], + double tolerance = 1.0e-16, double bkuptolerance = 5.0e-16 ); +VectorR3 ComputeMeanSphereOld( long Num, const VectorR3 vv[], const double weights[], + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); +VectorR4 ComputeMeanSphereOld( long Num, const VectorR4 vv[], const double weights[], + const VectorR4& InitialVec, int QuatFlag, + double tolerance = 1.0e-15, double bkuptolerance = 1.0e-13 ); + +// Solves a system of spherical-mean equalities, where the system is +// given as a tridiagonal matrix. +void SolveTriDiagSphere ( int numPoints, + const double* tridiagvalues, const VectorR3* c, + VectorR3* p, + double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 ); +void SolveTriDiagSphere ( int numPoints, + const double* tridiagvalues, const VectorR4* c, + VectorR4* p, + double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 ); + +// The "Slow" version uses a simpler but slower iteration with a linear rate of +// convergence. The base version uses a Newton iteration with a quadratic +// rate of convergence. +void SolveTriDiagSphereSlow ( int numPoints, + const double* tridiagvalues, const VectorR3* c, + VectorR3* p, + double accuracy=1.0e-15, double bkupaccuracy=5.0e-15 ); +void SolveTriDiagSphereSlow ( int numPoints, + const double* tridiagvalues, const VectorR4* c, + VectorR4* p, + double accuracy=1.0e-15, double bkupaccuracy=5.0e-15 ); + +// The "Unstable" version probably shouldn't be used except for very short sequences +// of knots. Mostly it's used for testing purposes now. +void SolveTriDiagSphereUnstable ( int numPoints, + const double* tridiagvalues, const VectorR3* c, + VectorR3* p, + double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 ); +void SolveTriDiagSphereHelperUnstable ( int numPoints, + const double* tridiagvalues, const VectorR3 *c, + const VectorR3& p0value, + VectorR3 *p, + double accuracy=1.0e-15, double bkupaccuracy=1.0e-13 ); + + + +// *************************************************************** +// * Quaternion class - inlined member functions * +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + +inline VectorR4::VectorR4 ( const Quaternion& q ) +: x(q.x), y(q.y), z(q.z), w(q.w) +{} + +inline VectorR4& VectorR4::Set ( const Quaternion& q ) +{ + x = q.x; y = q.y; z = q.z; w = q.w; + return *this; +} + +inline Quaternion::Quaternion( double xx, double yy, double zz, double ww) +: x(xx), y(yy), z(zz), w(ww) +{} + +inline Quaternion& Quaternion::Set( double xx, double yy, double zz, double ww ) +{ + x = xx; + y = yy; + z = zz; + w = ww; + return *this; +} + +inline Quaternion& Quaternion::Set( const VectorR4& u) +{ + x = u.x; + y = u.y; + z = u.z; + w = u.w; + return *this; +} + +inline Quaternion& Quaternion::SetIdentity() +{ + x = y = z = 0.0; + w = 1.0; + return *this; +} + +inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2) +{ + Quaternion q(q1); + q *= q2; + return q; +} + +inline Quaternion& Quaternion::operator*=( const Quaternion& q ) +{ + double wnew = w*q.w - (x*q.x + y*q.y + z*q.z); + double xnew = w*q.x + q.w*x + (y*q.z - z*q.y); + double ynew = w*q.y + q.w*y + (z*q.x - x*q.z); + z = w*q.z + q.w*z + (x*q.y - y*q.x); + w = wnew; + x = xnew; + y = ynew; + return *this; +} + +inline Quaternion Quaternion::Inverse() const // Return the Inverse +{ + return ( Quaternion( x, y, z, -w ) ); +} + +inline Quaternion& Quaternion::Invert() // Invert this quaternion +{ + w = -w; + return *this; +} + + +#endif // SPHERICAL_H diff --git a/examples/ThirdPartyLibs/BussIK/Tree.cpp b/examples/ThirdPartyLibs/BussIK/Tree.cpp new file mode 100644 index 000000000..a82e7f1d7 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Tree.cpp @@ -0,0 +1,217 @@ +/* +* +* Inverse Kinematics software, with several solvers including +* Selectively Damped Least Squares Method +* Damped Least Squares Method +* Pure Pseudoinverse Method +* Jacobian Transpose Method +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +// +// VectorRn: Vector over Rn (Variable length vector) +// + +#include +using namespace std; + + + + +#include "LinearR3.h" +#include "Tree.h" +#include "Node.h" + +Tree::Tree() +{ + root = 0; + nNode = nEffector = nJoint = 0; +} + +void Tree::SetSeqNum(Node* node) +{ + switch (node->purpose) { + case JOINT: + node->seqNumJoint = nJoint++; + node->seqNumEffector = -1; + break; + case EFFECTOR: + node->seqNumJoint = -1; + node->seqNumEffector = nEffector++; + break; + } +} + +void Tree::InsertRoot(Node* root) +{ + assert( nNode==0 ); + nNode++; + Tree::root = root; + root->r = root->attach; + assert( !(root->left || root->right) ); + SetSeqNum(root); +} + +void Tree::InsertLeftChild(Node* parent, Node* child) +{ + assert(parent); + nNode++; + parent->left = child; + child->realparent = parent; + child->r = child->attach - child->realparent->attach; + assert( !(child->left || child->right) ); + SetSeqNum(child); +} + +void Tree::InsertRightSibling(Node* parent, Node* child) +{ + assert(parent); + nNode++; + parent->right = child; + child->realparent = parent->realparent; + child->r = child->attach - child->realparent->attach; + assert( !(child->left || child->right) ); + SetSeqNum(child); +} + +// Search recursively below "node" for the node with index value. +Node* Tree::SearchJoint(Node* node, int index) +{ + Node* ret; + if (node != 0) { + if (node->seqNumJoint == index) { + return node; + } else { + if (ret = SearchJoint(node->left, index)) { + return ret; + } + if (ret = SearchJoint(node->right, index)) { + return ret; + } + return NULL; + } + } + else { + return NULL; + } +} + + +// Get the joint with the index value +Node* Tree::GetJoint(int index) +{ + return SearchJoint(root, index); +} + +// Search recursively below node for the end effector with the index value +Node* Tree::SearchEffector(Node* node, int index) +{ + Node* ret; + if (node != 0) { + if (node->seqNumEffector == index) { + return node; + } else { + if (ret = SearchEffector(node->left, index)) { + return ret; + } + if (ret = SearchEffector(node->right, index)) { + return ret; + } + return NULL; + } + } else { + return NULL; + } +} + + +// Get the end effector for the index value +Node* Tree::GetEffector(int index) +{ + return SearchEffector(root, index); +} + +// Returns the global position of the effector. +const VectorR3& Tree::GetEffectorPosition(int index) +{ + Node* effector = GetEffector(index); + assert(effector); + return (effector->s); +} + +void Tree::ComputeTree(Node* node) +{ + if (node != 0) { + node->ComputeS(); + node->ComputeW(); + ComputeTree(node->left); + ComputeTree(node->right); + } +} + +void Tree::Compute(void) +{ + ComputeTree(root); +} + + +void Tree::PrintTree(Node* node) +{ + if (node != 0) { + node->PrintNode(); + PrintTree(node->left); + PrintTree(node->right); + } +} + +void Tree::Print(void) +{ + PrintTree(root); + cout << "\n"; +} + +// Recursively initialize tree below the node +void Tree::InitTree(Node* node) +{ + if (node != 0) { + node->InitNode(); + InitTree(node->left); + InitTree(node->right); + } +} + +// Initialize all nodes in the tree +void Tree::Init(void) +{ + InitTree(root); +} + +void Tree::UnFreezeTree(Node* node) +{ + if (node != 0) { + node->UnFreeze(); + UnFreezeTree(node->left); + UnFreezeTree(node->right); + } +} + +void Tree::UnFreeze(void) +{ + UnFreezeTree(root); +} \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/Tree.h b/examples/ThirdPartyLibs/BussIK/Tree.h new file mode 100644 index 000000000..208176817 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Tree.h @@ -0,0 +1,92 @@ +/* +* +* Inverse Kinematics software, with several solvers including +* Selectively Damped Least Squares Method +* Damped Least Squares Method +* Pure Pseudoinverse Method +* Jacobian Transpose Method +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +#include "LinearR3.h" +#include "Node.h" + +#ifndef _CLASS_TREE +#define _CLASS_TREE + +class Tree { + +public: + Tree(); + + int GetNumNode() const { return nNode; } + int GetNumEffector() const { return nEffector; } + int GetNumJoint() const { return nJoint; } + void InsertRoot(Node*); + void InsertLeftChild(Node* parent, Node* child); + void InsertRightSibling(Node* parent, Node* child); + + // Accessors based on node numbers + Node* GetJoint(int); + Node* GetEffector(int); + const VectorR3& GetEffectorPosition(int); + + // Accessors for tree traversal + Node* GetRoot() const { return root; } + Node* GetSuccessor ( const Node* ) const; + Node* GetParent( const Node* node ) const { return node->realparent; } + + void Compute(); + + void Print(); + void Init(); + void UnFreeze(); + +private: + Node* root; + int nNode; // nNode = nEffector + nJoint + int nEffector; + int nJoint; + void SetSeqNum(Node*); + Node* SearchJoint(Node*, int); + Node* SearchEffector(Node*, int); + void ComputeTree(Node*); + + void PrintTree(Node*); + void InitTree(Node*); + void UnFreezeTree(Node*); +}; + +inline Node* Tree::GetSuccessor ( const Node* node ) const +{ + if ( node->left ) { + return node->left; + } + while ( true ) { + if ( node->right ) { + return ( node->right ); + } + node = node->realparent; + if ( !node ) { + return 0; // Back to root, finished traversal + } + } +} + +#endif \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/VectorRn.cpp b/examples/ThirdPartyLibs/BussIK/VectorRn.cpp new file mode 100644 index 000000000..5d2b563e9 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/VectorRn.cpp @@ -0,0 +1,46 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + + +// +// VectorRn: Vector over Rn (Variable length vector) +// + +#include "VectorRn.h" + +VectorRn VectorRn::WorkVector; + +double VectorRn::MaxAbs () const +{ + double result = 0.0; + double* t = x; + for ( long i = length; i>0; i-- ) { + if ( (*t) > result ) { + result = *t; + } + else if ( -(*t) > result ) { + result = -(*t); + } + t++; + } + return result; +} \ No newline at end of file diff --git a/examples/ThirdPartyLibs/BussIK/VectorRn.h b/examples/ThirdPartyLibs/BussIK/VectorRn.h new file mode 100644 index 000000000..8357f9997 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/VectorRn.h @@ -0,0 +1,238 @@ +/* +* +* Mathematics Subpackage (VrMath) +* +* +* Author: Samuel R. Buss, sbuss@ucsd.edu. +* Web page: http://math.ucsd.edu/~sbuss/MathCG +* +* +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +* +* +*/ + +// +// VectorRn: Vector over Rn (Variable length vector) +// +// Not very sophisticated yet. Needs more functionality +// To do: better handling of resizing. +// + +#ifndef VECTOR_RN_H +#define VECTOR_RN_H + +#include +#include +#include "LinearR3.h" + +class VectorRn { + friend class MatrixRmn; + +public: + VectorRn(); // Null constructor + VectorRn( long length ); // Constructor with length + ~VectorRn(); // Destructor + + void SetLength( long newLength ); + long GetLength() const { return length; } + + void SetZero(); + void Fill( double d ); + void Load( const double* d ); + void LoadScaled( const double* d, double scaleFactor ); + void Set ( const VectorRn& src ); + + // Two access methods identical in functionality + // Subscripts are ZERO-BASED!! + const double& operator[]( long i ) const { assert ( 0<=i && i0 ); + if ( newLength>AllocLength ) { + delete x; + AllocLength = Max( newLength, AllocLength<<1 ); + x = new double[AllocLength]; + } + length = newLength; +} + +// Zero out the entire vector +inline void VectorRn::SetZero() +{ + double* target = x; + for ( long i=length; i>0; i-- ) { + *(target++) = 0.0; + } +} + +// Set the value of the i-th triple of entries in the vector +inline void VectorRn::SetTriple( long i, const VectorR3& u ) +{ + long j = 3*i; + assert ( 0<=j && j+2 < length ); + u.Dump( x+j ); +} + +inline void VectorRn::Fill( double d ) +{ + double *to = x; + for ( long i=length; i>0; i-- ) { + *(to++) = d; + } +} + +inline void VectorRn::Load( const double* d ) +{ + double *to = x; + for ( long i=length; i>0; i-- ) { + *(to++) = *(d++); + } +} + +inline void VectorRn::LoadScaled( const double* d, double scaleFactor ) +{ + double *to = x; + for ( long i=length; i>0; i-- ) { + *(to++) = (*(d++))*scaleFactor; + } +} + +inline void VectorRn::Set( const VectorRn& src ) +{ + assert ( src.length == this->length ); + double* to = x; + double* from = src.x; + for ( long i=length; i>0; i-- ) { + *(to++) = *(from++); + } +} + +inline VectorRn& VectorRn::operator+=( const VectorRn& src ) +{ + assert ( src.length == this->length ); + double* to = x; + double* from = src.x; + for ( long i=length; i>0; i-- ) { + *(to++) += *(from++); + } + return *this; +} + +inline VectorRn& VectorRn::operator-=( const VectorRn& src ) +{ + assert ( src.length == this->length ); + double* to = x; + double* from = src.x; + for ( long i=length; i>0; i-- ) { + *(to++) -= *(from++); + } + return *this; +} + +inline void VectorRn::AddScaled (const VectorRn& src, double scaleFactor ) +{ + assert ( src.length == this->length ); + double* to = x; + double* from = src.x; + for ( long i=length; i>0; i-- ) { + *(to++) += (*(from++))*scaleFactor; + } +} + +inline VectorRn& VectorRn::operator*=( double f ) +{ + double* target = x; + for ( long i=length; i>0; i-- ) { + *(target++) *= f; + } + return *this; +} + +inline double VectorRn::NormSq() const +{ + double* target = x; + double res = 0.0; + for ( long i=length; i>0; i-- ) { + res += (*target)*(*target); + target++; + } + return res; +} + +inline double Dot( const VectorRn& u, const VectorRn& v ) +{ + assert ( u.GetLength() == v.GetLength() ); + double res = 0.0; + const double* p = u.GetPtr(); + const double* q = v.GetPtr(); + for ( long i = u.GetLength(); i>0; i-- ) { + res += (*(p++))*(*(q++)); + } + return res; +} + +#endif VECTOR_RN_H \ No newline at end of file diff --git a/examples/TinyRenderer/tgaimage.h b/examples/TinyRenderer/tgaimage.h index 7a4dbfae9..9e2cbf877 100644 --- a/examples/TinyRenderer/tgaimage.h +++ b/examples/TinyRenderer/tgaimage.h @@ -24,8 +24,10 @@ struct TGAColor { unsigned char bgra[4]; unsigned char bytespp; - TGAColor() : bgra(), bytespp(1) { - for (int i=0; i<4; i++) bgra[i] = 0; + TGAColor() : bytespp(1) + { + for (int i=0; i<4; i++) + bgra[i] = 0; } TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 86c7d3a74..ebc1042b4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -23,9 +23,9 @@ subject to the following restrictions: btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity), - m_desiredPosition(0), - m_kd(1.), + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(1.), m_kp(0) { @@ -54,10 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity), - m_desiredPosition(0), - m_kd(1.), - m_kp(0) + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(1.), + m_kp(0) { btAssert(linkDoF < body->getLink(link).m_dofCount); @@ -119,14 +119,14 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; - btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; - btScalar velocityError = (m_desiredVelocity - currentVelocity); - btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; - + btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + btScalar velocityError = (m_desiredVelocity - currentVelocity); + btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs); constraintRow.m_orgConstraint = this; From 53fa57bdc4ee17891659c6ac7700e69f4dd9d04a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 24 Jul 2016 23:50:18 -0700 Subject: [PATCH 05/36] make IK compile on Mac OSX --- examples/ExampleBrowser/ExampleEntries.cpp | 4 + .../InverseKinematicsExample.cpp | 3 + .../InverseKinematicsExample.h | 2 +- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 31 +-- examples/ThirdPartyLibs/BussIK/Jacobian.h | 3 +- examples/ThirdPartyLibs/BussIK/LinearR4.h | 2 +- examples/ThirdPartyLibs/BussIK/Misc.cpp | 195 +----------------- examples/ThirdPartyLibs/BussIK/Tree.cpp | 2 +- examples/ThirdPartyLibs/BussIK/Tree.h | 2 +- 9 files changed, 14 insertions(+), 230 deletions(-) diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a6abd4fd7..bf4ac2233 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -136,6 +136,10 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0, "Inverse Kinematics"), ExampleEntry(1, "SDLS", "Selectively Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_SDLS), ExampleEntry(1, "DLS", "Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS), + ExampleEntry(1, "DLS-SVD", "Damped Least Squares with Singular Value Decomposition by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS_SVD), + + + ExampleEntry(1, "Jacobi Transpose", "Jacobi Transpose by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_JACOB_TRANS), ExampleEntry(1, "Jacobi Pseudo Inv", "Jacobi Pseudo Inverse Method by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_PURE_PSEUDO), diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index ae1ed8f17..8f902b008 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -113,6 +113,9 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) { case IK_DLS: jacob->CalcDeltaThetasDLS(); // Damped least squares method break; + case IK_DLS_SVD: + jacob->CalcDeltaThetasDLSwithSVD(); + break; case IK_PURE_PSEUDO: jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method break; diff --git a/examples/InverseKinematics/InverseKinematicsExample.h b/examples/InverseKinematics/InverseKinematicsExample.h index ca0d1e3b5..845925ef6 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.h +++ b/examples/InverseKinematics/InverseKinematicsExample.h @@ -1,7 +1,7 @@ #ifndef INVERSE_KINEMATICSEXAMPLE_H #define INVERSE_KINEMATICSEXAMPLE_H -enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS }; +enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS , IK_DLS_SVD}; class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 646513b86..2b0454ea8 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -8,7 +8,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. @@ -30,7 +30,6 @@ subject to the following restrictions: #include using namespace std; -#include "../OpenGLWindow/OpenGLInclude.h" #include "Jacobian.h" @@ -468,35 +467,7 @@ void Jacobian::UpdatedSClampValue() } } -void Jacobian::DrawEigenVectors() const -{ - int i, j; - VectorR3 tail; - VectorR3 head; - Node *node; - for (i=0; iGetEffector(j); - tail = node->GetS(); - U.GetTriple( j, i, &head ); - head += tail; - glDisable(GL_LIGHTING); - glColor3f(1.0f, 0.2f, 0.0f); - glLineWidth(2.0); - glBegin(GL_LINES); - glVertex3f(tail.x, tail.y, tail.z); - glVertex3f(head.x, head.y, tail.z); - glEnd(); - Arrow(tail, head); - glLineWidth(1.0); - glEnable(GL_LIGHTING); - } - } -} void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 ) { diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 1c0bc4572..b1c683f8a 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -9,7 +9,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. @@ -72,7 +72,6 @@ public: double UpdateErrorArray(); // Returns sum of errors const VectorRn& GetErrorArray() const { return errorArray; } void UpdatedSClampValue(); - void DrawEigenVectors() const; void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; } UpdateMode GetCurrentMode() const { return CurrentUpdateMode; } diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h index ee995326c..51f57a666 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR4.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h @@ -195,7 +195,7 @@ public: inline double Diagonal( int ); inline void MakeTranspose(); // Transposes it. - void Matrix4x4::operator*= (const Matrix4x4& B); // Matrix product + void operator*= (const Matrix4x4& B); // Matrix product Matrix4x4& ReNormalize(); }; diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp index 03f34812b..6aa696d55 100644 --- a/examples/ThirdPartyLibs/BussIK/Misc.cpp +++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp @@ -69,75 +69,6 @@ static int zorder[] = { #define LENFRAC 0.10 #define BASEFRAC 1.10 -void Axes( float length ) -{ - int i, j; /* counters */ - float fact; /* character scale factor */ - float base; /* character start location */ - - glBegin( GL_LINE_STRIP ); - glVertex3f( length, 0., 0. ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 0., length, 0. ); - glEnd(); - glBegin( GL_LINE_STRIP ); - glVertex3f( 0., 0., 0. ); - glVertex3f( 0., 0., length ); - glEnd(); - - fact = LENFRAC * length; - base = BASEFRAC * length; - - glBegin( GL_LINE_STRIP ); - for( i = 0; i < 4; i++ ) - { - j = xorder[i]; - if( j < 0 ) - { - - glEnd(); - glBegin( GL_LINE_STRIP ); - j = -j; - } - j--; - glVertex3f( base + fact*xx[j], fact*xy[j], 0.0 ); - } - glEnd(); - - glBegin( GL_LINE_STRIP ); - for( i = 0; i < 5; i++ ) - { - j = yorder[i]; - if( j < 0 ) - { - - glEnd(); - glBegin( GL_LINE_STRIP ); - j = -j; - } - j--; - glVertex3f( fact*yx[j], base + fact*yy[j], 0.0 ); - } - glEnd(); - - glBegin( GL_LINE_STRIP ); - for( i = 0; i < 6; i++ ) - { - j = zorder[i]; - if( j < 0 ) - { - - glEnd(); - glBegin( GL_LINE_STRIP ); - j = -j; - } - j--; - glVertex3f( 0.0, fact*zy[j], base + fact*zx[j] ); - } - glEnd(); - -} - /**************************************************************** Arrow @@ -164,138 +95,14 @@ static float azz[3] = { 0., 0., 1. }; /* function declarations: */ -void Arrow( float tail[3], float head[3] ); -void Arrow( const VectorR3& tail, const VectorR3& head ); + void cross( float [3], float [3], float [3] ); float dot( float [3], float [3] ); float unit( float [3], float [3] ); -void Arrow( const VectorR3& tail, const VectorR3& head ) -{ - float t[3]; - float h[3]; - tail.Dump( t ); - head.Dump( h ); - Arrow( t, h ); -} -void Arrow( float tail[3], float head[3] ) -{ - float u[3], v[3], w[3]; /* arrow coordinate system */ - float d; /* wing distance */ - float x, y, z; /* point to plot */ - float mag; /* magnitude of major direction */ - float f; /* fabs of magnitude */ - int axis; /* which axis is the major */ - - - /* set w direction in u-v-w coordinate system: */ - - w[0] = head[0] - tail[0]; - w[1] = head[1] - tail[1]; - w[2] = head[2] - tail[2]; - - - /* determine major direction: */ - - axis = X; - mag = fabs( w[0] ); - if( (f=fabs(w[1])) > mag ) - { - axis = Y; - mag = f; - } - if( (f=fabs(w[2])) > mag ) - { - axis = Z; - mag = f; - } - - - /* set size of wings and turn w into a unit vector: */ - - d = WINGS * unit( w, w ); - - - /* draw the shaft of the arrow: */ - - glBegin( GL_LINE_STRIP ); - glVertex3fv( tail ); - glVertex3fv( head ); - glEnd(); - - /* draw two sets of wings in the non-major directions: */ - - if( axis != X ) - { - cross( w, axx, v ); - (void) unit( v, v ); - cross( v, w, u ); - x = head[0] + d * ( u[0] - w[0] ); - y = head[1] + d * ( u[1] - w[1] ); - z = head[2] + d * ( u[2] - w[2] ); - glBegin( GL_LINE_STRIP ); - glVertex3fv( head ); - glVertex3f( x, y, z ); - glEnd(); - x = head[0] + d * ( -u[0] - w[0] ); - y = head[1] + d * ( -u[1] - w[1] ); - z = head[2] + d * ( -u[2] - w[2] ); - glBegin( GL_LINE_STRIP ); - glVertex3fv( head ); - glVertex3f( x, y, z ); - glEnd(); - } - - - if( axis != Y ) - { - cross( w, ayy, v ); - (void) unit( v, v ); - cross( v, w, u ); - x = head[0] + d * ( u[0] - w[0] ); - y = head[1] + d * ( u[1] - w[1] ); - z = head[2] + d * ( u[2] - w[2] ); - glBegin( GL_LINE_STRIP ); - glVertex3fv( head ); - glVertex3f( x, y, z ); - glEnd(); - x = head[0] + d * ( -u[0] - w[0] ); - y = head[1] + d * ( -u[1] - w[1] ); - z = head[2] + d * ( -u[2] - w[2] ); - glBegin( GL_LINE_STRIP ); - glVertex3fv( head ); - glVertex3f( x, y, z ); - glEnd(); - } - - if( axis != Z ) - { - cross( w, azz, v ); - (void) unit( v, v ); - cross( v, w, u ); - x = head[0] + d * ( u[0] - w[0] ); - y = head[1] + d * ( u[1] - w[1] ); - z = head[2] + d * ( u[2] - w[2] ); - glBegin( GL_LINE_STRIP ); - glVertex3fv( head ); - glVertex3f( x, y, z ); - glEnd(); - x = head[0] + d * ( -u[0] - w[0] ); - y = head[1] + d * ( -u[1] - w[1] ); - z = head[2] + d * ( -u[2] - w[2] ); - glBegin( GL_LINE_STRIP ); - glVertex3fv( head ); - glVertex3f( x, y, z ); - glEnd(); - } - - - /* done: */ - -} float dot( float v1[3], float v2[3] ) { diff --git a/examples/ThirdPartyLibs/BussIK/Tree.cpp b/examples/ThirdPartyLibs/BussIK/Tree.cpp index a82e7f1d7..6891f0601 100644 --- a/examples/ThirdPartyLibs/BussIK/Tree.cpp +++ b/examples/ThirdPartyLibs/BussIK/Tree.cpp @@ -8,7 +8,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. diff --git a/examples/ThirdPartyLibs/BussIK/Tree.h b/examples/ThirdPartyLibs/BussIK/Tree.h index 208176817..673f41c9e 100644 --- a/examples/ThirdPartyLibs/BussIK/Tree.h +++ b/examples/ThirdPartyLibs/BussIK/Tree.h @@ -8,7 +8,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. From a6216f4f24693fdd4e178b92db9af223e9771fe1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 25 Jul 2016 11:48:44 -0700 Subject: [PATCH 06/36] add robotics learning grasp contact example add wsg50 gripper with modified r2d2 gripper tip expose a fudge factor to scale inertia, to make grasping more stable (until we have better grasping contact model/implementation) --- data/cube_small.urdf | 33 ++ data/gripper/meshes/GUIDE_WSG50_110.stl | Bin 0 -> 23884 bytes data/gripper/meshes/WSG-FMF.stl | Bin 0 -> 108184 bytes data/gripper/meshes/WSG50_110.stl | Bin 0 -> 20684 bytes data/gripper/meshes/l_gripper_tip_scaled.stl | Bin 0 -> 49984 bytes data/gripper/wsg50_with_r2d2_gripper.sdf | 298 ++++++++++++++++++ examples/ExampleBrowser/ExampleEntries.cpp | 1 + .../ExampleBrowser/OpenGLExampleBrowser.cpp | 2 + .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 7 + .../Importers/ImportURDFDemo/URDFJointTypes.h | 5 +- .../Importers/ImportURDFDemo/UrdfParser.cpp | 23 ++ .../RoboticsLearning/R2D2GraspExample.cpp | 235 ++++++++++---- examples/RoboticsLearning/R2D2GraspExample.h | 1 + examples/RoboticsLearning/b3RobotSimAPI.cpp | 2 + examples/RoboticsLearning/b3RobotSimAPI.h | 2 +- .../PhysicsServerCommandProcessor.cpp | 21 +- 16 files changed, 558 insertions(+), 72 deletions(-) create mode 100644 data/cube_small.urdf create mode 100644 data/gripper/meshes/GUIDE_WSG50_110.stl create mode 100644 data/gripper/meshes/WSG-FMF.stl create mode 100644 data/gripper/meshes/WSG50_110.stl create mode 100644 data/gripper/meshes/l_gripper_tip_scaled.stl create mode 100644 data/gripper/wsg50_with_r2d2_gripper.sdf diff --git a/data/cube_small.urdf b/data/cube_small.urdf new file mode 100644 index 000000000..804f7f0b1 --- /dev/null +++ b/data/cube_small.urdf @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/gripper/meshes/GUIDE_WSG50_110.stl b/data/gripper/meshes/GUIDE_WSG50_110.stl new file mode 100644 index 0000000000000000000000000000000000000000..a7b584732a514acba85785903075f20c46da1634 GIT binary patch literal 23884 zcmb_jU8rSOk=~>6=Rx8Xqo58(5JPZYTC^V&yr+)?cGTcgJNPsh0u7^xjSL3K%LW@! zL})?gDTy@0yr`3r@i%JjIa~YzHR?mu2ZIk0oEa1qe-H$jsjq5%tG>1NUN?D|OVX*F zs;|Das%q8VtM2K4`1{MR`2YNQ;%_GVnRoaTKUc5*^6;AHzHIr%55Djw$d~?l`||R8 zzvS{X-MRenjbD5d>?&f06*WC#dBz1tHwfoY1g(Jn;h$gU+zrAxsDYItUiALcC+>$m(KYa^2;*?{n!{?hB4|~1?%WN6`>3l@gzag++PUw;c3u&*0*6I?-(=9q zR0CNN7U}&egU&%;K+p=Z)!Tl%CsNZRtj3-Of>uC-d%Gu611m*fh1)$*5wrptG2HG9 zYK2&_Cyo)+z^@{Z&8A-=A2owu2ZC0yr#ttX$K6L#I8ap4Q>mQH$ETP;WPJu{GBBtqhgeRWb%CSPwirk4fdDg|lPj9)nYc=Zk zc(jvNpXbvwLeL7HD91h?vR(h_&G$J+YSi!X$hVL45wrpa<%&2ZM{3mX@o0IHeb}lt z5wrpa<=}Y9vo7u)E3rzA`aK@4-XtF*1g*e9IXM2?;`G0^vR0#hk4L@-pO2swJW*~C zUWutuzrn{dI89|$D+JaKE3P&XKd?Bx)M~5MsNaYi-V9<@BLpJN_Xo;>_}caBr$4k) z1&-9H-^f4TGNx&SK*hico+vxVY=|>*q(=Ql9p^qq2vk1zK{+_SdCSGaD@&|WqkiK& zW^_jg_7&`da&U~gPg-eT0mgEEuuAltTb@ft_A2py4i2lH+S@C_Yd1BCK)-^g>^_BK zg`gEUDEmGpJ5dLo!PKZ<&wqW^PD6xMj9Y<&a&TzBN{#yU{MUDeX&ND{V(Qxf<;tPm zr}$F8o`2a#*RLwVDyHs(a&TCu-m$l*M*VtSNj^phs}i>=btMR^tDQa7JcG0<&!uAJ zbtP3YLge2frSA^R2O;II?5;~7ki`i;2dIUgYqX*uUE2Zwea zS|L^{qe`~>)s_|V(IEPMReVKVh2N;-+(*~1Dgu?yeNgs&%+{$7?Cq&hzi}QjA0q@# zF!MnCqsIc4XVeqo4vp1Ai^H4)Lc#Y47x zNu3%@Lj1K%I{g?UXa!G{V;}Fid zw);+<;!M*BK`U@j4vzS9b`EOPFJyb)6^;>tR^Xr|>M%J7t3bcCQ4I4B2) z_FZbgp|aImov^fhcgqSKEy8N|jQxrlaHwqcRwqT%G(yk{9F$`p@n`g1YQUkg)thjP z5VQgZ<*>C%JmvF)8gQs=^(L_zA!r2-%E6(1ml|-WY&9l1A0cQ34$8ryeU}<=sLc80 z`3PEpgK{9+z8f=EaL|f;c?L%atcrb?a&TxDrAGZiM%=QOj1aV9J`zzf=>3XT>K8Kd zpFMGepcObM$3EiE=)2UYU&yHAz+592}!AN-J<^7oF|@0@+Rh?all7&t9Hf@H;`jSuAzxOoYKy(<1)m z=8KkJ|MO?#_k%>tJtD1Q5>lti_mGUBBtqg$NGC5#>#iD zh_s4*a9+}KU>~7DtmyZ61mA1FP=4=Bj=FOq(n|AhX=o63+p4kp&F|h)V`aBKpJkcb z4w0JUQ4#O{$2TmueeBC^UHPgMkyep^-h|s#p{{tB?Ge-Tl?@I>Kn|-o1-uEj9Cq6- zJ7guoZ`ASjJ`{m!4=b&)K31_3oP*;L`-pRQUbQ1uah_WQA{}SGaLmXI{3;@?Y*jL+ zZg-xV$RK;k&39f|n!1{u0;j0@Iv&$X=gIx5bn2{#X?mySrhNCKsYFGjRd938;QMG+ z6&hraeq)8(=Ul5Ds|qWvW67ZLID4IPS#im?3exBIRl(#or{ z%m8~1tE)O4>=DDhTMTe=Y0sz{ z-JAjTh}0OLAgmMZhyAK%Fs-7lhJCkA2YbY@?^ZX?o$=z!+QEvQSu_C(J zxyOo{9%0d4Zannp28SYOwYjPpVTQ(TsUF4Ph?_TLuC`f~$BLRB!8>(DR1QVZD$o&y zih$r88XraQPF)f67J^oV!=6588?iDD*T6~uCVP49>2!!@u{L_Db<$>`4HP%GZHfnFg1C^^S_ zLUf!K5%Wvf_4!ByZ}G{o)vv6U{P#|PXKlU zMZ}Y6t1~EqR#+kW{B~VY6Z?pcQ{?@2_vsLB1q5r_Lo_{l+x>?pzjgTxw<3GfEAzG! zi&M_8QU<0VPJu{`B2E@~;v4c9kyh=DY?}5zyK-1wf2&o!YSiz^VsGSi!ZJprmA}0T z;>)kU_3)OTU2%@IQokpQ-9^d_2sN*SvvJGaiP(=+uOaf^x0c?d+BbDxxfC^>hI)TrOcKW6}A1S*EJ*Ob9g zD$zMoqkdV(F+(0BqVi*wk+M$K1yVffLMqqzsPI8Jr_E>NomI=3|6F zCt^NQ21n_M&XF4R8~risYJ|u>uRR|eqo+Y45H~&<41p7MuoLCAYc*OXf?n=*U4SwSRug2$z96eu=8x~Xc0P9Yw&azVR_YgV%gLR;`)nf83i;ST_=!r5`VGE3=VetZ1XjrBN6Hc_?}1R^c{s>t9d8FWNiso%(d_T3Qz6(fDOPMzDQL0YL_*74T!LlIHO_4#3^ zpdk9^N71O?IFGrH?)gy>IFH;%$`-1*bOw)AYSeG^mCVNoflkDHqzsPI6P+V9>Nomh z*3}4+eO`YH1dh?uAQ6Zgd%hv2KiIdt@h{|Qx@NID*o_xfl=1ny?DNHM!aaf-ML_1~ z=KGNTZxqcZ%opWj%pvETA>IJvP9AbhykM8p32Nd-*Q1H} zr|(=o{j@!A^9~rZNlrZB6Vn~e6Q^l}pjEjK3gUy-so!r;&t5|WIBXJ9=hV|QLeL7H zC<8rB7p+r2ZqLGAqgEf5dHWbaD|n(D9C*`z+}=xiO>2cnGrA+hIyXcF6Mx@%!FTjSKmL|F4-^1?Y>*k^waP55oliNKQ+{i^)Umv{n%pOkG+b>1FRgDnKDN~{tw zm%i)ypcOb&w%jb4^AUnp;GpcQoy*gJIj_Fd%4)3sHlsd2RtQ>wgK}_`r*sS|Dk3#4x_n;VWXjXV-(-vf?>Ln4_Hx5M1E0M66ohKF5PbJ(-%n0YHuvgA^FJY+D%WZ#CW1A z+f~^uwboT;&2Pn<-$PanR+W5to9ts{w{7_y3+f7413IT_?W$4(t3>4OwdYVoTItQZ zM<70|5=4PH$GyGzHdPe6tv5As9*MyIIALk`VYMXDy=Hrf{9A>V-1MtT4R)T0=vP}A zY!TXt?lccu=ffgeG~h@?R!mt{tt;J!RePDx>k9jQ=-OKZp6!EbH%-LH&P1^y$KceO z75h~pz_-eW?Zr<{(RgR5Igiyo6$hfgoFnS|omZRl^yQti)>uS+{r3&9!|b(&-g8f= z;}wVRBQ+kMib(E0gB_yAEo-{ZU}`i6zFI@eJ~q!s?lzuRt9=+sPiH%a%hTMiDm5#_ zrBz*dtQ28#v5G#nsuEvSo@eZ~L0o!j1ZIn2r5(c@<7c*i%Dd0CnuE-_zHzv{{{e(* zJRke0692r&1yMK@p;kAH7iT`Ioh;x`%?eRDwAzWNv!&wKKQ{zEZ!@SGqOlTwURG81 z0Q8CZ#F>a^YRde4jNlG?bgT-#v1|NvzMKzM8ilYVqSt^!5o->8D-N0Q>EDo@V+LPp z)(E0CA1z1!M&2OYKi}>{)4ufsO$63Lp0HXY;%=})I8XXgj`lXe$bu{8ZM>7|T`f9X zM7Kq#rbXm!a*T*4&P}h^@D%m7g6WBX{1;8FaN30f7Bnt8us6olHC~ zSdI1HtO-PFc)N?w)6*{nMc5<#dH&Obd> z#0-dP6ruH25dbs@waOmT??W|;fXsP7%Mm-T=fk4F$!$ht3&C!gl@Yh1IFC<-2rJD~ zkKipO&S1`4v8q^Yk4O!oO9Zmn)Rjjz{z!ycVO6Z-$}t0BR<%ZrLw+B@H}XCoTB9xE zg4MKqXWRXQLlJS#`P-9>NL^I`>WvQnn|@8USP4h5#(&b6T4~&1#pj1VIc>doI#Wje zPwX?CO+K{?Vs15xfL9<`J;~Agq7^@9g{~3Q5J8Uexvj?E;STs*&v~9lz8U1IV2Rzg znnbkEc36``5o&eAc8#d7IaISkG_i6TTa_b@$B7|@*d#ytWDJs=dq7W&$L_%(-C{j^T;*k_0(Y@rH zefHYBi_}BOwNz3rT~KsWB;WC#bB;a7TxV~e-ygs8dOc6`wa0jmG3S_Ljy2|(b1m1k zy{zv4pMOJ+_2{oC#Q&kSde!da$FD31N7w$P4cf0Bu9>|3?VnZqjB8egORwGA27C&E zhC(nUis-twe)5Zp1)?Yug1LZ(q^lS2%b-JYp`G*5zI3lLBUl#XN(9C%g2t77)yZHm_zS8tYvBNlFv}uPlE@WaMg{vn(K(3vG@P5bZ_=IR}`*#zS;m z$vz)Ufn14zk1ea1$yO4>2}w{mS#|rehR-=cwc$5-!CefQBTFq8{rggJ3S!Z4vjK(K)cM{=fq}=7^-wwij<4FvCmr1%Z6YsJ+l%qs?!Djpjb@lU@q+? z%vIOP89Xl=Lgo+L*5jzY<#vCczeah9rAWDToovbTvLR$v!)>Lbh@v$;d2J4-^C&4- zw>j$rZ*~N@U@PcIBZ^F~)QYkcDOZmP87JhY$593$pFZ4HfTD$$U*I?PV?MN7ij=G8 zoS(IxmklAGqTJRxE^j?2R)54$rNdIBT(o@gBvi6Gqlh zIxI!X)v=2+GSAC~;1~>BaodbMOQg13G6w(Ab7`Veg&X{J6+TldNWPTwUdY zz#D*^VK*V#6XF*_NDA5==hY4pbMy?dB+pw#h%2Tw@n7C>c8LBmMIz9i*syZ2`%h#5 z_w-ttcH}(r5q%5>YcYMUZ@D80* zTf}NYl<#|8?8gmvs9rHeBG69$lj9e(L{43edR27mb+O+vAealivf0wrgxJ&Py4Yug zkh0J|`-wiWPDkrrSwt^Fl>cXH>F{QQRj-&L5oj+j*E?}rb@XaBA!htLwe+?O2SaJM7kb6}1kbyZ5FN_Dn%L9o7S$`}f?Ur_h99lgy|RdF3Gvp2l#^VYuX@E4 zi9maQVt4q!P4MM|?&rJ4PgeZCjA&w`+lNk`qgW_>5n>!6 zq%5?L`m!K-(i!^cY!Lw=wtO>K?FpqUiQu;0ZTtTR0$eN!^%>8T2q}x(KW~pBE+idS z(>NYH{HV$RxN;*AXu}GxhNv^NB19)b%*lXYE}lVM_sJ)OnU9#1g*Ikz$Shs`z&vS$ zm~&ssk_fcnF;8EQUC|bbaP7#iu6ea$83c1d53!+^QIGG9Az2rWI!+% zdc`|$&+AHvMx5B1@od#Arbqy|Oic=Cxi)ygYTVfA7!N=R(%*!+!8l`F(BB_PikA2*-6T9ewIG9_U{(tE!6a zOpyq*bK-fybG1+lhhLpbh|@A4mppAUDZBJd%v9{_!h}NWIYlY8Lq-Kglu#R%m zP}xRC79+%p3<&fDTs(4JLq^s&gb?)&0^A=o5Z33K^^1<3!cM0o|D@wrs!!;Zm8HDIaLH;*REMm|Sh;E(N$vSZLeOXpm-L<} zLe^v?g6tM%wys22uDVIf5IQu)ML=KfYo<<}6=At# z-7nfJSXZA;)=f$TtPu7y6?#X9L|86a_lxR~ekBo@&GjFf(>_OsL|88AV^JM4eh?zi za*q^C5EFHkT*oU$P!EGLBQL^CNrdInD-GwLaB?^9Vhw7FUUyoqvb0}j29*wpp!qOd zvdSIRfmovJ>Qb)q7+?FCKqws&fmp(xXgyRQXiRSXTOzKRkUDSmD)_NLC>>T--K6D` ze*E7tU&bf>^>pcVO~-|J-f_jFZ{HV^mrR+PxZ~Je>PgLY4bS^&eYgBd-*!zj=yk6` zFhwHJZh6gsu+im+?Guhl#^-t^i3$Y+%OIEwy18QEc^_<<6d#kfCUL@s+ZBQ-QZCwW z&v-bzdoXlVZ~9RD-(yP>pI@SNNCb012UkL1E6U2KI-j>i>tKqMi#Gobe$V@LcAcb&5E0w9 z+fHs6JaZd#wD@gl{Hdq=`WKw0bw~trK?hfEJ@1y+zRbJj_3nNns)tL8l#BK!UC#*4 zABpTN9C1$k>Vv`AA7!W@;H|C#w*#!#06p29l_XCxK5B6dP z=Pe!(Z#F;AKkp2!Ln4?9I=I$L^{amK|MtVN3+8AY z62V;1!PQ~UE2-Kt-z!)XYqMFeD5glcXw#bab!4b~)f4e^*7c6PI(eiTheR+JbZ~W; zD)i3{h~NEH*I2KfI{Gk0%0>IQHNWMg z_%u2}ZrMlcV2YHBHpRyn;-lvs_fq@(#YfF7-Pu-0QHfwK=-}w;d4tbh8b>_2X; ztK>olNm4QR$Oz^#y)uX|MtvFINvrm~YFlE^&igZPVbz9%e@jOOz4Ll&MYsI56d86l zo1+lC+mZ;h5m#>Xku#j%GZCL#X^dZW+-YSH%%yavcU~JWofzNr&{+SU9h(XyMao6H z_E7~vOR6n;-WxL}#si9Q*G|wncz-6jAW81kyrSe2WBq}D9&!qTwQ^)#uR^C`D zDH4J9P_H2Pn5x5`cO@adCB&Kx2<8HUpA62^*@q6U)`1I8=e2MCna%)Elk>dNkGkd8 z*xog9xaYkJ!PPp6KpXkPQ~QBvU-V^sK)IQvZJLfNgJ3S`=9--2?FzvZDHm;czSeQ$ z&r9PU9D2I+#9XaIBA5$0sI97ao_EUhFY_=f6aUo+rbxL)Urg)h`^Y)*@n7_gEqZ*U z>Xk%bor4QHxIX83TT2GSmww$f*2UP0DN-)lh(rTFgRL(5bU^${idXfgXdM#4T+qRF zKf3*J>-_kz9bIF?Zq)O^6e$;NL{!YC=RL4|bN(m#`Ju)k5zGZ0T=%1MoB`Y7L(X3l zyZFP~6@n>JF4}l9pq}h`uPogjpR@CD?B!>=mq9QWba186^JX5`GQZb7HT*@Vc2)?c zNV#a^2{&>Wc3$nTcp`q%b9sJ;(=IH7U@qw3N|=70|I<3=*SqWjg6?Hrz)?U0OQFCKi1akqwQQPzG|Kixv9ldLWyAE9$g2Fop zv|(LjlRSesiS@kS)^*G8O*5F&Ugx5i4^SimZSt|C&Z%?8ei=WXyre-+wNT2ET=<<& zVmou*NTs;?gd#!y-yQZMf(|~rB~P5M_68yE z4VWV3qD|hO)O(z_?ms6UJacz=`43Meq%6sW-}&s;^S*z2K>U~?L&M$|>3tMaBm(W> zV=hj9Qh@sI$Rz{f>t@~^*4n9cNCb012cP!REx^0x$G_}9G#uMl>tKqMi#9#ali2Bb z-nVaW&Y%CoxDXyGWl1jnUDp;Lylq>2%eb-ON&jw8Sq5}~A`xg)zM_7oUGT!~@!>mN z9gJWu)`9b3&wIFD%lx0Fj}70ix>03)&;g2+%ldR?`O_s�NcfcUWQjQ!2tq1V#%k z=-~5V&pU_q2FKETbh=1quS}70(T1g)y^HL%>zf1OYiK?i?bJFXg1MlBPn12cpyNsL zb;E{+OX!ZGq)54F!>^EEd*0LUwa;%@bWPZ#nU10?OL9RHM_11~bneo4i-q08<|ei? zMIz9K=Qq0<&yQ~jL9spL*e(&w1q4>De&VFoS1-Qfq#k8obrtdh97d?;^yKmU|E8o>8GiIQQrl4#MS< zQ{fBV!6 zPtS8lF2{8A0~Z8u-8IGEIv;z^zPlIawI2LKqRXkb`Yy#<0cd+p(c}IHO4@ugA#qyn z@G=PI`lwUm;Lm@1`vV??j_!o0`R#0J8SbcG_ITY zc++U#rC0=d^~!~O^3&v*StUbS+>rMpW#Lf-bJai4G8j~2X{*KobTlQz4(K0wbwKB0$A=WP3koRfhy0H}X%B5I@v6VT^Dj4!`$)UQFVpCS$QwBk{Qe4HA z8V5)3>>X}Vw#p^MC-o-91{1=i=z1&8po%{}KWM+?vGAEi7{}iiyqQ;zR#Ps`pi8j` zGymrN>qGR=7V(%})(^NfiM)z}z_mlib?)pLbE}z}} zwYk5)vS*WIky-P!9?c2OlCSF))h&yi@?%!9ladPy@c;eKimDK&) zdBty4XyF%aI5GI-<-qgRWeJfv5B*Go} zRU_LMbpH4Tf77289E9ZR^`9^NA9hqv4tW|ne!lkM;`6CTFH`SaidrAWqVfdmt=f+j zpSbL1|D7Fw#i9td4Q{dS{jJ6oP!Z{F9@ArZkt zSNOG7HcOr}73gDrI zKkI)@lgBT{Q(yN=pENaJeRJ;5{fGM<298fiibPylshxkq`sT?AR|4I6>*lKJ;mxfD5TdUfe@yx6*z^RNG>zkf!96=e`uwcs+(+T_FML&t|Zk1N^#spp@X*F7{@ zM)>BEKRC+>FMA5RMja*e`CzMqjq=WUxrM*x#^#h)TZ+jt6vAW!$=TzOk@cB-Fn0kI zJb!hrJ961WlS_p4x?#`R#6Jj+9;Zm$zlNluLN~+q@5|w4dD9Z?4ibbGTr%&XQ(GHK#Dw#I% zx{^Krjw^#;E|W{Vvo?edAQ~jEFQF_WbSYM@$t7avD}>215|m|xF2y2@t;}h!$ubg@ zWt2f6BZEavF5#c0Y-O^H1Z5eaOHm`&2%Tm4y|2eO4qo_XUb(w|NR*=)G+BnBScI8> zbI#qJvWyoyl@AwmyQvI*(ES*%OmX1m6wE z`uLl&jN4yp8g6diDwGt7a7TXl-u6^TzajkT&k7Dga+%B^n0gy@tfMSr&e1Kxd|Kx& zMXitY%>i1?X5?>DmXW*c=J1>yet0B*Sk{??+Agmjdst89}hM`!C zy0$V|Mo3vkzKO`eyL1QAWB|dK*7}KK?`xPWBcv=t%?DGW2%cH0&`(~Q-@D1Uu%P}d zenzSL9ui^lhhWx!p)cP5{QOsTjt={csqJ`)T-M*xSw=`%hND9wO#TpjH5h1e+Wjs2J~xtHs^it@~E)yIe893a+zEr_~>gO&fC5@ zZvi1LJTEUMDJEwKrnl6YC1z0H3S0Ezg8X|HjSUxdSj~B&qDX|v27-Z$fi_u2NLhx{ zE0;^<5`nHQnJgouEF&KyXTOpNlRpGUt9y$k%LpmUa1fG9`n2brU2#-+-_MO=9XGC| zam=On2hR;EzK7L0mu?H?1%2pUfOkgEO-`J1aWHH$R#bgL{8rA7ofnKKa49Zg(xkS* zxK8kr_T##SZC6zEf4`?mOd`-`uC3oZmwa`EA3Sv_5Py#C8g@Kb#UJ z%2sS2*y@aH+6N1|{;F)nW1<__gs2w3EH-T4!hG5HC}Q2t%S^BSM7# z>rB?IO4fa8-$Kg5=i1U>4bRYlKrt8-<^_>lqjN2_u9dv+pycd;hok1htOzXV+ zrj_)>(JOTpy{UyZ*8Jc3dz7uNSY9tE*A+Ud!*>g-$LijCj2gM6xQO}hRtTnagO10^ zcb7Ir~%a2(NCh+4`{^`SZsrgv9^F#l*8R%7OLX5b1Ucpf#S}N;Wii?<9bGUy(RUr10 zC;rx^U#!<1_7wD-S6Nw?DN)46SLelM{PYv) zcywu%a8Z}0v2u@faS)KpT(H9DJ)Vm#_Ry=5gz)IbNO7xcm35ht0g=#%e~%dzHtIF6 zpwH-*%2sS2*lOyzWBiYQ{7Ko0$3)pHAxbKoS1{?GOO&lF#YKG5U}5ahk1&oknplz&>(AFJBh=qshmsT90=YuS2C@zAm z9%|pMNS=7!fIX!{$rGh4w3!Q5AP)_-?;a#?f4#6@Vk~*Pq(l)EB|;rb22x}=Zt&7X z2}K5pK%2Q>1&SD^-lv6?wbtdlW&dInD{{qMax?ouoX z5o=*xior?5U|4~kS4sU;yMPd1`_&W2_twurOK}nOd`#*m=Pt7Dj>hW~&zzv2gN%S& z=7JUI*`CxHz;}cg({6vF&wDy&U`iB0Sw>Q48Czbi5>{K-DjeEU=M0R1T;_rmDA!8r zjI0jLN4=FD!*M2uV@ec3Szl7;yJykJd;i=hv3{e@87NOkB2R#=DECbwe?Si@4@&B6 z>0CmbR^Cs19Mk!grML*nkdr#^szxI}d1b}0*$ACqF#>X#3q7Q~G^w-KhY7LkV3lyK z$#Ya9p z9jTFFw{mM81klU{-{sSHs&bO=&e=3De5m<EwK?`zqL zKr@%@A>B5Fw_mnDk^lY_HF6$p6oJ(|{ArBinDpt}`Hhptb<(BCeq304pFq{`(R}9uQ(vta{?fp2x&oii<#W z9Wo!|XgP+~c=Pp%=_iH`0%+!f74W=z?_nUejqVyY+_W(9_|e}h>oO&Zz|*ctCsc&* zCr`Zlg#(HA#x?Y%EVP*mRzS|MzZDQY2{CZU(!_ZmwNch(N)&+{EWG$VK9Do)m<;V|HxXW3W51dYA<@# zQP#B-7lAzZ?u+*mVg*HpRrl;EJ$*rC9~NZ<+ROzj&@Ll!M|B{~{==jFhq5kHG9Y3a zVfG&$?LQoZk*m0j6=K@DV`%^J+OT?2 z*ew(pBqfTVh#Kg)yPl$G&u32d@1!Uy5oj})iLU0(_E8k66VK)PQ;y5wxT`2p1mCUp zJnWt8_MTTzf3)^p?3dxMm=Elu;m5E7t$ANZhNEacn$+4|P-~2iC6?kMD8>Xj`s|`8 zn)~^}*pj2ZSKiJD$Ym~Afud`m<8CvG)H9l|kDWk~T2i72dcpfDPq?Ub;B2YWiPqkmj6Md?Y<(#0OQEVTenM?N2^R^SBpxyph@;#k1@Mxn5 z$|VAwUwuxV_|prm{9|e4QWo0G1zXvXo4tWgdjrm|lue=tRD^ZDdmUx3qp7~ztHpmE z1mrRod>8M^=sT(XO4}tKEE(w!Uc0V9Qlbd^?ovatZg;Bh4m3F&*Vy&k=vptVz;$uY zGx=_;#jgwFE=A?L0rms`

;6df_Qf9zR25;CCAnY)u8Y%6azY%vvZCL3 zWRrxXL=mvG-m9HO`R*wzJNoa^&PyWLy5QpZ(0jE_G=n>622X9ZK~>K&Q=mi<@M*nw ze(lY6iJz%@{>)hi9R%bu7p!ip=c|jUW;y$0fAR&FtGW-}x>s*6a>T*?cAyc7bS>jq zLR>XB*Z;}x&lN=?(B?Na>FY^U({D!c>a`3A=8`?6w*g595Pw(FH2~RnMnDJc^v!#z zQ^*sar;)#GMlKP|1%w?rPOz$!_hY&K2vrM*U-3Q)XF6hEYfb^M6TY3Q=gVj(jCrQ) z-cW2M9lv7*>l(l|@{%sJYUk08ULqtHW)rJf*8uhq;&9{jv9VM&kd!C_kx$oT?jyfC zZ)k0Q9#vQ*0&V7UXYfHnq+U1>TSHYGNr@uxRMQo&8{Q_rA}?7@ULp}_GZ(DO-s5>E z6XFM|p6}|f>xoQ>BDhAL&Ua&!?^0%FPZv0fikgf$X)yV20p+{Ox`?8>TB9R1Bk&~B zHL~Gk-HCM4@CQ}AU{OY(&0Meop6$9iIDioMT-q--o2qJ(5=9_0&=ttnDSI7ECk+>V zsOyO=3uxwo6_73IYV8+-Lz_B<v1=ifh)GP2`WdPCc9uH1MK}sIu)+${9^ZQXiqk^EHA0H8-990cQ z+E}6}5`i|qY2|sNsjBw%?32T{Ga#5t_K@zwQS^b1&>&>r837%5$4tMgUy%^oY2@|o zX`ix*{A%Q>yGyswX`f4R5fqh7 z)frXJTd6|-fr(cv3uxxTY*L&z6?&Y$e$-@rqB)(uN=g(#PaIQq{++yJ-(9uCN9Yt; zBG6_oSeKrtrb2%WAx12qs`JG+p1QfNqQ2`(ekhhsk#t0lCcO&ftG3FS(p5^o8aw4$o~AVJq}M zke7T%_4XUcOQbBcnG4pXIwJiKH|Ta6L>H>iOG*@BEA$tUb?Z$wU1D}{RFfBU|B#j7p%bN6c(|^+>K&N6ah=? z3jMic-C1PaMdof4BiOp&VjJq+-7?yL^ri~^VzVE_e1H-~U=`@Sb2nNa@6q~r%P!61-~%G-8b2(c{EqF;-NagKE#o6p28a-`b-( zT(Vu_If_0#>Xk(>m+YbEHKbkYn}m4VAY|Ve0Uh=o#epU55+NrTyc&^^wT>TH5f7orG623=<``;P?eKVt9Y5BAAeXsdU1R{}q~Q&E z;>4-`)j?$w$%VEZIVyM4&fHzvi7I#S3_dGBn`_oOO7JNFs)J2?%`5Fs z)j^lyBDfmuc{$`IKVI4|QEotaRi{Q?g1RBl%!S!R_Nr^IrxW7p;d@HYplYq8L=jxS zrpi23og2_8z;>!SO9a}?1?ys8qU+S=PLW4L(e;%#_O{c&ZfgBzAEiz+BtmkT*)*pa zKr9%xr=SjGU-C>iGk)j$`y$D$4?zxnKo~`R2rA96fR7-c{T0u~?sN zGbM_k%)p!i+)4Y|8C0|UoU#ll3vK4YeAp9{+vs_IGgY>SP$mNkSFq8hr@s2~7toX$ zm~(?GXq}H6bA!LDw$3t`A`xiwmts6`1kL&JgxGAe3?)l)$sXdXL+uh@QI&I>&1BRF zB?4_!JoQ%!_7eglUuZ@yN6TD5*pc5%h{-ha<7rm}3Py|Xd!P*~;2Rm}73Fq;-g$+3 zujbFRS-7G|1ls)dEE+i>eoT$@dt^W`7j)Qr`pERR6Qa36Fh$Bmn=<`CXZj;nT$XtC zlu>@Oo;`D=EXjqo{eoHHeSYGXQ||SfpSfP`4VVHhK97)+=nd%?FG(!^a-?6eX8OL5 z(jG;?RywCXM`z*wczdTvA=siKzcz0{yhB;|ZFFw%h`lGHC{YAX8uZD}Y>J6(Kdk7_ z8=k(4qYzjh%!Tzx*`Rrw12>!OuCUGDNzI}+d8wvO^*2PMzIg+CWl0zZMk$6 zSHDSun;gqx)nh5T$${qw?wgDI9!9P`WAo@weuXv&o>+Ej;n(VGfk9C2;ZsxR?#FG@ zAXw71W@_?%Xdek-xfG%>GlIAbMam6=*s|kO^+$c5L5D<0)OCf^H2;wpnDK`lIt?)|nt-`Gt zbV!8dQivckg18Jt$_;|;lZ%qESGHu(ArY2KA%e^Z;xZH|HwdmjvsUt@*Fc1<|u#7sIQ!g48uuMuPD?dUUqTv&V^eWO8sNkx9S1?$7En`Xzf z@F!oksCe#CEu62BxL*yCh>dfa`xR==EB@dPeCx$hB*OiI$Ya!-KhIoLd&794z{91xL=L=i4e`}Eh@%;r7X$iegmc|$=-DGqT=@nA^Yoo z%fyb4z8p!M6-Y;o$jBwa{RYT~#QFS+g~gRkuh>?SOMY{N-WVaykb3ns{TJ`izFW6W zVWXdZZezYCpuT2w+8yit#sg{<-n$Gpft!$yRt**v&nJYWNQC>fo?nRb$&(fpe-}YW zu04;{3WlG1M&Zaa@kY!W#5rl(!s0rlOHw34TAlv&Izp5?dr|QZ^j}Gl2>CrB`XUg$ zA>DM_i^cuRy9mi8R~mikmU=UhY=v(jVYJ?fby9C{`ngT)?N9s;n`@;GmF!V=v)?sY zcd)r%?$F}mf2(K&QzYU)-Picftge;n^%ktVfn=XwcTw@bMh7D#mw6A`e7WTyagHD_ z*+l=96p4UE*ca&E)T416`~1S<1`&khlHZx3H&F?({isF7uM@(hSh=oWHMnq)-|d_y z^9wt?osi%9Fg})ASN*p(^6MV-F7od!{jW~FJ-+vF(@;_*;_sKn`>$VEC$;Zr+#{W_ zv!kEeDOIwn=9vyca$W7;;6FN{PO63a7rHw?+}(d*^&fcy+IXStmHd8*)JI=Coqwr+ zxcZFT6>pvuN{U3tZ>M-(Z$cb8X-4k82tsl}5`8Poe8uCekB0cI#{?xEx<8-DJHNo+ zlc)$7eEM>Z|MadpsTt4hYa_oi;(48Z9_`<@@T}aEYSd0hibVW+PhEe)p*pEs zTcH--W$!q@%I00mTeN&5E)kMTewl=#&w+9N3HQF)s{4zb^Cd+h2Cl8^e}1q|s+_8~ zcOV`Ap73U?A9}tKmk7z_e(_}PzMlTQjelDH`CEGva;7esANbcVtCL#acCe zf9l_lwtjv1^Ul{{BtqJd?$8$v@=ty2mDVS0eA7WlF8QSx&wKrfQT}y@p2#cLv~5CC zBtqJd{+*v@xB08rRd4y-#xG;?3qA5%KYiXT@b7%9PU`E=aDRWtp1b{JD@$5muy=Y) zQY1otPl&$7I%2RteXds$pZ$u1kX)xXyuhEdwNC1K_4fLT=g0WfhWokmHysy~6p4^u zIifSKRipeilOHWk7PoQ`l51rDdj9c$*Gawf9Nuxil;-^SkBV~lcX=Z&DH7q%IsEFh zdb?W17k18<2+7s|+q(V}=#U8c?Iq7U z!E5LLFl}tfbC;auAS74+$8Ppt?_VeNdM9{Ez2>+1@80@Y?p5o)j7f?_NPqLZO1Jj# zgI_C_#AY6H5Rz+arAz(p^Xb|3B6OTQY_R{~l}%f>f8~{!q)3GH(2P5;)H&+Q)#_0MZ61a@?SD~-TzY4d~w8FWa5<0MZ61aT!bMepdijos3@ z-*|1y&WA);F1-uOj3BP0rsy3Vx3OF5xu{$Q9TH)=^e!wjg1C~JqIY!MR=cI9%w6=m)n~BX*2ceuHzQv z{!P7-_K{y2lf9*Hi;%5OZM-OVa%3D5;eLDTa?0>eN-WHMsG>V^$tA5$-y9$vv{%ce zy_)mgDEEs{(pGtdn0fr7+!>}rV&+MC&{<@CTz63} z?I0sx<+5^JzuG{1wX+&L8Gqx_w`1~)Nw6E&1LQZ7(tEXh+N=4JA`xa6R#^2QzB%&e zj*kBRYg2i%Pd(E?NG`JjDExQ`@{+eEclV$D@Q;!aZ9QN1N`3`O>Pzp{ifON=@+A1M4|+b_IVE4;nOUf!#juRxi- zS~2a_VrJK%C}yvw5N1zRIQv87yJoM}n)Yh>5+S+d_nmZxoEY-vJ1;Nj%sa0D--nV2 zlOY%C6BF#!=AQB9J3sb(gLiidA-PO`t-gguZ(97?)9-NZPpv^A@9gH4w$y8p|B$#K<}(c~A?Ouk!q)`hsEGn4jepT1YJ z{H$N6CnQB8%$~}8yY2nqgZ&Q{czGW``-+2*TxLgBxLAG9bRF&0rjPQA3pXB@kQ9lK z--@H}m(X6V(xgXoPg>r}K}asMzbd@82WkLjuU1TZHQsqCibS|`ZuV;L&|ZyqcM2i7 z%#N(EppyD(9qrY2*4_2ae;0L*ONvBDAESS9>aqg=t>fnA4(#60iPTtk98ulx_1$vt z8vps>-xm*juxcnN^1E|D(_T%*{PbR}nD%Op4vCOou=BhU+N)Ix_G*WQK3077x-S!wA`#NxJn!pXJ^TxgIyUddXAU_C$z^tAg_mCk9i#6V z?0>hq>GG)yUr9)cL`V-+nLa&Z`QOOZH#zVH5}e8*Th^x}_D*sdfd>6aiiiaout2bL>&Uc{7H(7z`i7X9_8rZFLywP<+68RA==f!_i3XD?61=2YK{(x zuw3$1XjF&f!roAy2_6X{Z$(w^~zFQgxVYOX&-x{(jgI+%f1UjVk3UV@0Fle5`n#8`m~Q{Q0b5e%cbv0>8~mY z9gI-lSb`3Tpte0xWF6E)DN7*?mt6l*9W?UvI}KK@8k0R4Wm_p75}|g*%Azt#M0Lpb z?#L6(>AQWSj>JZ+%WvmlOcFtB#hj}PL=m_o!gA?52>M$#0-?r%+X?)Y8;PLRY){;I zY9x1mHt9P6agLl?F5N{zI1J=Os^PWxgxhk90b3s zjro@dSRq{_%ZjjE_NGc%>tfFNJE;y8m>J#xj%>6JuSE_+L}?8y1XC#)+Gh!W`DR9TH)Ln17f&ad<;h>o1RJ<01sBFOVi z#rsGI%cZkHe0ibl49ffM5<%>bu2dt&68Kd!j^;R~BkN7UN*wO(QIq z$_9h1S)ZY(JkiLVo1}GPDicCS5o|?yVj5w&^f>f8`~u+tA+rpLKo6aKm$>ogA}p88 zION0&nD!J6jQ%U-&KU&bl!e+t&2)496K~z`JZQFvMwqso@f)otn-#t z3R+J&JT7e5a!83h=XB3Dm90!p4aB2_sGS&6f^l&4kqCKSPq$uHeo$Pa?zpf=ofp-c zNQ`i~bPliQywTTFBZDjkfFT&6yw z?m5uE$o5DA2~8sD)%Qsk*V#>bMVE;?#&iFft5uwKvUjt+^CHwx%qGr#lElEXX3 zhV!PEIta-nZ;sHvSol|Nyw-!G!f!gpA~UG>QCN4fs_uD*Umcp)==>4k8MlmzxfF|V z$FcdQyYmWLjSg3~Z(Ih!qcv4db!UdYxqMGhlKaHiaQmT61un(f#~nx0d4u2C_uQE9 z>AlA#v8SJvV*y^o765 zYMuLit{RdjYh-R+Ovc15grIfquUS4M5A(rMO(JBim)^tC>)gMfjvhH9TrOo@6BFy| zb?(zTS0g9yQxu7iHEa6UHWw7cr@S=If4)~owyr`*E?I~5yc?z!#K#cg_Io>)N{Xxq z8-FvsYB_FiNrkJ&`3r8I?&y#RS&jC*|J?p)$$=eX{mM_3Ita-nPaV?h+^2OOiS70Y zU{vzpcYOV4}wZne(+ z^@lcjCgPp!)Z4zkQBF0DSI_RM+U#smHiwY z5+Q5k^zSIHQ|sI>XP#=XD8}J(*>yvw0CgG^Kem0Gf9jvR)l-evheWv5bFBD0 zj=*kPSAk_E1mifRpq5{~LPw>;a!I-F`v%z&s2b>Mwp%a3w`+fkwLQ0|(qXxzT=#v0 z>L!pm8L49F`*Gn)^PczN>ibuG#*!U@J*V(p(}oUOA0e*~ z%O&ONdt`jZk{yAauvmkp4IQ-3Ltf{WOUl*v;he~j5rG}Oh$ZP$85Mm(j#rjT%2lz; zoVTD?6yY)=DB7h@Wo#P=in}4lT}zR2^@+H=jJE^}63jgo#H7Dg<^AnquX; z?*+U=X2FL zI|6G(bLkUIi`brDi~fbYj!K8+l5+J5=IS|pF*FMTYhH8db5)C&LDA>zb9*WsmP^Xj zk%+6D^tMG71Y#FgooyTQadz-RVq4uIN{6LLxhA5T+G}=%iouFYR-mE7JPni%%O&ON zCj-~1vm@{<(Omk(%p#zNYD`Lp<&tt$J6)&N{87mGsGvS|1q^w7LwJ&Mebwu!z>V&+FWBNx5pxr%!&=I?srp zC}Fr{-52Aac;$1vvRqQGie08UObA4{j0lQ$=^CQm%@@=^C<%yV!q} zL0~T;V!K5Y(NoQ*ry9q0Ly>ar%|p+7hMsC8^xnA)!rpt7`@!W`oYH#j?<4#*j}BA_ z-rdPNknRo2QbG*)V}$?gco!kLS4@FEi73;nv<``2E}*H00eYC$kw!2DToOU!2v)BW2tYk43vK4& zwlnfHqP({?pjxyV?XOoY37R)6Qti^(%Y#$yD6+i*q6o6iSiK?HVba%%Yuop$lLN2Y zqS`c1Nz9XC4DtW?1A-}NOGKIZ06^)G2<8Hs=0Az~SB$JfA(#R#iJzD-p08yvNa@Mjk;ZE}(Cz|5Q-UyDtOc`>U2DZ|gf= z`O+J;mnSDS)@`1fv<}sPELycmUbS+~)5w!p!45+8iYY)#1bd8wP<|y5%mp-AJ?YFx zx>rmAmqf7Vm(_vMVyyuIEASdDV_iViIG7Se)Oz4qCCSlI2EklFXP*zIfJ-8-t^ZWg znZb0gB!anszUiGOC|+fnbEbexBCdMpiKK%_>yQZM;&IHHdB2_WB0wYsZHa(B2a(o+ zae%_T;x?=RuLPsLa|bTzulJ65SINci8GScBABaofl8CXpKFBh1mIVZJu?}YqrpF<@ zL?WOs^9)J^b3q4tXnN#n9sHaC*8^kTRcqVyD#+R^MTsIF+Vz3!C21Yht2BbSfadrJ zMmJ}WT<8_gV0HvkxL1t8_%hGAL@*c7wB~6XnV!x}0hdG|9wYLl$5DgkhTqGa~%G@*v9FEf?A{*UF554oh(njMh4C9e-!| z_s{Ps9hQ>gE@%mf|8fR#1CcmQ)tuQd|V1F$Oh*F`jcvQC_0<SY3^EoD3ii)CY_X|n(cj%y)s1Pp2 zMNkw?@0~r5V!O|=-BMJXPw%|#@6f>r=&%$Q!TWabt0#`n&nQb#PqlQN$^I@8&|xVq zf}ULptyFmQto8X>Ybolvo33Bk-+^HSbXbaupl7f`YaNs`_?$CXipn3-H30iNFpPi> zOK}mL)oL9SOZ>|H^Oj?6TZ+nq0%T5*WPjHfjc_S0g7bOBUqt^;|3l2OT&A659S(v3 z_)F(kfz06qBB_4|@8)*H{o5u?P5cN)^lmtGFqf2zHt#I8j%*0al@meq9zP*=sNHa-gSn(!v^m$-It6zv1whfGoS4@!zw0R$` zb!0$UJ~P2!dy}=+FaMrIoZzMHiYHMaS=|uVlF8cZLUnEZIzC25@ES=T!a&^m`lnmz0Y( ze8EPaYzWJx{mLSYCn_DxCFP<+^2cfjfDkXg9Q`fcVMMOccf zPiC*K4vDZ_HX|?FE7=pf+me}cv{zEDMA%*5kr0;4?s3cZigu~$G#u6?ggP-poAlXJ zGG{()1wz?Mo$vxdR#zv#X&psqnOQTZVJ@PKjv@tcw7UqaJ34YJOZkX#;mU)7kx4SwxZwK10Z8rnRj^JznXnty=CoaP7;$(Doqo_g@0Sj6DbVi$>iD=t( z6V)Nd!TSUtICep<+bc_vdk%MRkR8GM1nA&+TvmsrNV)Fbz||oUyib4*ey)_&VJT9s zyEkxkNCfW_pu^5n)K-=v<+^(VSBFIKKEdcn^8UOG!ce4KcW;m#!TSX0;C#NUtt>_6 z>UstL-@W3z9Xe#*p1oI=OGb$d)=ftriGV(wN0uE2SvQTaTy70O^{NP1^jEGT&H(Xy z_7!C*GXHe*M0ey8!5JWQSYOC6@<7{N%60QZSBI4)xnx`sI*OnjbFNQ7WgPtfIv+Bw zl!A>$)S%93AMij;e#IhP1Io3h^S&WEK)xo%$K zc|Xvu>ljss>z8VN+NFiJ^&LlE9i(Q>>`Pb7|ccLEr5U>0d1fp~G@Xxj}06s#b-+4SK-n$cC_7{UV5qX$;@0J*T8dxdFXN zo%+$%wFA{ai#5d7{srXt|_ZtxtO>bYw%Ihnh=| zBaJ9JmFgv*s-2glNV(c>ovs;e<~$n$meyQ)KGKMy52;>)N)CM2a!I**{+l%$Wprdi zz+*I*_A3XGU>%lA%GG|H?v+P6G9s`(cpSD3>(aeI$lWxJqOc%W!t2}iC2~)P?b*hLE3D#jLQm)eH+i_$=P~Q!g>a9f-(a1x#mF1FhmEC;Dx)~8PABIa= z-6D!;&O@Ga%O&MP-zv%}@Eu|NWkXmlUtij=GLg-)+BBB+OkORoZppppA*E6XM2D!ZA8fnH@pV4aKk5QuEi$0DH5 zMxXQ!vIu*?D^5B(HD*4t)F5@%$G;{o?v9gVH^M0dapO-?Tm;a!=Ii~St3zgFmP=+% zw9IK9xV|eXT8Eaad)w_py&rUS$ovXp(p)lgGCI^KFb9fer2#~)%~iydR07VF$wYNJ3`ExT|RvEP-UukbwRW& zgK#Ao9kEP0RwMSwnn%*3?g9>++T}!5#N%I z8*{_h33*}6rRbilUWM4M_4Pdw?H~2X{Yws2rtB4zN75rl-+^!??I6V5+2#FDjIHbl zE!stxUL`X13Ui=)qUT2Ua0}^J@NebBGo*)YHESknme8RQt`38US;XwLTv!@t7x6vm zc!B03$BZ1F!S><#cM%!n>4f7m>lQ(!C~s}ztRX-M_Iq(5o%irzcRYbiVAr}!85FV^!&Rz zL`!Tq9G@+H~Bt z6s?01=xyeVOd>3odU6(Wjm&+X({s>Jv=umq>s6t3VdjiXA}p7B+7)t*EUiOXm!6!4 z5~L*J^NY_a%$kv<5td6mIm?bi>7eIedK?n*Y{~J3nKLqpuw3eC=gx;me@X{E2Mr}i zNkpZ?2ZGEQnM7DF_2hK@3OeXdQi7C3(9SP#JW&H82+O6%!8Mumd?+3073WtH0ezV> zGKsKUdOoe2+QT3-KA}n9=VRM+E$PCdx+%9GuCLYY@~J(nE!VZW+%)usB}n#<&tN`Q62V) zqrF7S?U$N3yjS6>DyC}S=#U7@CC|L0I_#5E%7vx-*(l07ieM9ouv~H#M0MB|CFN>; zvulpAGe~yApOhuJ!1)!N|jS;*ls%z2Sg;ZXXeBm(VO_n5j5k6iW2p73gf z=5o)Sm0zWGz!UlO&C1oj`(%-+WjHz{0%Ou#dJU$t3C%1j)Xug(Ohz^2n5+nSuuTrvFC;ZNsgk_+w{qgyJ{_^UrHiQ zY<)^9a}<>b%cVwcPHUpQqLCX)zm!Bg{py9OtWh+Luv{vZlpTi}IYkCT>6elS`nPdY znWLygST410j19#&Xl^70qh-1MQhQf)OJ$Ctv})7yVY$@$r+t<94hzZ0pSB`LQP_&3 zD6F2MM{P7U5=twuJ+x`QPj$kTzU;=_C%~Gj-s#v z$6c(I%u!S#ESFpbQR^a>@cNJltof`_G>x!aa;>;Js?pPVrhi-CT@;a3JLI-Kt%)Km z#YOb(TR%CX{%#xF&3w40J(kNo`H|~GQmWAnDwbOS*{>+cXOM2Ms18FCa5^cuv~E5bjUxDD%sp4LR*zcOQd|UC-JBI331PXE z-2!JmXz=)x6g4+St{R6q&5Y`>Hqo|Py$WaMM;bX!-?U9!1ZMN0FUtPgPEwYhbIql_ zhtIalT31tC1lcg0HSkB(fPxJv})DYK3U!f z%hmIg*;bfm9tT-8U|ZqTI7v(1WPgc*lLST6Sti`y%^KBU~e$=}Xdo>={R zM2G5?L|86;*PzTexL0PKOSyZKmCoKAp1#*aA07vfJdLni^5zivr-LBtng}Q5?oIwu z?)>DMkIah7im+VDd&2bka1i#1BjxT*QgxF4r|-$Qy^;varN_Z{SX>0g!8a=;0zGVe zv3b@yy^;varRRh1ez-aicQr+iL(A26+fmiz42}+oz}{NxAw?0H1$& zUN(fiUqacLxWnQVef8C!r8#GfRXQw1%9S@@;D6Z=_MS{Nac71AMQ`1CVp#Q+tCbGR zCFQy|ggh@B!ru3(rfxv-36@vXbZM{f&b@oooLep_SKdg%II!?OUjk^i=ZPL!rm#8aSVuT5zZoH>;mFv zx-a^Py16XxUo>%Uz|a#lecI=FTL{rn-CXV>xFn*9xMc$!*%0zBj%@?+B;CfnO5I%6 zz0&spq}(P{UE$j>M?zRGeRD+L%K+jgdROK!jfL-&Xo{4J_7WPou52F(!CcV6ZKES* zbudNBMVoI69T~w~(7|mW$W}hv%2K4E-4pneIp4PsK%8Q!Ca8UZG%Wy9ZZpO(bo5*lnUs`f?zJ_ z;5INca&yaDQlwnH9_@T&Ltt(5eAqU0WQ#tQOU4x-XwF0S5}Mm2&megWwe=l8+XljX zNk{pvrC=P4Q1fr@8XXA%eS9OywtfM?zRGSY0CAc*PVc7wsivh0O6v zBA5$0xQ$+s4jHeEEGZZ5xVeRPWCU|T2e-}0MZ98)l&kzo%!eD{EP}iwJs-9W9rVQU z`59#?Qm*nV<1t4<(6cY?SGG-vBJy3IpPZH=2k?zC$ERZkq8mf&jyTa*sj?%Q`xFtVfmw^BSW8^Uts(5Zt!6ctjI z@q&7%+EUOf=0bb?!Z(AyJN3Ur<9XQ-mMh0a6n#%)sA9er%3M+|TLCsyhVrr@*wUG- z%NAC)qM1tDO0|JtyD1$-S`+5PQqU9Pl4xah%}c!sc^p`GV$RcT8eh`dDjULbDO(8y zSvTbQU@l`T#f3J_P14SJHiYF;Gb<3}B?%&_>Rh&~y-ACRRqzV|#8gd3yceo8L20=N4q+svF zT=G8Tkr4LQWHoVv5{QAcAH2oXeJq!hEAMq431M%4RueZtfsj=k%O&ON`-z$BJ`!PX zu~t(z8q)};?qj*6TzStHy^?hwiLkeEtBISr1aRs;mP^W&_l2QD)_o+x-b1dY?pbH9 z`&ce1SKfPu4q5k+2z%?gn!1ghx$a}Rq+EG_8#>tA;fYa%uIT9BGuM59;JS}&+gqB@ zAS*?gxwNe^*L|!k_vX62I|?1LQX~pl`;?+EKPXzM;V(mKD#)O{?Mlq>HfLq|4*ja@pH@a^byyo$j~ESHok zW7oj_yW?m7c5|ZdYa{(;&8AUQ%PE{W6Zc!~oz!)APfM*Y$}7C@Rotz;^w#I|6O}fW zHh5&L?@}xRXnW7L#mIU2|HgYHhJ7}&41&47nldf*bV1v~`gL&c^^u_`m44l>R(Qk8 z4!%pVax3IbPd)O_RfX#h;J)yv+Dr1=>^q$3v#`7GQY-?!vNw#wcb4YgbN`v)h2uMx zK`_^Ck4;Y{I^9tCMjyPXcizTl^VRGYEFC|^=kfCT@xK{)1?xQjY z=K6Tp)2X5pdKO-J0B@NB(UK6?Zu=rLJ@Cm@h1+N1=JLmc*jmycJVXeWV&$IH zc2?^1@7fhk`~){lmv@~WAN0rJ#F)k1LziL^=oQwAzTH1Z5PvL~ns}BFF2%}y@10qxnl&31KC=>t|JT^Lz}r}6fBdCUZbeEL zy2uD!|u&(mJ(`>f~wto^>vdd`Pko+|s8=J_?##w1;d5L_xb zpIUA3yn=5!T^_r8=cqKoYQ3{?UeIaM6@`;$vW@CQRO@nitS=F+MDiUsVqS2~V>JuE zX~V>kQxnVHyM94z-@OkcU5OA}Dmj^3_QcGB!8>aBwO98{BdpfLjpqgZ-l|;a&Eo!G zq6-n%5aCKB-#4es4gRwAoA_rPxzEcFC(7Pze~n-An^@A72*IV2Gs*WoHl?8UA8+yR zidRe{tk&vZ&kgD={38DOU>>`}h-geiCn8*l9D6<%Km$nQn1%PHA3?1+mf* z4>*LYB`rdK^<@1y1>3v49y_>vnA28BG%TALw9Bbn_`)ab0Wi_O>+7+>5dyWa-RL(| zx+fD@PsW}LXMg3|vF8VwJsFX>C&}gHdeoCa!fLUtGqe@0)uPv&dm$sl3Rg|okM{~6m-nLQbi@OahvvcEDa{y{3UCnFLexKwg-J?hCI zVYS#_@eY2R%IwLAMDk^S<<0(wQ`(c6PM%C>@?>r|o{UI@u%kqu@;@5!WRS2C1MIJS z-~Z>7_GFGkJQPVAhRc930Dj2@h*8XJ;{^FF`kSq zI}+@p3><%Fpgo!D#*?|(cruniEw|~=jFwV$(J(<m0_(}x zy`6q}5iyWFnahnQVAH*e#P8_)F?mb;$wr74{pCD|Xu0R6XeeU%0`HmY7ro)b$#0(RtxjxH1$nN`aByUTB>J( zpi$!6ab>kIUry5)OUc;HMu?Ugp+L}R_wD?!T9_}VX|AN?{a`jiwA4%kq5_?4;~QML z2RXvUMuTCR<4tVbgy&@=L@S3QxPaJ5Cu=TI_a~wR<1yT54q6wp5E9Li!;xV$+U<>t z|3~j252({rD8YRB%~m2Vr=znGqNT@l8ll!J(ZYPSJsVx)dB#Q>A<;Fuv_gDOH9t!K zxIaV-^VRKUql7#!8zE6by48g+J*aGm7UqjlNi=C~pQjNL6QxIwjgMl6KYC>lEzDOD zVbqq^2f|2@P-!5j9x=YV){I>w!Zg({j6PHK$ViBmYAYb9b$#0(qNUo(XsN!TH@rIj zL-j{Sf_l?vsh$ObMu|^jSB(jOF1JkBSGzFwA9!YB4KRA%t~z}n6I|y%qX^z zMsNwWmaa!gBxu(9cAkqC=BwLH){<-lx3t#M{Sgug)8{_dL$okoJxcUC=Fuk`L8H>d zzUq-E1XI+K%7$oRzKSqwC9ASN5V(pO8;UmHZ+PnV@A9`(J^Og=W^FwySiEpSYQhB-+EE0=wA*QvdtPeCG1VUry;!Tz^L|u2 zGNox{`?2l1CmIF)7F7wp{Lcycz7-MA{jxW2=+0v?%ggH-MFD-wM-5xdW*UK|jCHZWhTAN@UVB3fN^PwrJzDpw*U zaqH2Xh=WhfEtwu6Q0w5#%Yu#dykO#qpQ+|=6Y=rU=_U7WKNefj`DE%~uLkiG=T&T1 z^h|}Iu-V}FzV|A$8?^1LpzDeS@ouVhA0=W*rz$0rsUBFvVe5{k7QVYWd1wIFR!T_E z6NxyZ!>POuR4SAp@oeAYsa3(^R!`h*+bZbsd~q_;x~Mhy+~cWTH_Y@;4P~M`*{E@U zt^CW#MxEzt2m9~dAAhCNskZk#a!F9J=ZW~3hkj{$)q+OBmcdmDD}TX#-iwIp?FZ$Z zLj>l9t$?MazZp(M=V>GI2SncOT#^&qE2u+F=ad=?IDK)3$9iAe~5|x8eQNsTY)Bz8Fi$HnI^CqgdobHG1Be6wT2@ zMLVGc^Tn7*wvmmHSj!?OI@3$+38S4*g85=>C)>zINX%%F6J_cp9;IlfmMYo_C73V9 zsIrZ0gv7oUsn}i{E$bzEQ8R$@5fVSB+e(NM>W_2Y=t27+N-$qH z4$||o5fas?d)7uyhNDE;5G~9X<1V=#W?ZEa63?kesEq)n-IJmLJF93XlwiIXKgu?; z5fYcGXPS*<4QH(?m1tqU7zfKX%sfvcB)%5cF(x?LDbork1g>2KB$(2ENm1&rz0V^* z;V|tJyxP?>>Gh4|d!?-YANxKiL84+Kp+9dpG|9l=ZKbgZzvQZ4qw#7bZ3ixHr_zh= zSf8 zyIFfUij{t_-FqIBA*`iPW?$ov9wW4|F|{ zkQmHpTOlD@ZmgwSDv7nkXvzPzR1!JKgpK+>U8zLN+Q>8#ar7Y}apIY6=y4@lk+I8j zFhT$5(>U09clA`qam#6xG!7n~@_6d#sHG~6=c&~`s{zsyiA(%y^QzRUKQ7TDQO$FW zxP)lQX>8U@vk}*4v+3_MZeA7F`6?oApWLMrTH+E^r?{?DCL5LzE!84%-6Bz9YRP+z za+|ME?L#tp8o{v=rz5C!Oh~l)$lzg!B`A>vL`NCx`cpIGu#0Uc;C{ z`+fTAgA$jZxGCxzJ|PsfArg-BvC)#4Q+4R~=t^R;9V-I<^ytgc(aTs`*5$8$YN^%Pj%~rELv$TXCsu^g7qc z#-FG4dCHDm(aH%%pT9fx%CkioY=}g+u8m%oIz>x``EuXrIMEzlgH0<$36ZFA6%rVi z>ej<0o;9`A<0`FnRa;5Bvkiv`BS2jnNKhJDcPh3!?Q93FfQY%|?K_ZH0tHOX}X#fA=72gL*J-=LGMZ zcCPTcVk3)P8zRYJFC!#)w|jHU?2P?k^U7f##b~K|#52ziUdikTN8;swJhAxw_hx0Z zA%tk@9{lek5eeRjGS`E9(9UhqQmtUt7}L559+SK>VC!PL(QJ-qo##fPdd>6!r_0>7 zN+=U#L-Il`yGLbNui~)!ge}dxUFNo;aTOAxg}W$~Cei%JD1X4IOJl`H9x=b@aQ|4; z_UZd41iSm~PQCOhuW(m)?6v0d4@ddka_YrgiMyi8%C~H1k5;2T9pJy$;nOt2YF#j6 zLQwsiU8z;iv5h4}6dxMkAL{gJ%$3Mm+x*OF!I)!jrgo=zXSi?h@-@dU8sOi0-{F`m z5yJGWSw;8vc%#)V)m!*8erV=L3A;Z2oIfpiVa}@5jK8yuLquFsvxQ&!Z8P7M$m;y^ zzy2CD{c>We_Cnsf8qNP=%_e%g^aK5KB|>niWVf3(?cc3>KYk>3OWXcwgw+~%?yTTH zy&g}sp2Rk$5^-YEky!o0{=O@beBZuxUhwLH^HM9H=6T+GeT~xL)K1%|yC> z)rXwj`h^Q}V*@IVN+YaRl1>8FZBacHOk^9?h-gzgH})VAu0--}arU#po>7;Uzw|z@ z40TsrTl!hkda)zBNBOQq2riY(`9Ixqb?YErxxDh~(P@O$n)3bppxZZoX~o@a<5nVm zX>At`J|^pX;%%R=3ifP3x8Kpi;RKAxvAD^|7exjjeO`>?!~H z`a9DIYAdC6?!EJZKdh`4-#nRZJWs@tU3<#kY<8#bN@U$V{K~9g_13ZR1?{-cH|`r# zdewc0%O_F~x)LGGxH9W}`*EXNKi92k@~0`S(g?d7^q~{K{SQ4B@9`Ad=t#uwu1%9G zr?&E4i5ev;-|=;(2fwPdBK}fao*yk66qR-k!j(wA*F8Tf zSn%_K_>=u#B;s)ObIM96vS#CT^2riYZ;Jj}X9UYsz`GI8xQNn7O72K@(w6urM z8TiZ%ueE>p^D8P=TYDUfAgyEd!(a3Xb4rKu(l;Sk8z!eX=fq_S36CZAw=Ade99hQS zwTiHkC}B^!rQ1NlYB9}wt^Q3NEtQq9781Hnk(#SgAz`(w4I8oU)|?3&*Ny~_n9OZO z^$70(z_VELTRmD2D=}@Q&UJCxo)$}QDJ^;A6)oI(kwk+5g+SkJAw&5H@E#c4Y#6=9#^ zB=~7I$8GDs^rfDj7V78t?wteS?+-V(gPy{{S4a<&%#PFYv5G`vX{rMG2@NNM2J^;5=65Wpx{g_kcQn7WHkofx35u#Po z>^Pa~q5DAFKtkf{qc)HbEjRMst+_mGzWa8$ZZBn14ot-0f5kBv8wZ z2E3Sv(t7cdXFDHRi!JKLc46ysJ#MHvIIorThiZ2u*tW!Gaer)UxJ&hi=|M%HmK(Lj z{jqlZq2`Z~4ZPRDy|F+>k8Z2R-IwOgqPD`AT8!cKJnZuZWMkG-)$=Q=kth-*-1|T- zC!%4$waXt+H5UT4aJ+dQ*Q4DfQM}9qru;k_S2$}?%e{qx zOSN-c_2PzP10_5vqy zn=hkM;mpAZS(o5hJ8)jFl?NiT76~`@l}E|C5s4Lp$p+@-YGKW}tu}q~L9@w`{y@Tw zmgQO7f7FJRpQ_O=d7&1r`JPvUh*}r-$)9+mTPnN{0@q5dS|ywo-FVNsw^k z=$YWX6Xv-hTrHfn1w`~Xqh^bfZFwY$1PP4W_q+>;`106ACBLQp0}0f^bAMS_GIm&*Ic#rZE6KOG@Z%Z*EAqW;rWo8M0a zMlWJiquU=`j{)~xxMHZX;r54(mUN>5dDf=(#PWZuW~~sYg|URX=4+Q7%IirsaP4#N z`jLLp^L#~@WJ!sXs*uMI67Ib}c9if)#2$3DWTt7qhZyUJM5ZFSuv@%(OLLyaFKFmhHCKV5WH$?yo_YDqtN zULC4O+viuccrem~NMJlKor@!4!n(6cw3i`yp%#ue-MY{J>F3;w$Of(%c#8|JF?zlF z+rOVIelD_JA;GO6cfIh~t@>{JmApq`UZ}-&l3P`n`0R;}#a9r4V-%xh-TA?zeZq%R z@}i?135@5}tvjd5S9wK|)nI9K^Q{?%RP|iI~;9c=@*!n}P46+&4`)qh?YB&l$z*OSVu1 z5Bj@ExbKjLk&Ugl@5y~t#eYa%sO7#xnnc8FSKOG_od}e;Z-FEy9WheSVa@U@RBVPw zkZ|7!^&}g-4_*)tiBDKeM3tQi|rl)~VZ>!i)>n;-R8`Bq_ygfOi<|nIuyEklIMW7arP|y30 zMq<&Nck&;kSQ#v@`%VYj>d#cFwYe7*?~zie{y@Tgztf+H>mJQ1-X9@Q%Y9qJGm6L6 zcSPVRZ#)-{^TzY~67kQWt8$Ns4OMd_-1q8zh?p^`c;zn<0=2N8=&hwG)mNre{4@$q zs26hDee=V_p^hU~^2&e`Qzu2>Ebu&^h^xPtlHZTUE)uBazIEbyOsu>t_h+)7xo z(qc?<&7yy{er)eidrsd-ggCEnZg5`T28Fl%iAyz74JIE#sVk;wcJwiDAXT* zKB{6Gs3aT(q2d@!T17{gwAH5vQbeG{C3KxQC;j}*oxQ_0X zTPpZ_ilPl;-1(`X`XpS5OHfQ~7ev+?T>R*>{DUZ&;56OOOyPHwrr>RH^vR0Ugne`EqZ{jVFqr z{CVbLUZ|zMH!O*TZ#;2pjs&%p z(ZW>;>Y;2%-@3giVx*0nURiO!5lP$lSt zdFhqY&Mt{@kJeoB(ql}|B8>Eo5|Woo@OZPgUxedIl?n;bQZ{Hb{wSkF)q?ARa~laB zZ;p*@gwfLD%B~Eack|}a$-li^iTrkM(R%cc`@_ce>9njwrFVZ`R`J0}{KYF%Z#CwK_5F7|S2nLk=j6~Wb=A(9)BZrJe*=nGhdU0KxN~~d;M6IOSYJ2f z>NZbSZI%3GVpBy}2@-B}|I{6ym0i7Oefb50hbY2YkCuw$}_gsWAQvMu6y z8@}(=CcpBT$xX|8E5b^U!22LP?+@b_l(qaKFM09Ve6ysR?rXT%qZX++jecpJ<@v_fuda!K#M`P0n ztA%$FcwYX|69wDnRElk1I@)(7n6I)I*W+qz@!_(AH$4=qGwk6sg6g5P@Wukq8%M;& zL^LA8l}NsLAB5+fd~#nw`QG)hlS791u0#l~IsJ?4ag`|CQ1<4WW3ksJ-=0QTExhA^ z?%yS1>a1h2c2jQmU5Vt2cO1~28uGAIQ4Jf#O^bo(x7wgpuC+jW}MJd@= zc=@dH{C)nbSGuZ z;N#kdH=dZ*HLEBS*H649x%I~iYGtsis7v6!Qc1T}QnwYZ$f%|26#BcWt&+N}P=W+{ zvfR3|qSWp8#(QNVp&698@$PoDV?AXfB%~xRZQC%N z5Q$5q*JufXeQ3n#wvxYKUz95AaU#Aobx?a5PV0&U zZ;&Wb_hr~SAlyF*IKPGkp)FONa#X zElQp2`ZvG(%X>4FN+d2JH%LTV7YWf)Hj%GfJ<{(b;7P(&mpli;$4D9c*MFv==USz+|$a zZ<$5BEgEzQt`a2bMzRkOKJYD`5grW z8N|GByyAUQK&YDYEYf#SxrB^-uF31YdL;+`RE=60pNp!w5{`UX^G`>J*7`=0X5!vK zO_KlUQJ`#y1otJi`h@#Oc0zWB^^GLWHfpc_wtRa1e#(YOFkg57$WF*!vc8d|*~V>y zN0uMC>S1L=B$%(e<76jfFInG6(riP2cnqTi^Tl1l>g&EwBg|f+z7O{(RjQb+2ktnS zmR*8vP|Y(EqNUm@+@nSU))!?t!PGNB*c5gTa^e%(Ela%|DrdEUa|dNzi9{wf>45lP4lyU z{fXM(yig18baDxl2=QF|0&@$LweiW5qm!TIogHI|y~oMb658|LSn;2-+1tk^r_Eok zZcm~eGT>*^czY7&M8D|UJ-s|gj!F)Gt-fOe3B0XI*CIv4{0Mn-88L|~KOB|3DW_gJvm}Y@`v5%8Mj0A9{GQH|86qk{ z!lpS_I$e6hz~rqjep1c^N^F|Hx6zKC*J;Sm(q}Fjkj$faWLF}k;#@hN*WOL#B`!VI zJ^A;CYpFZ?B`?&{_busI%a2~4So(Fxf&RW1!cv8itTqPE*a+{x^1L4?I(jaZDk`CR zgtSewj%q8PTi4WFY4P`{g>xl4VYN6Xo93M8jH0PI-^7%y=1yCACn%=+D|NRz-m8Ue z<#`;#ZSUCfvJ#@D?hONCKDE>L)Qemy(Q@d9@z-da__%VHILc05-nAaFkfmzbB`OBirOk8 zAzFBk8xz#$zU_0-QvDI;OQQrvdp1I})aU_%MxxJiko#P;)VNYwW;Uw`S1uEsMb1FD zTKYaR8zIe%-D~Z774DpIy>_WJkJ~CO6+Q9W>nm+e4%HVK*H6sIli$6MO6)9%nC8>crn8;YzH%aOWgC zQjbJtBHR;&NZ1|}g7dOAFdc2(%mi!IG_5WK*G9LO?q?niR?ChI8>!2xtZxbOqYAAT zhO~Tdw0%Z6UoBzl%F`oSmntJckR`YzNGP9?XzNo`_7NLQM73;b-BR&?SxF|s*hoCo zZn)2$KAwj={?4!Y*NZwe$776cr!{B%W<0oH8F~7OKnW6bAN3S%+tz~#_Vkf(wIU@< z?1+u=zrXc}IzgwO8WK-9}C1e({mK~LL1=njy zCc>^VLg2WvwbrxOuIFq4wXB!P--ZPLN9g0K{|F;}J2f||KJknWgwBg2a!N6^d(#yC_zG} zc`u39Bc0G%=u5aZP=fhpZ!08F3q5kz21<~~-h=Y&mEy+XeI2!7=1Wgi5~zh^I<&#npbwv*)2j@& zoBlr2xIzgMG~U8rESYTRyikk!-<%|m)&nI-(0Gg6U#YGQBv31Dz0#GA)Lha?|r`ad3pNBGwrk2p6mKtZ})x9 zxzBx0uRZdpCI8Q#&p)QZPwl^}dW^q6`1aB0_{}e#hkWq`KRXQaWc@kwdK^0Z)Z?~B z3Cdv`zaxfeew6%9DMN2DCX7ugXXP4;mkG#9)H5>3iqR>5@OkHS>L>|T*hhed_s9kU zqBD2ODMp!5^#ohn-eWK|H*1)9Sm|oH=Xf&MttUT*gpenu*2^ zV#h8k$A8~tQIgP?bgA{l~5}jFDtj%m`4$3)GH2;Glyr_ zl2^yIgvXJY6Lnid!p@b>)hA*op;nYTXH<=7^h=qU@QQ)s%^8J%sYYwUancQi=+LCA zLB0|p+kQedqn6G27#!vK1eW?}Y zJRW(5fTdr`%*5*Oy9ACma|-`bjn;(Yq#Fv+p-JQ6GAr*TFcaMhzDUb>x3ANJts%q!m$9Wxl{%!Z74kdp6+H+CIi8SUQeEuk5W8)d-G4CbRD#>Y<*a$k2#@u7Tml8ID9wU!=FQHaRo&MBGqqCs zz~>w_U3BF~oh}L0C}9~*-}Cj&WmbmPGbPk&{mn;Azp!;fPYl&4ao$HSoxE-LXZFNU zjS}mAdfN2%uU+scgj(%;=f#sFMhEr~*3IeVw|&_2J?O|!PA!hh!`)Sl63;$<^Yp<- z);e8wR|&P!PM3sglt^+@M;%-_OVldGZip19o~cHORMVpnYL$A}5IHNH)7BH$;L7mi z$IQFjJKHU@dZrpBa78$7)eCxJD4|w3Yk8)nqW!NpYpGFU=ke!H`eUdswZavHp2`@i zQDS%|Dxp?5zG9DWHkBh#jb5on37z?F?008K>(`t+eeQQIoUC4Y@rd&4?_W9(`L1*?FPI0P(w>pJlueV#BU^ zVo8~}hA~JW_WiT-8lncw7@43| zs%e70!LF>_`ZHm*AlNr}Dk=w>V?YDHN^oRhSH_?(*1_>dM+RfKrjHoxRVLsUR*4=J zvL--n(&&w9C z*vkMF1U2e6$;H$5Ad#)iQ7vnxeR|i4w*l>jt*g-(6>xCF~HJS+p0e+ z5Y#A<5VIl!?e6{`E;Q$^Hbv!;q;O7UP=y*I=p6?&3NwPopQ|>1ut0^-9!TP+8A_g_E zQljY9AVI4XqZ#uy;hK~?d*xl|ufwuZVgxJDoNwJas7Wz0!8rz^s;;&D{2jKhZ&rDZ z+I^&u>1kntR`5hQh`-uiyZ!MP)F6h+WmZE3tq_B9#ISc0v)k?6foc$gR*-FMybB8x zw1OwfLB#KRto?x%$0i;3Gp(0!jS@+2h#K!$SO@1)%H3xPWUDEkQSnx$kDwJ|Q1+~9 zj0MMo8ubg=#wMTVRW(G=3Na{044YT0Ij^WuzmR#RMX!bkT6KDrT3nAWx%%AsYqxJ7 z!NTMzWyCn{&R_WPWE{!)2vVa2WQ;jHElkh~F(_Bn;$2scZ?Nuy)##M5`qpeqLN$HF zbm=9wdi|4~vG9)>k7o|<+(1UNj86wmQGCT$Gpq;UZfxlIw1S}B)$HKCY zP%FsIE)Mbsf$UVH1ZqJH)D{)@5o!e)F@iAbI{wt&EA~hGhW!~Hhj;$eGu5;Sc0JBN zFsK91QH&0mxnq|Ka`X!4cpsrwy02AL52rbGkBx_Fl&}mv2KqNls1;;n8-(TW?;9qbQwpx8I)c|PVnODC#bSI$(Ugq^J(;~QJ||58<* z_^*Rsc%Q}ir^Qf0t&UrE-gK|mUo$%Tj7wG_rmtjbNbt*NWXe3Q&*#nB2UgUy2~-Xm zKV=LhXayQFD=!IZXa%2;V_vntPM{ULODi7do@F-%H0qbK%>%EctQ=V>0V~D`;*U13 zc&_Vi6lK)GJvqo#HAJWt<>=MFZM^Tb@dm9L$~exw49HbAM5q za>TIndwh2q57khHU+(oqj3Gj;C`XJ3Y+m6@5%mQPt#DjBQdL8QT2YP|>8ypTTKhv8 zR!N?~2tUpFp_)FTsKf6FDf$E&%J7+LN-%Ka^p`cNSqAzvC*Wnm!_8{C3roW(VE8)_bLI z8Ysgf-&utBAwsPvM~t0U9<^<_C3Y`p;nY5hIUsql;Ksgt*RkHttdwf_~5+K z+$qBfcQfqihi&|hd8L{@B4Xek$k~ons@O-#IUiLMSk%%TzkcJkoFC> zT>?k>JOICxzp-`ezFh)-kp--{ZYR;7m1d#@*INBCmIBeGg?%FmYGX(RX_oV|~% zokxtPU;UFMYBj%T-8^Z;h&44<$f{8Sazoft>WojRTp{bTGcvU2&ncdeH~j?P6rN$( zoMz9q*IoD1gH=NsM7gH^mc=+E%|s>CipQh7PcUNH7f=1cEf!0yblqMO)+e7OcCVVL zMu{ZbUF{tgd7nVk1B6>8Pks-sDrEJ;!%uzDtxXKqsNa*eh63=JB{PL|$?2RgDsq z^^2__RHI*osi%n0&I-|$(C^lU3AMtt3pH5|d9N?BQjHS!rpXDu$3vVhp;lJMe18np zD3RV~jk+;fg!_e+W$1Ez2^h8K?z$$;_AH}fWYs$!S;KfrcYCG2NZ^=_jeCl32Q@3z zC{fmdvk4T=C>;;paS+4kDe8!ssDx_TL~~cutU%aX@BHNeWgh2yXE4!S)$r{!--d(E zL~Z*S+wQUJ39R@&8??&yZ~q!1avCCN1y7WnsQ+mFdYs*>RD&3_f?U20A0lW4Pn09Z zy;j3bR*P!XFXW=~YKWi}JW-Cy<9F@%bM^;Tlo7*Z|1zSN$lpwmNVE`^)tGfybpIA3 z<(_4$Jb#yA-|!9)v`QTj!n!-=c+e{K%B>*VpZq0IIpO&zq1wO zvR6X{tvbD`ZMKgQ4ZhebSW)J2ev1R!l8|%4yGyzuQdVO|$9E*mowEJgziom4*desa zI|QxZiE@I*NXWQ1B&qkbWyZ%ySx1g+qSa>R(=v0B2Y z@#`JRh+%U67N_|xLpAVABFPO=VNS6Qi|*INlzWz~a{n$XSKdp|Ds@B%T&+1Cv_cG( z?ce^rSHu`1XoVP*gRmNK-Q}3OFT|h~)$LW9?RC1V`IqnMPUhd!TMXO> zC&itUXH?cGkyZfBmo-*QKBF?BR_XrO6GJshn2bATPmC6kt2oax<-lNI?QSwJxRG| zna39%b7Dq^^VUB9!IwQMCxckfaV*GZVV-0g&6t-SrXJR zD-wKS%~{ouk}p0x`I~$a);E7{>k`zY7$Xu`^>IAPtl9*uK;w8n!RJ0$@%c>S<*Xp& z?*N#I5_}3HBiD1mH1qszmDS}|LmAE08)RilJ{ z6VQp_J%KN^GTM)$rmEVp*E!2?wLcbTrf-z=4VHPp`}0Zc^__Xwi{>IyJxiJqz7Rl1 zv8xIlWSp1GsHz^iU1JLu;J{vU+uQZ zd!_x+w*Y0Y&OUv=WhbA$pA)K40%L8u6yte|?wotZQR_N{S|v}uM_yH*-*oug;&sQ^ zPH$c8o-2V_j`s=Cr%&9zXzqsBJ<(=LNvIWM%$V4rzvHZL&OP+Y-*{H~zD{f6o4APa zk>l4byXvB^`P@~F66M?l_qrSZ)*;l2Z!jaq{ny{PZ1)XUJE0mS^jxW`$6vm7?!L>` z`k3q3rnAfYqqC=~PPqNPxjlESIqKOg3CO&wm(idZ}4z9b37*0^r zCb-thL=6d6tS}L|R~CDcSI9Fn&IC1W0-fgbN3O$Tvd689uD-18dxb||IWLEo>h$& zs4vL0f^6C5b$h1{*R%;6o4jXP#-J|{w1Uj@22CwvP}3%?yT0$YFhMKOkhw>eV|eAr z1Uq!FvOm5f2pQ*K=UT#fmCtP@(CO~~ucp2r(<&YBUauI#HEqJ^ysMV$=p)<;wEamt zUfAtI$FN5R4X+uHQ7x_?SZm|B-SzB=UJ2E-30vtW0{-o%w*UA=ylafW)33|s3C|2Y zglbxZWaSb3SN=|)ezO&_ZDOQv=)HQ4K^f13h=(pjrHIpvy|ARgy>XT#Jz-p&BL9Q)-&KLxftD&$WBoHyMa=y=A2uC3O8zRYQbYB{{9B z7p-53@2O}1us`~xgRZr#CLckbgYl&WzOqn_5*iN`^b=~OHC0vmzQkvVjYqx$Xofr< z_uBLg%S^su@v}$?&9BTaHl$Z9({(fbq+_r&;^)|!xRq(Ox}(W~)b#MZ0z>9eovmEi3D)aH)^`Ek&meD_v~&>l~$aPpz% z{zH^=`gJJuWUQVavHBiC-fpS-$E))>Q)sJ;edLv*R)L;CA}DPfxqCX6I7HNC{c$;W zGX1sg5L%Mil_Q{5dBOzJ@7h$7;n|ZB_GVGk^>+dKUg4oUYut|Px@|;E-o92r-+97d z++<3BA4V%uofN1AJ>-dAJ0j5aITEegz8zh&KvB7W+DwcSJ4y%VzgM05ol9D_Hj>`> zb`c*PTS9&v_{*{z^D6`Se3R(&uvQ!awYGR!ier;ElWbQ97RU70rubb+N1EDIpF#;F z;OG&Aed8MAlYSlP;5k1PNIt=H!F=5p(afR!4 zV)1&o#1r!RKmxy=(huFOI8Z`sm^#ocMw-e#iN!E~}rb5evel_xgsHN&oVI?xd-^(o8`)Pf!g zg5us1b&YbOs|NI-u%DqlNDBh}*?`8!CM!qqAZ|v%F}Az1IqA7UpHw7mQwc(_w*zf% zUSHXzucA-_2{@(&LFL&OcPiYg+*(tQ!W9RuU2W8!Vp|sn^4p{t8;L0wYhlN-8_JzN zm_i98_Ouekn%^BrS(?Cz^7It^YyA~sbm5j3_A^|g%5ONeb=b1ChY z56NAWP5e8xkak76lOWe*V&C~Si(~xoku*FtS6SY)N`dP~^X6knoAfN=v8{z<{4Sj2 z49+5B!n&|HZfZL#k$uc*oe4v@RTQp|XQEw5+Vj!G^7;;DW0~z`<-~+~G{9Ek)jwZ@vPQu>KXZ{o8_+fziNzMQz0+Ui@2$2J`U|yquTIKl^w>osI@Guwg?#`C*b*O768;QT93FOs>0$C6?S{%^~NHZZjex>4PPcS*r>%975i@UP@)jp)9Z@MP0!Cm=H zxuNLM<(9ff-@A;E|BO{%cxJ9`aw8Q(2_&i})fL}(%+|PD=!osUF`DQTYn6fraWr;l znyT`0IN4!xN?K}?BtC2sO-$TROWL}D;`%EKN#g2@ELU@1jnN$2v{va~!4ps`x1yFx zzi=Lz6mWqNo5miK>oxL2%HSCkN+9t;-COLrXgzsrdX^E05j`29`GY$VEw#f%u; zy)haRH3@%gasojKB#LY&ikWu#q{I3MMxb*xXxYPXyk*e^j(}QckB(CH8^3~B+l*wy z>M2F?jrsw2jMflC2_*P+*Jax|lzM5ga@!=B8@q74UUU=1+V{LksOvg5u9hykAiW3= zqeV|&a5D3GR1zV!=8)Tz%Z01})#KKnWziob4_S zi`Yw+eICVZ%x{kI!ST&#ZR-^Z)Pg%Co;Wum2D=AtQ*Kqw=bp8}??#KiEw_?>D?%km z3qon-K6ET#I34KtT!C4HS%BUOLYF%ClzZ({XH7tS(=L=WL*azII6yb2`;&D7P1daloCEAb3=}Xwy6lXvwtE+#U{Q zfhT?*?WmoXUYjN_3E&90dxbj=wzC|Wsl9uyIdyU!!tE>}0iU9Ra3&yA>r0!{XYvp( z4!C>eiB-3^Ylk22OsmUP+@2afk$K|T&+XbF6FSp{?M04&yJbiV!o_5bHa))wUH06W z+f&2+98W||P;0}Vy3zxgT{r?h&mqm$_72r@*;azKcZj60Z(*M|&(tHc7rM9!m-CMa z!bCRF51Fexv$pxX$su2!kuiRtSC||VKK4qGlZ=^b1M?*`C6T| zBd<>82&kp+IGLR3JBRcb<;IBIL{DmVzTrdX zI-OJ;%gYrgfy9mU@nm=2YI1znKt{NCx1xv5ENDusF}H7rdtaXDn$?854{b>^%8esTpdZpOYm1@pdb9-ty!g-=-M=U)vuA?&O??NsPm@AlR_J(C}eOfTrp014& zXiZQcnb%?isa)nGK@WKwc-3L0-=4uVBlxcZ_qCAbiJ9{(m0m`X^ly)2%69Lit|R=< zlg#6*q?Iejl8noTNZWPGCAilXgjEI>%I&+6G^FPtZqERTWjUisto=@M%OsBV$F2Jf z6#M+~bi?dhoL8`2p15DxRe5+aiEbM(LWb=^57+hXL+lo>C+0Vnu$|1>vvs83w^Qio zT_)?%N)b>!OUrHnA$?W~IK5lTH?yhf0KS}E<^35r=oerr}RBJ{zpqOJqN z=ta#-1SOEL+1pAy?zf6`Dbo=xI}B1kS~ij zliJ+Ua`^Y>q$!A$`VSz57KMz!M}Mjg z4Y8ze)4ed1K;lmAM&i|X14zcV^^9-~8sX;Q<4Zqh*5?SQCEuzq9<*^KEk0~yL?_=R zntjDX>5dF@j(}PZo%@pmvWk?C))6O8&QwJ{n?jqm(;_H=gngg>Wb>`AL>9I%V(1Su zl4zDlUscRPPyz|Z!+prReVs{Qy={!>YLcchij1Rc=d43e0tve@t;N9b&Lp#ej%c=)S8-)GNsqK?x*gw6zesmE6}{G}96H zPv-X6{4|PwGda!?P|JCZNSxZ8*OV^5$%vriYs7vjlWEP_3kXUe(QrX~amVAcnwm*E zBILv-amM*jdTnXui#vVb_ffj;(FWUG^Ci8u z6{~yRyDbOp^(ISvCTeVNm&>nu4koiY+*0Rmy~Awu`(;6=`nqUuPV&M~0tx6L8|}%v zmHM|crRcxW909f9`yfHk+D)b_kN8WmE|(DWDs$6VQr_*dT5rfr`EmRhGQcHG<9}J# z=N~@>Q?~_W&uWEFAhF8@sQDu27UK_udBcN7dPy&hnlubrfE@y=A z5^I!nE*6hE%M(!R=q`8Cu+4HZ-ZqyJj$?YDQ!kS6mMa?xlt7|`VIt{cmrY)Y`xx;# zum?KJi0x%O0kuNB2a$*DtL68k{fyXjtuyNLs}J62(V9XDB=UPEkaL^T$?gXVBkEo6 zj0Q4dMCKqGlIs>TF==!iX=$TR zQq#?MR4WV3rF)BiF{0YV1|Pg8V~@w80=3@%x~_^et1rF#TlEj2N}7qaGrOVqeUG#b zCVfQLH7~?-SM}uMlU>A&M~}o|#$TlOGkc14s(*@8#J|kOnFR?r>`5G&;cult2_!PG zjkx*FM{$?kTSiob1mn%ED$$uhQw7EWY3QvW_?L~xH#e1{+}KW>4cIPERJ)GBhAk`5 zJJac0nC2`>mCre5{IWVt|OWR;-ucG`gYbp0mjr9Ii7q>-u z){^wrK!)}N;r-4W^dme3|NX3RUNx99MddNGzSJ?{vDDi8$?=1+^`%vv@3J`NEsH}h z>xAQ7w~i@Lk`}f6So_#!(sZRt;=MX?n4$$M=c3Syq4dPP_A2{6&&2PW%A~@TKaS_w zKN08LERjx>jCSpx^<7-tvy9oeQGXh~nz~kwd^}Hq8HIg*yt09~@yau?@297Xh_nyE zi#KjZu6a9o0%-C5A$1soJ^c2d{WXdLC9vJ{s>dpm$5o>C@I7WDc1Az^;`v*&+WZMe zK&@z-C92R522$U9x?GiY#yB{_3TMpDs<+95}c=_KrKRKne5^_6_^H z;B!Y@&)63KJfbL20tw!$KViklZA>pb`(bSg38)2oRuE)Jf0_Mx^J-xb$_+Qo0 zqNcy!Ga`D-;Z4>zbp>TYJ$nliFk079^mz>`Ox{ zYrL<-jLv#d58v_WAb!}o$IWZ$8+n~SmU?a8Te;4}He%DI`sy8e)y%86Z9br| zH}3TJ<(o*`sGYc^)nc`cQpIUOS`hYzw7>?x^y%{5EijZod#BEosL}?SsA3-Ka`c7O=$W_jmAHd zcU|wL{@Cx8`s=Y*%m!YaDSt6erJY`ebHt?FBGm-xms+`7!O?=?bIuvPpEQly4Hqx`5kPyh8lHa>kRmvan~X}5-tISKR%w#1(2e%F!Ps$n#D z>}3RV1#Q5rv$unl2Dl>Ci0;mI;Ia!d3dfru7&QHfR_WKLZcljvYVp|}?XN{X9}!(z zR-dy0wP4=}g7=D}DF28bU8K3j#R0WoOoEWmDH_dQIDwjlYY^0eGm0nj_c|bLVjNw! zECWF;*v~w1a^gU2Q2(}a(C{@1j*k$>o^DtaWmhHto*XTfx9+N1+V_!=vYVq?U?{+6#IIROcYk!m@pcdbQ8Dl1(ot`sjPZI;}hO4c_ANn`dU#cH+ zqYsWs_IK?b8&pzf7PWklrG=7F`FmB%zJFA282(s>EwN`*lf&psn+RIaJPN^_z|qHh zm45RII_EW>t_|;lPL7ns(!XkT-~G>Is0C?3NNjoxg$6}ZQ1~TeO9NZJZ7_2J-oK2Uv4VR%bci%S}+UG4%w64 zEB~n1j(x_6MmyW6CSFLT^|#w{1k~bfl+Sx1HeCN)DZ3Je;mQfu;60=c+3@;{oB8XH z%tm&4d(wkiQd>JOj(}S4+SMa}Yt1kEQ~H4sp_?A7_bnYjJ^fm91k{>$$%I@e304mo zsCycGzMSLsGGY+@P|KQI6QPGZaiU*^+GS@1jazz?dqzPmn`yPkh*ne`wf+sW(W&n; z(fQG2+Vl50Zbrd1m?zfbo#OXX)2VS;8iMl<&PSd|yqh2veoLXl^N(@sE~K-O8F5JY zr7o8%nO8}l^)>nLXV9rL$8a`ayL-AeCKfTT)Xx{`*7iRrOKokPLYiuT*Nk-dchq13pW5V!maT%IZ%lwI*LRaP&a}w#4S(u42tPl!7bF-^nH2^3}r2 z#!}M#D%r2F1=-uGiR62{QVz+hP450^AQ^VoJ!{vQ7K&rGC*co7M38`52A#dtR%T|> z2XkEy{wb(bcYGU<&%arLpac>>%8kf`Qe&xqj_z42&Fn0$dmfAL9NfSWP|G=ckNRVjx-?!8r<{B(&Y7;uRng@%w8KshznnUpiv!v?e5Jmd zZ{siGiF{qIlI};Lb-N7lBt0JtwIX8%7X{jsipBju$vhFX>MlBWv;vji9)zJ5^pGc7 zzxau6xqm@+%~TjlAi>As^-#uA$r)r=R1ZT5jQpZ?q`0p7x@cJQn#J*X#7Nvz+=(uS z-bGO2*=D&YEh!Z>iz;Q$+3{kP<9iV`*3H@$%SC)Yu6&qSyDOSs+Ye72WXR1ZI9`J^zm82fY%D34-!dDg{jAZM4*qz~2m@{; z!v5z8uWzf7?_M9g`Se!=C6M5~(sQ%IUUl5CPo1p@<^*QrOkV&VJxlJK*F*%((VR9?0;6}z8YBtfrc zpS!1;>D@%C6Y-4mkbRZW5V=?5jw0dxMEs!dK(5ar%@gA`Z9}V~!|};&ixBK7*n>QA^IjkHeRM3YYciK3 zpcZ`Eu~U;aeb9~CvH078dED5A_IP5z*ORir=O|p)`2d$I_`HI%KoHXUH^W|~9(aCB zD|rLnB7U94#?Pj5`ReGC;+a~tq(c=q<>7|vvD&2E6UyJkmKW8>bLrbi-9EelDcyS{Ut|f3(vL|P7Gd$*w zJ6>_t9YF~s__btE---CU^=edTxs;0>M$3EU?COa#4C>-#LK=b+NI-jnU=lbOKk#dS zZA=mnrv-Aw6Elih;th-Y;z|4aAt-^_<>QF7*oWGTi^kRGn{#=GqYtiQY$UFphEpd7 zqpMR}A~+J^=!5+)2$`HDzA- zeR5;mQvVM(cHwyCiERry;lZ~>+^g)l3?-0&b5jtGX8GgMq6V$LagN&oz)=Z%RuGI{ z48*?;4RGksgIs^W9RN>cx)h>QJ7e*%v2ol}4fY!EmASA@$(lVFdnDK5PQ?2y8c6bY zXw`|%cmDbM>V0gnV$m%ICr0!}P|N*&=?vIX9S%eU;u=-d~$W z)Jot8s0BR~g#LftpdPhH(98V_5ws4q;28};u&!MT#hFZ|m+nynB`_w~k|12EorLup zIVzb67i8%B_%YdHx23t7?RQG#U8Re}5r*lS-}Q7SQXZ^Nzy=#8DL#KYB1k~3Z{Zo@ zh2a^RFE@3Oml(z1P98bRq?Wxn0&2l5us1A=Wc=VlL%N{kD1y;Kt*UcFMYYv7&CRk~ z%*K$>O>pJx?$oJA85cREd19@X9=5dXN&EKy!4Xia+3!~3blcsU2`_c0UanquMvGH@ zXoH<5|2bds-}5jq(>!s<_n3S(us1#G+<`lh0#Cj0MDnw7c>Kndii_V_1fzwy8gwyC z?65ycV|_^HmAauXj$U(A`P1(Xg1$qVC)$~+@U5+QAAJCA}dAXo1|MA z4jDAY@?;4+ zH4~p1FhKdx*-D1v6^_bPjkCprzs*Se!0L53H1-1kU;3Lm{GOxgDv0a}y&{=LO-5+dn8BULWOV6eM6v>yW zVl#3sf?6=JKVsX2wH#ys26_+=d5%x6bIUSc& z2P!}8ns5ZvDlV8KM!xJ%hRoHS7ri-Z0*>3eNxA-W6_;I@1)kW}^C#*)#DzNa{lmop zeTOGO1)=@wA^5YyTgA54c`my!3;bQOx5G-2{_5uRLP8e|wP3qEVV@C!XCGUwWIE@f zBQBvLYBP+u<>t%KcNmi(Y?DO1e9b9k)Tag*5>Sh`p^hj+v#gubyzDL*dIC?%^2Duc zP4T9}5AIP-}5gwpcGDkxZ?- zoY}bDt~oxl@35lp?}A~pFuU-KI9uD}X5#uit(C$~^$~0f+K_+D5r1T6k|!m)Q};z( zWAU*QLzU}8(-G8yk@LiU*Ffz3>X1^qlNLcO=pj#ZC=9~}^Oh@DU++gy0tw&Ep`z@U zMQF!W%qySU?eOuZM)a$TDHl16mM0ukUGdAj4@&Xrx)@3zF($`ZyzQ`%d^?oKY`i#` zkIroBOwS0NFw}x4`~M-bX5tFHPD*~WFEY$7jHyTFVsT3NGSc$mLT2N-c_KEn^i`fw zBLpRoXgPY0IRC*4(tdglBfc1p#wV6PP$tb-%H;~i#1r+R{c+mQ=gOp~%Mp}7;>w|5 z@pow+$?{&xYy`Tr#;8X}`W4?rPzy%O6a9a-z;E{0(~f8EAt-@_E{Fennb!7I*HE0?eIm7oL?bt){y@e2sK_i70vM%^BWZ_Tr&CO7nDs0B046WT&A zT+Ht2ujySSK?x-Evrni7PdZC%?bb0H+atT;-Bzyj`?oEeJ}GL%39-WX!1 zynQ2aedp84xHbc|@OCfk8<=%=d)amzF5dK2*&!U~2&e^fCr z4DInm_8eP$#>17q@C@eeJ42c$zLvRSI>?TiukhvwsHIMNqcSMh60_-r|UGmA%DD^8Ad>#)KGsl)h~uRh>V_-6ey! zrFp`jmJQD7jpc<-NH^dk4ByfrQ@&1DNqacKTmX+UWR6O3!&NaLMj$=w@M_=nq==@r}5sAA@SMm#k`tj*&W^Z zXOuuKNVD~d5!0BBxr_O61+^M>ZAhFfKCA1O=&D({GNLad@IN-7*2y*c#CU3%>efY? zGp{z(YK|PvnBi`Ny(!EUq4tRNri77iYZ-W9Qc-uav<3e%&i(s{JotJF1>j)!83}giC zHAwI_*sR3`mfe+~jDTYaYC+ronNhfu5hGc4p%%2k6FRS&GOv7@4NeQxGT`H=?cWnE zeUgOfuPG9&GXQ7s6B|9U-TRHoC2li|Jbr=$a?YQLSB$Baq2%OHRy!f$wW?vw_J3+8 zgxzb2x|PP`>Io?bYQfb2+7^W3r45nai7?#A_%4E4(085)3n)@H1r4KSzn&@Z?mWCx z&)-Q#{!y7m!I;lbDUNUIBEd(*xxbBVg?WXpbij&7DTOPHL~rV-*2m zWdca^#D)&Zbfsly?e0d6lnm`m5|+1)gzD!?kmd=)AIY?4*lO8SD$&B~5RiZ^3Bu;* zW9fpGca@IyW-2h+eg)G=2eVz|Q^*>Lx8dVpO1GEwqxC#CDzJh;hc!{;fYCwHbaa6P zTM~rj$4zPE;(pX-=LQA#8l-u`RGorDtWU|-)jtUIw`oBbd30|P8GCJ!{5~g)SXM73 zMlG}0U#Z$It(4shXVH5TeYm;?u>Jt_P!JkwRw#02JPp4$QHByo)NbZUER5EXNH1L_ zgrwpCx~7LA-DSQ?`(RKOsmR<;jy`E3!(73Z*j?m{Gw3tZG0NN&3mL`{;y8}HUb%wk zP0f*&eIev$!a{PlyY8>;V9y~qZdnU@xS&9U(L$OhoUBsl47~_#_~v_Z%*S9dxX6P< zF3gkbEDt07N=K94vscL3yMoAsB@v`a7u~xIzjtQT^T1ifwx`J5)rYt6Vf6w*$aCmI zTW+|i#5HfmRVjc3fAjv3`&wo5p3c<4vlCZ40k&IneGpk#dm(W@sq<=lJ15#2eNt?u z)Z=Wxc6nmjn;z8v;RhvXx*>)VNF1&mK$g#)PZnr&@8RZ(6Y151nM%f-jR4WY)k~pwLf2}q#$Dp_4*P>AysX&$66>kjV?*P(UW5J56YEwi zQCf{~rv*0}aMdqhT??2IL3nn$13L}hmP&_jASi(`!8#hO`d;e})Gx0st4wi=tH}Y0 zf_Z)rRJWeIRa`yZw29+ra3*~!I1&S7_3sk?(p7;r>dK|+Fb{iAUJCw%@Y=hzm;aqeQ1U8 zGOpUf-0R-t*$@X}JZrNID?JFp)AGN{)-GPOVfRg34G2iUY7(q=oAZ0cxSlur=Cz-z zGXV)JXFu}MLnL?XbYB#OREc(3(43x1$wyG@hQnwwx2X*Yu-PQ@M9D%QT7I(@MTepg zlt5yy$2emDv^%-^R#*4Kq}D`gQLa$}5BEb*>!5!KSsv1yERWwH^MuQlIO@MYTM54T zo0CAoct99wRP02eJau0I$oN?_KG{OK+GUdrwbo1vBYF7^NbjkIGEYRfBr8wpaO&Ar zkE`MVGuo?a5Q%u{POi+*Mi;E6Tg+tV&*8`1E2dKgL|!H<%S zMxnGuDpVes?d7ry^TQM4Xax0`x=!)llZ~JR5^#hH!a=Vjs`$Ou)*Nbqpcc#zPi+0P zS1Gn{NImYe+JKzW9HbAejvH*pd@ncSHp zpw?!MH)-m4L$g`Zom=u6YD>d+$+XstdfaG-vx_HgeeXyEzsU6UI-Y=94yL0?+@iA@ zPlHls<7I9LJu&!|5_M*e3}+P78u31uq^0fDOzEg2I=ckX#g9KKcG@G{{D3r1R94TR zO{ni&jPhjMpdZUTsf!&(+r*vnd<{#@xAI`C% zr(b0us0HougoTL8&|WSM=sQm= z{aL95?i)%CzG)DYKw|Nb{;Z0JR&&%}cS7jp$bs_E8xeH>xa$aN!Cdh~yVgCGh_@5y zuMroy+B>k~4Nuhjy;0ll)+l?d}MuH|(4 zf+D3LT12pt3e@7Os_Z-&O=Yi4<<;0RT%W_f;fVvU#?bk~5#`d)LkLPBami`~F_^nd z(`laW6w>|g18LjpuSx>@st>hb-|$4KeJko_W=`)ktIh8A{cl^4D3~{pq(!XK*jAP^ z8+QlQrcYkm(p5!uFw}w><%!Huhm|kUJ?OebD-5+@O&^}n%kQVmtlOW~e$axe2m}e( z5R!vZJzB4(}PCfZ_H ztp{rHHd=3KOUs8ZR%$l5V(1B^c_OI7OZ)e{G3}=B&%Nb@Z?oYWPj>$!W~kQik}(Yy z{3+Ceby#@fYsw7j=Zw(i&VmB}K8MwMV0}nd&+=Rpwg0&R*>vuzKrLA7oF_We*`Qv1 zHx(0i6RqKoMP$)(Yw7VYL59^2cKpa9CRHt^PrJUdcNtkeqg0bN&BB=zY!s*ks~_;h zxq6$)K20iZZfvWC5=c}w@FRbW%%p39UTp0C^yw(pc@Txo9F8kc3)YrUB`+WY^&3jJ z%N{VVY?B17d_NW6OSO|A0kwD=!@`?s8u$d_7lHZ|w%g{*0+Je3M+zNxTY|J8jO{v0 zRTC%TjscDw0c%T~$XZCU_Wuw)hwJK>T-}%=KAYMDH(1@BCx8~T$L_1mSXi{MuNsX@ zh@!AvSR*B-dLh}lutIF7f0fyAuifL=qglt$*FQV~wRo=vK1jiRJ2%%3UA3iUIxJsgCNiN-@IMSt;0}*hzc7s+c1Yt1SX* zz3{||Pm?gZKSFDsbCDyU*8F?dRm-vs#e`vxm{*-V%Tcy>wsvIPS+UG&A}8OC_11$s~%3F1QJ5dWL3*Pk5nhO+-8K{)6UqxZe1tfT~KRx@*}Esjd;g;T%ZCr|4on!*`lp;ww_IXFVl_*Z-Ai$67ZHWtE?Iyio1UZ(K?A$ z-0fvZwCKE4{M=xHYN7W_w(iz+4aOaBq-!4!|G?dFgYELfBEM)n?cg9S%G}ElP|L>n zfM}bzOJ!R5gxN4oor(we1Z(lOgWP>Jcz1~>9*jxEmcN~~(XHdTTU+qn9Z#Hbn2Ae| zOx7aPVH^Rq%vKbOpSxtKdY9_X(3=&^!d4Znv|C%X;qH>bTVgyhx^XJD+wP|QV7#2W z)d+8*@r0T^2OF7MYHh6!a0Jxq`}Kxs+-i+VHUPO!&0z0R`f95sV*M)fZ6zRHVR4C6KPM+F9b?p z^%LI4he|`d&exy5KU=}suykJ|RtDRuZ*_Yl!+I%#aH^?-hWSsS&dt0z3A6#dWfd?V z1mMNv6~(Sp%T=IB*DDaWPt;e9U-yiwc*BU+QP?unL78K)4Z(hfBN5Up@^xjkJ-U5M0fJhv4v*)NmEs@Cs%S{kQ z8nGo;-v@fd6Q`SX#GHAS9rNg3#_#I=Z=WI-T`nwg~6i#ZKO0{4S!dY<-W*A*-I$-bUW=EQQ*H?c(Y! z!Ic5hth#uvNG*oxQMdRiwEF5u5+XKs8+*1&t`a>-UWmC`8Td-}I5~ury?N^9bzfJx zrO(;}%JHlzw4X^m33s~+C2Y-3~8R2nO&iLwF{&+EoO5B)SB zLSt$+zCUfzVA`n`J!UE%ab3$>E34j?0DpHirufS8LTtI_KEfM)E$~F)7B11;7C-Dwjq^^x2fk_>MEY3I6l?nC@UvH^t zcAj?C@)TMfm&eT_*lRqorp7_}*fNnO&U8W06KDh4X16(hVr4Cfr{n8}a|G1lH$h-K7V80JY#aW;F+VYb$f{B>I(oMTWk^>SEAC_IpJx-?j5{<7mXL zB?yiTNWhi^p~sDIx_3_u`WAN>ZNC#od^%kf$Fu5mtLIN3+p9~&x7#Y^(wNDl_u|*$ zAHCdiQs{3CRA1-|)Lpp5t4C0&rTGV#Y)#O;NpFqzw%|W4~ z3*@&pbBOul_u_QB>+;BuxgHTAM!K>}Q(wR8-a2d88JSbX%aGNBm7G?C zt#e3q-BNL{_nm);Q0o|KGkq1ZwP=T6Tae(rYIG`=Mm1l6=1uH`pcagYC%n8T(h25y z=)m2j2ufh&e6GTljHEXYA4JAWt|2Ia*&XFLipVVT9>uzvPWgHP^b^*@&5Jv5aX@>~ ztK*21zaYJ?ugmVjQNwBNXaRrdmce-i?eWBsjXh|1O*=d;W+mqp^u#VIlqjo=q=zxO z{ceX9A{F1Z!V&fh5Nr$D;E8W7edzM-4e_KW-m>4gWKuf8Q2K6so$G(tPpnRA|2}kA zz74KKt2qK{Z8e-myiDs#?U3&4Qp-*%T8KL1&COoRPyz|)Evxp{pcP#lndDSPB&j^+vD+e;Pn zHy-P!jpO2gG*7t1=PIv<#^a^s=^O#I_&D6_Iw=kP67jLkT@cJ7%r1;c5Oz7H@>&$*X(HV#0>Zz_A0kvRE>>b&xZ_0t?fjBMR8o?ff{m&k1QM{8BCFUtd!lmxZXoX0^d*7>)Vh>tOY-KJNKIpO zdpM^v4oZvaFzn!d13@iV2a+e27UwFTSbg7p9YZjT7S?No9ty&z-)-rvl7stD}qV-CP$8+{(BPfA&IpK=I z&LL0lM-!ivpbq7K5L`J2tA>->7q5yNYQK?TeC))pX?xlz*cw|Ls^kc$1-)hWafHTn ze1wF1u0O^RPz%l#_BA}4Dqjwc#jECRMo;5;j@1-4oEEQ*WY9hk#5s={f{mX7 zjjKK@7mD_x%hFH^zsth!wf%Z8CvP<$#N>B37*TWelTu-7i9f#Rz!6Z(al;Do=0+`P z{V3fpRAi+&(Ul8dppSAx3blT`$s?|FKa1fZ_a&YPbGokh4710lJzG)uMH+g@6J-sX zE6s+?!tPBAxswC%3xEH{%Se;cR#KN*f0^`jK0l<9c;Uo!VkykiWW zvZN_TK&=-OSCYsi3u$!aCuZYBaFsH8SpeQ0G*5wAus?WWSG$*JyE+tKc$uw*Cj?+` z!j{-Gs$XBc^f)_@)5TGNzC)TPwp6#HN4xdG{)Jz)(7Hv_wM6&pDqo`BOD57f62yL8 zB{oA~PlJbM*7U{c0eJ7YVl9;TdFGK0BP^x6v-D)x65A)$|D_Tnu*e~b2WWSta`*%I@T3;#JZG0seITaGmUiy;nozKjxQI6-8 zGxq)Q`!GFj?85O1`=7n1PVG!5RJ6uJ<4!A33)){P&A{8is1Rsaf=0Ws)<7PNluPH}BEjSj~?=&>9q{WvX$&u$gC|o~acH#VI z6|D

ZWgqkJP)%MGpG_ddR+7ZV0B!zP&|vF6C;WC$OJ+ui`ES(qZb|$eI0GENmB! zCEkXIS1@fgb}KSN2Nb9U`sS+fh z4M?-^A1_v_hgYW33m@-s1oQ;bf-vl1Dz^7b(~#l%Tm?K>X{;q(EcUv4MZL64cSiW$ z^LuDdO*T2wJ{-fEiSXA80}8u}?F)+4(FMA1!qIiFBU3w1^4Mh}SMv;3M&pUEcV+xM zZn=1*s1t?~NX&Q?B3hW=Qro=Joy=^1&>f!$-#~u(wZTveR(s=#?Omeq;j`N`!!~LV z)PgnWctYJU5?2@%k}X%yA}E2x>bfc7f)Yp^+n6O* zw7;e9oUi**J!(Y~KG|kHv7aI#s0Hf|^2D}hDY)pHD3zQiTo1xty&9TXM ziChViL^*gTQIFaq!CEG;K8GOKJW9p8`WA~{z5j5;2X$|;=c4uGt?5~gW>Bxg$N^&`$N};~RD8 zJhqSOWUl(NDO7DX>)C()zir>?ns1kHvFFFCkW_qE+gE-V;;DsNFmhc>|4U3rOTuC4 zF3O>uzB2R#W`Xyrd0-O0SJGSQ9cUwI>jsMJuPh{qt1oi3KVgkM_TAuYDi#aP%o#i-!*^rOIe>^_DjW+ z=O;=nPv&q0)Z+WRXniWq%{?Mbn>$zwM+RKsc;dHv6n#`ngC5$a64(c@Z-)A1iZ#pn zkZ0|6zYTczLJAHUeqOuKsHF;O!P>7pp{r8BDupOB*xhg_fyDo>U{wU3 z5MHL@mf|8QD~In7I4WV^2twuUM6A9z8Cf@%v@ky~CO%iWbIeeP-4vYh`XGWeAYk2x z>Q%uc^Q|$-X`uV7XzB`c^mJV$?wEBL!JGGx=82OJ>L8;Tp*TG2F@iNRAOTxq-{^nV z$j9FYVUv(*j(}SHUHu+;wUKWm`*rRLeGIE=KzlslKB+>c$GmYudP9zYTJVNE+qVb5 zkT>odii;mN!tfqFw8sQQy?jb@5rVlvg`{l9A>?1Sr%D6880&JzQtxS%r6Bs^0)R|_SO*xf6J zq!23-kl&Wsa5n9U-knLp!#dW}LM=FhdBTqoZx}I%5l{k&(ua}c&)wD}{;&zNaYA8r zih3vFAw%5|)PggZCyI;hk$wJj>|VDYf)YsZqkY=ghRA$M6dtf=7lK-FZu7)Qk0$7| z#S|u&`_lA#g4Rex48uGV8PymS!dq|6XI!)qE*VJaZ4n4^8wN@KKA#= zp;UT_*vJ-}$|d+~z3`5^&c^@V?$=dlvS&4GQdrFzXaly(#}UvfmFf>Gl(XFLN$}Tt zp*`NlmCR*IM*jr7uG*78-(j?HEU>Dl0eh9@1<};%W-*s5s0BUzPc;(Sp2abY#R1nr z7!x0dt`<@+Ms&*=sfBk*AOTwvgjVb?iLUd5iT&Z_BGiI68DV^^0!>i@-P&!j(rNB6 z?%o}|g9ZDa{T;P8m2NXQqKX~qA^)rO-xY@^OwOg!mUV5k_kQ)0;4d4)K7jY#1;IRh z7OgpDgFdC#a0Kj4NDIQ7*0boMW_GB_c0VobXZWkraEuAU0?S$S{dHRuvo%Z$wcu}I z^F)_!skCxoy4L)5Z!U6p6HOQSf3v`DPEJpytr~UG8eJMBLkX;6-_d^wS?M3DzL@fi zt-IwcyC3)c5`TLS;_k#j0=C57!+lDnz0F3E^snP3c#{=on$K>UK@yEQF%WGY+LxQP zkbo_*{`i|pP3#7V`cD?IQv&~ccN&f{o;b5(CjB;k5K?~}s)cu#;LRtPX?Dt@#Vq=L zkS(&AJ3$L2kl_1c^ISu*h3x|EvPr4j`x1ChBjDA1G9&w*YR)U&4W7J(TD5VH4ce96 zc>>-AfiyeGF)mFR={y~;V^v3@`$wsc)>x@sU$A$2r5D`Bb{M4oO7vyD;z?p+{1J68 zLqGO3NG_{U$b_-D!_deO!4MBi3!r$a{@VpoT7WryoLExZ{7 z?-ljiXfN*hv`|$Mt_!oRj^FtXloP>NkE1MX09(YXkFqilrh``pAQ(y4m)*Bqeve+U7ukUOxVc zKr76T;>7Xf0F^3ch`fFdge7?Am~UkK0%Av3)q2&54j4K}92rPpO~$FympUtbs{7O3 zoAn6$9PbvrVEof5q)6mAj#MKZPRTfS@rmN2NBgC8hP|Ps#MZSpcP&rZzFol7}cpszNk)B1detb;mIRwm>*ipnJhKz&!AWR zQL06ID5@;2%Y212C&Koc)WieLsBwJ{flnuTniIFhc=ea^2SwZD>FlbI!2UOK``V9D zucRImZ>A&*EJ1?zU?poSHMXV~&9A+W-3QJR*#E{!8w)=v8I?onWA&x%sm0#J=TVZv z-D2rO-vQ#cniUmR0&^GdkBGqI%7w9gX!!%#-y>R6K9p@;_ zoa7lJZi}qyE>TWT9TLWJ;xKcL6BGK6Qy+L-7n2r$lra+pa{;#QJZ<(+)$E7`_UK^V zyI9rzUw`Tyr-mpJ@v2|((b#Or&ntty+%VWUy&mMYT@)U<$3-oAn?P=n>;fCViN)=Uf~$`lhjreY?`S*nGBKmd!8WI;+WO!)+!F z&hZeZDNYG*IbCy%+Gu+G{GlA%EkU#RtuRFnxhq$j6{l5o2{GLXFR;IzUx!j~=gs1J zg;z{qxW?Oy z*6#GBBZre1fmYRTjnN#JyV)Gi+D~4*(6AB3XeK(aJ%QykVDnU2lY@6RRBNl`njomd{TlzJ&Q+kY@C_D6yel;+Gj3aPmtPA&UxK4lj zzCcomNYluOr!roxp((07l=?MaC0cJSP;e|Efg{G)KX)2IwY3}( z*p}E1_imVaUyQ#vhB~>NXQKp1Mo_I$TH@~2wrf%L z9$d1^jYjVZqT_MR*+@k1B5f$PP9GMD7beq!o%bzR7hV;PF-h82cNE<&-6B$U9%KE1 zeZcShTEJ+Eb`H|Nm0u{RHbTFl*Nqu&Nd)y>v{;`J_zxpcw}iALt?nK}-h~tNeSe)$ z&b$F^XAN8hFw`1pewqBPK7`FYRu~RC;(-oM_@+pHkcs^|f^aRIC^CH277GD%xo5(Vx_tx0=rM zqDY{2)c9ks&oL2RZG-Na8N>2FFz164fkrmft3~ycn{UHp)W%?cpzgp>q|BR1P108> z1+xdSya~*@;Dm3`2r63eRyn<_C)3;@!F3sBTJ)y}JHJuO{?d)9`BCN1iSikq;5G-|Dvn=zBAcToGo2`87itnGtC9__xL8 z;H$c=-Dmr`mJ6$PQ5`%Rs`(q*C|H67SBu0~4OQQjtD;_>e^ti)bEG+uw6;*0;~c3@ zDLY<6E2L4cAxRb5b|#OtlH#Zfxx=#yNmy$ zIgt`ol@_)LQEf}Bv9o3IJXub}o~&RF?^2$2wjN428(@5ISd(R1eAigh<^}f1-It(z z+schkMaAGTEY^n+FHZO$cADuT%s1Z#&ynhvFh0!lgn6sa2|7^BWz|k%`TD3eiP^-B&#Wu09ugr;F z<|(FYW2|)G_6ino{RrF< z;4zuAvx*e|2BT<9QW>^yfN=&+l-w_?R=qux91<%t0;7>g8yV{>1J&Ozy38aW=|-F7LtU*@erQV@jwOtK;Or_%qr0Rj6S~Y$OFzCm zi3Iu$X=C0s{@eC(NDMWOxX-+c{^5l8vM74(e?nv)ZLi>Z8CMgy3YVm!C||m1YDshN z)=_ZPh*m*WXK6i^dbZ47_KaNX_WD$=S2sHUq>_SbOQbn*!|F`Qj$w3Z<#ibew8FKh z@#mJVnPPhKWa?OXDT_>?Rd}^=+PYSIZBNhHPdN_R-(SQIiZ}kS@vV$%Rg7hD;*iBG zve(a{Uw*l3K`UGdbK=VRSW;&M2(6Bzf*!>+Rbl%i?cFK|)1Uu6vpAvZMC$nFgm~1~ zi$y<>=EUiFVf6FRN_4DWp^SZiR_HflB^>ru%&Z2RvRi6BVhtLm!(`39uq&eX-*GlGgHT79b z`?LGNbrk<1^PXv?w8ahes;xuVItst%I8pM!Rn3SBrtNRs1ik^}-&bqOI@xw8O_KTB zAk*7X4Jr(yl;A1?OOWt!Pu5-<`?b#F{x;s6r*eN(+-grF=OyQKEWvjl)+9;)xTcD< z+oLIO&k=#dfrPbMjQ4}!W9I5qu`f;B-NRIO{}@`Jl_Sq?)jD*%rn)WQS zoL{fE_MD%T_I2spp#b{oR>}yps^E~SX%9bTujypZL<&xCN-4Q*Y5#y91o{X4*0Ajs z?eM%YrlL*u9xSt{1;u$bqjsg+n0K)aPQ)ekpl;5!sBxQMfh9v1bsUb3U&2?-SxmQE$3b%AY|T`FynR4e%h3q$lFLFbp&Ci=MXuSEumlP8p>e)iM;}TKEkm)}oY^yozT(6W9u+C(b1;p#AInBN z&O|tlB`IU*SMhh$Ai9;;RbUAc+*e+&ReJQmlN_5m3G_SC*iXh^pxocfV{0X-$+>U* zaHhu97Jf@eQs~V%-D$p6{Ug1d1+8$c#))B%7j~*zWK~OK8_O6#8rwG1329+|C9kaN zivpX4eJ4V`0VmF7CyUyvX41aI(`F=ajf-nh;}4j(V^1zNa$jAqHuOURzgQV9t&NJO zU(fUr;d=vQ9O3vifX}}qRsT7Lc4lOWqfHeV-{DALO@>0~>>!H1=0Mvn)n^1+@g5{^ zh5EGep%$|pWt`iwJx=5XD&*zhLzP!M%J^-71lDBiJ1==Jnk7Wg*wSPReq|udiS9w4 z#k%O>6g(-_f@2hEPL#xEi9VlV$hxrsYXfPVB_!!q9bd7tU=HmcJk^9F4%^_w*1T`Vtx`Mwx2~JcSESoZ@=8Hy?Y^O{)(-OEnV!P9lHCd0c;AL}Q2C8!VGpKEA^SHg)4kDi-j^Q;sy*Fmn5zFZqWW4AVB zAwQi9Ym%gMw<2gy&tl=FeUh;R&-THZ47Er=y8gH%LG@hHR7WekDo#|{_(A{u>^y2v x93Z22k-(ZH>1!twdCzV^_m|z3u>{XS!kP?K*t6ZD>({C3{8Uv#D;yb|_zxVm7n}e9 literal 0 HcmV?d00001 diff --git a/data/gripper/wsg50_with_r2d2_gripper.sdf b/data/gripper/wsg50_with_r2d2_gripper.sdf new file mode 100644 index 000000000..b022faf00 --- /dev/null +++ b/data/gripper/wsg50_with_r2d2_gripper.sdf @@ -0,0 +1,298 @@ + + + + + 0 0 0.27 3.14 0 0 + + + + + + world + base_link + + 0 0 1 + + -0.5 + 10 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1.2 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0 0 -0 0 + + + 0.2 0.05 0.05 + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/WSG50_110.stl + + + + + + + 1 + + 0 + + + + -0.055 0 0 0 -0 0 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_left + base_link + + 1 0 0 + + -0.001 + 0.055 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0.055 0 0 0 -0 3.14159 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_right + base_link + + -1 0 0 + + -0.055 + 0.001 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0.062 0 0.145 0 0 1.5708 + + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0.042 0 0 0 + + + 0.02 0.02 0.15 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/l_gripper_tip_scaled.stl + + + + + + + gripper_right + finger_right + + + + -0.062 0 0.145 0 0 4.71239 + + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0.042 0 0 0 + + + 0.02 0.02 0.15 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/l_gripper_tip_scaled.stl + + + + + + + gripper_left + finger_left + + + + diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c5901ca34..977687da3 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -249,6 +249,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT), + ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_GRASP_CONTACT), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index e06d5f205..68626554c 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -519,6 +519,8 @@ void MyStatusBarError(const char* msg) gui2->textOutput(msg); gui2->forceUpdateScrollBars(); } + btAssert(0); + } struct MyMenuItemHander :public Gwen::Event::Handler diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index a64bf44f5..597f1a4d9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -232,6 +232,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat if (mass) { compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); + //temporary inertia scaling until we load inertia from URDF + if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING) + { + localInertiaDiagonal*=contactInfo.m_inertiaScaling; + } } btRigidBody* linkRigidBody = 0; diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 65d615ee3..2a6bdfef4 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -17,7 +17,8 @@ enum URDF_LinkContactFlags { URDF_CONTACT_HAS_LATERAL_FRICTION=1, URDF_CONTACT_HAS_ROLLING_FRICTION=2, - URDF_CONTACT_HAS_CONTACT_CFM=4, + URDF_CONTACT_HAS_INERTIA_SCALING=2, + URDF_CONTACT_HAS_CONTACT_CFM=4, URDF_CONTACT_HAS_CONTACT_ERP=8 }; @@ -25,6 +26,7 @@ struct URDFLinkContactInfo { btScalar m_lateralFriction; btScalar m_rollingFriction; + btScalar m_inertiaScaling; btScalar m_contactCfm; btScalar m_contactErp; int m_flags; @@ -32,6 +34,7 @@ struct URDFLinkContactInfo URDFLinkContactInfo() :m_lateralFriction(0.5), m_rollingFriction(0), + m_inertiaScaling(1), m_contactCfm(0), m_contactErp(0) { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index a6a5e256e..3cccbfd35 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -606,6 +606,28 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi TiXmlElement* ci = config->FirstChildElement("contact"); if (ci) { + + TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling"); + if (damping_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_inertiaScaling = urdfLexicalCast(damping_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING; + } else + { + if (!damping_xml->Attribute("value")) + { + logger->reportError("Link/contact: damping element must have value attribute"); + return false; + } + + link.m_contactInfo.m_inertiaScaling = urdfLexicalCast(damping_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING; + + } + } + { TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction"); if (friction_xml) { @@ -623,6 +645,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi link.m_contactInfo.m_lateralFriction = urdfLexicalCast(friction_xml->Attribute("value")); } } + } } } diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 4da5cc22a..f1fc2cbfd 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -9,11 +9,15 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" +#include "../CommonInterfaces/CommonParameterInterface.h" #include #include "b3RobotSimAPI.h" #include "../Utils/b3Clock.h" +static btScalar sGripperVerticalVelocity = -0.2f; +static btScalar sGripperClosingTargetVelocity = 0.5f; + ///quick demo showing the right-handed coordinate system and positive rotations around each axis class R2D2GraspExample : public CommonExampleInterface { @@ -22,7 +26,8 @@ class R2D2GraspExample : public CommonExampleInterface b3RobotSimAPI m_robotSim; int m_options; int m_r2d2Index; - + int m_gripperIndex; + float m_x; float m_y; float m_z; @@ -39,6 +44,7 @@ public: m_guiHelper(helper), m_options(options), m_r2d2Index(-1), + m_gripperIndex(-1), m_x(0), m_y(0), m_z(0) @@ -61,36 +67,9 @@ public: b3Printf("robotSim connected = %d",connected); - if ((m_options & eROBOTIC_LEARN_GRASP)!=0) + if (m_options == eROBOTIC_LEARN_GRASP) { - { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "r2d2.urdf"; - args.m_startPosition.setValue(0,0,.5); - b3RobotSimLoadFileResults results; - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) - { - int m_r2d2Index = results.m_uniqueObjectIds[0]; - int numJoints = m_robotSim.getNumJoints(m_r2d2Index); - b3Printf("numJoints = %d",numJoints); - - for (int i=0;igetParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity + ); + slider.m_minVal=-1; + slider.m_maxVal=1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; + args.m_fileType = B3_SDF_FILE; + b3RobotSimLoadFileResults results; + + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + + m_gripperIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_gripperIndex); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;i=0)) + { + int fingerJointIndices[3]={0,1,3}; + double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity + ,-sGripperClosingTargetVelocity + }; + double maxTorqueValues[3]={40.0,50.0,50.0}; + for (int i=0;i<3;i++) + { + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = fingerTargetVelocities[i]; + controlArgs.m_maxTorqueValue = maxTorqueValues[i]; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); + } + } m_robotSim.stepSimulation(); } virtual void renderScene() @@ -179,9 +290,9 @@ public: virtual void resetCamera() { - float dist = 3; - float pitch = -75; - float yaw = 30; + float dist = 1.5; + float pitch = 12; + float yaw = -10; float targetPos[3]={-0.2,0.8,0.3}; if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) { diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h index 353f684e5..10d419269 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.h +++ b/examples/RoboticsLearning/R2D2GraspExample.h @@ -20,6 +20,7 @@ enum RobotLearningExampleOptions { eROBOTIC_LEARN_GRASP=1, eROBOTIC_LEARN_COMPLIANT_CONTACT=2, + eROBOTIC_LEARN_GRASP_CONTACT=3, }; class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index b29e0e429..ce9f032b7 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -560,6 +560,7 @@ void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; + b3JointControlSetKd(command,uIndex,args.m_kd); b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); @@ -627,6 +628,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS } statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); statusType = b3GetStatusType(statusHandle); + b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); robotUniqueId = b3GetStatusBodyIndex(statusHandle); results.m_uniqueObjectIds.push_back(robotUniqueId); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 98c7e1fa0..3666700d2 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -61,7 +61,7 @@ struct b3JointMotorArgs m_targetPosition(0), m_kp(0.1), m_targetVelocity(0), - m_kd(0.1), + m_kd(0.9), m_maxTorqueValue(1000) { } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 420fe37b6..469c178f8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1216,16 +1216,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); - - //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; - serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); - int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); - for (int i=0;im_sdfRecentLoadedBodies[i]; - } + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } - serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + } hasStatus = true; break; } From 8270096fad4edbcee9e07742ba239cb271bbac09 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 25 Jul 2016 12:30:47 -0700 Subject: [PATCH 07/36] add GripperGraspExample, separate from R2D2GraspExample --- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 3 +- .../RoboticsLearning/GripperGraspExample.cpp | 231 +++++++++++++++++ .../RoboticsLearning/GripperGraspExample.h | 23 ++ .../RoboticsLearning/R2D2GraspExample.cpp | 235 +++++------------- examples/RoboticsLearning/R2D2GraspExample.h | 1 - 6 files changed, 320 insertions(+), 175 deletions(-) create mode 100644 examples/RoboticsLearning/GripperGraspExample.cpp create mode 100644 examples/RoboticsLearning/GripperGraspExample.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 0678eaf4a..38e4175b3 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -210,6 +210,8 @@ SET(BulletExampleBrowser_SRCS ../RenderingExamples/TimeSeriesCanvas.h ../RenderingExamples/TimeSeriesFontData.cpp ../RenderingExamples/TimeSeriesFontData.h + ../RoboticsLearning/GripperGraspExample.cpp + ../RoboticsLearning/GripperGraspExample.h ../RoboticsLearning/b3RobotSimAPI.cpp ../RoboticsLearning/b3RobotSimAPI.h ../RoboticsLearning/R2D2GraspExample.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index cac015d61..60c62c43a 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -46,6 +46,7 @@ #include "../MultiThreading/MultiThreadingExample.h" #include "../InverseDynamics/InverseDynamicsExample.h" #include "../RoboticsLearning/R2D2GraspExample.h" +#include "../RoboticsLearning/GripperGraspExample.h" #include "../InverseKinematics/InverseKinematicsExample.h" #ifdef ENABLE_LUA @@ -260,7 +261,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT), - ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_GRASP_CONTACT), + ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc), diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp new file mode 100644 index 000000000..ebdc39acb --- /dev/null +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -0,0 +1,231 @@ + +#include "GripperGraspExample.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include + +#include "b3RobotSimAPI.h" +#include "../Utils/b3Clock.h" + +static btScalar sGripperVerticalVelocity = -0.2f; +static btScalar sGripperClosingTargetVelocity = 0.5f; + +class GripperGraspExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimAPI m_robotSim; + int m_options; + int m_r2d2Index; + int m_gripperIndex; + + float m_x; + float m_y; + float m_z; + b3AlignedObjectArray m_movingInstances; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + GripperGraspExample(GUIHelperInterface* helper, int options) + :m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options), + m_r2d2Index(-1), + m_gripperIndex(-1), + m_x(0), + m_y(0), + m_z(0) + { + m_app->setUpAxis(2); + } + virtual ~GripperGraspExample() + { + m_app->m_renderer->enableBlend(false); + } + + + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + bool connected = m_robotSim.connect(m_guiHelper); + b3Printf("robotSim connected = %d",connected); + + { + + { + SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); + slider.m_minVal=-2; + slider.m_maxVal=2; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity + ); + slider.m_minVal=-1; + slider.m_maxVal=1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; + args.m_fileType = B3_SDF_FILE; + b3RobotSimLoadFileResults results; + + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + + m_gripperIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_gripperIndex); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;i=0)) + { + int fingerJointIndices[3]={0,1,3}; + double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity + ,-sGripperClosingTargetVelocity + }; + double maxTorqueValues[3]={40.0,50.0,50.0}; + for (int i=0;i<3;i++) + { + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = fingerTargetVelocities[i]; + controlArgs.m_maxTorqueValue = maxTorqueValues[i]; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); + } + } + m_robotSim.stepSimulation(); + } + virtual void renderScene() + { + m_robotSim.renderScene(); + + //m_app->m_renderer->renderScene(); + } + + + virtual void physicsDebugDraw() + { + + } + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 1.5; + float pitch = 12; + float yaw = -10; + float targetPos[3]={-0.2,0.8,0.3}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]); + } + } + +}; + + +class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new GripperGraspExample(options.m_guiHelper, options.m_option); +} + + + diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h new file mode 100644 index 000000000..1088efffb --- /dev/null +++ b/examples/RoboticsLearning/GripperGraspExample.h @@ -0,0 +1,23 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2016 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef GRIPPER_GRASP_EXAMPLE_H +#define GRIPPER_GRASP_EXAMPLE_H + + +class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //GRIPPER_GRASP_EXAMPLE_H diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index f1fc2cbfd..4da5cc22a 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -9,15 +9,11 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" -#include "../CommonInterfaces/CommonParameterInterface.h" #include #include "b3RobotSimAPI.h" #include "../Utils/b3Clock.h" -static btScalar sGripperVerticalVelocity = -0.2f; -static btScalar sGripperClosingTargetVelocity = 0.5f; - ///quick demo showing the right-handed coordinate system and positive rotations around each axis class R2D2GraspExample : public CommonExampleInterface { @@ -26,8 +22,7 @@ class R2D2GraspExample : public CommonExampleInterface b3RobotSimAPI m_robotSim; int m_options; int m_r2d2Index; - int m_gripperIndex; - + float m_x; float m_y; float m_z; @@ -44,7 +39,6 @@ public: m_guiHelper(helper), m_options(options), m_r2d2Index(-1), - m_gripperIndex(-1), m_x(0), m_y(0), m_z(0) @@ -67,9 +61,36 @@ public: b3Printf("robotSim connected = %d",connected); - if (m_options == eROBOTIC_LEARN_GRASP) + if ((m_options & eROBOTIC_LEARN_GRASP)!=0) { - + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "r2d2.urdf"; + args.m_startPosition.setValue(0,0,.5); + b3RobotSimLoadFileResults results; + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + int m_r2d2Index = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_r2d2Index); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;igetParameterInterface()->registerSliderFloatParameter(slider); - } - - { - SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity - ); - slider.m_minVal=-1; - slider.m_maxVal=1; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - - - { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; - args.m_fileType = B3_SDF_FILE; - b3RobotSimLoadFileResults results; - - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) - { - - m_gripperIndex = results.m_uniqueObjectIds[0]; - int numJoints = m_robotSim.getNumJoints(m_gripperIndex); - b3Printf("numJoints = %d",numJoints); - - for (int i=0;i=0)) - { - int fingerJointIndices[3]={0,1,3}; - double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity - ,-sGripperClosingTargetVelocity - }; - double maxTorqueValues[3]={40.0,50.0,50.0}; - for (int i=0;i<3;i++) - { - b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); - controlArgs.m_targetVelocity = fingerTargetVelocities[i]; - controlArgs.m_maxTorqueValue = maxTorqueValues[i]; - controlArgs.m_kd = 1.; - m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); - } - } m_robotSim.stepSimulation(); } virtual void renderScene() @@ -290,9 +179,9 @@ public: virtual void resetCamera() { - float dist = 1.5; - float pitch = 12; - float yaw = -10; + float dist = 3; + float pitch = -75; + float yaw = 30; float targetPos[3]={-0.2,0.8,0.3}; if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) { diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h index 10d419269..353f684e5 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.h +++ b/examples/RoboticsLearning/R2D2GraspExample.h @@ -20,7 +20,6 @@ enum RobotLearningExampleOptions { eROBOTIC_LEARN_GRASP=1, eROBOTIC_LEARN_COMPLIANT_CONTACT=2, - eROBOTIC_LEARN_GRASP_CONTACT=3, }; class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options); From b221d2ad18bfec48efeccbe24302aac206948d7f Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Tue, 26 Jul 2016 11:09:12 -0700 Subject: [PATCH 08/36] add targetPosition, targetValue, kp & kd to pybullet_setJointMotorControl --- examples/pybullet/pybullet.c | 72 +++++++++++++++++++++--------------- 1 file changed, 42 insertions(+), 30 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index be5994c16..2bd48e963 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -294,27 +294,29 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { - //todo(erwincoumans): set max forces, kp, kd + //TODO(matkelcey): should this just be three methods? int size; int bodyIndex, jointIndex, controlMode; double targetValue=0; + double targetPosition=0; + double targetVelocity=0; double maxForce=100000; - double gains=0.1; + double kp=0.1; + double kd=0.1; int valid = 0; - - + if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + size= PySequence_Size(args); - if (size==4) { - + // for CONTROL_MODE_VELOCITY targetValue -> velocity + // for CONTROL_MODE_TORQUE targetValue -> force torque if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { PyErr_SetString(SpamError, "Error parsing arguments"); @@ -324,7 +326,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) } if (size==5) { - + // for CONTROL_MODE_VELOCITY targetValue -> velocity + // for CONTROL_MODE_TORQUE targetValue -> force torque if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) { PyErr_SetString(SpamError, "Error parsing arguments"); @@ -332,20 +335,31 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) } valid = 1; } - if (size==6) + if (size==8) { - - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains)) + // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. + if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd)) { PyErr_SetString(SpamError, "Error parsing arguments"); return NULL; } valid = 1; } - - + + if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD) + { + PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD"); + return NULL; + } + if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8) + { + PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity"); + return NULL; + } + if (valid) { + int numJoints; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -357,7 +371,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } - + if ((controlMode != CONTROL_MODE_VELOCITY) && (controlMode != CONTROL_MODE_TORQUE) && (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) @@ -365,19 +379,18 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Illegral control mode."); return NULL; } - + commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - + switch (controlMode) { case CONTROL_MODE_VELOCITY: { - double kd = gains; b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); - b3JointControlSetKd(commandHandle,info.m_uIndex,kd); - b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } @@ -386,31 +399,30 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); break; } - + case CONTROL_MODE_POSITION_VELOCITY_PD: { - double kp = gains; - b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue); - b3JointControlSetKp(commandHandle,info.m_uIndex,kp); - b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); break; } default: { } }; - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - + Py_INCREF(Py_None); return Py_None; } - PyErr_SetString(SpamError, "error in setJointControl."); + PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl."); return NULL; } - - static PyObject * pybullet_setRealTimeSimulation(PyObject* self, PyObject* args) { From 4b75c5d9d0a95763152f0f9f8bc2d0d4876d7cb2 Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Tue, 26 Jul 2016 11:36:34 -0700 Subject: [PATCH 09/36] add another combo of args to allow kd to be configured in the CONTROL_MODE_VELOCITY case --- examples/pybullet/pybullet.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2bd48e963..0a1a205bb 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -335,6 +335,15 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) } valid = 1; } + if (size==6) + { + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } if (size==8) { // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. From 98c6181ba85aa90cd98a5c8a87f40b9cfc922766 Mon Sep 17 00:00:00 2001 From: "Erwin Coumans (Google)" Date: Tue, 26 Jul 2016 15:36:21 -0700 Subject: [PATCH 10/36] fix screen width/height issue on Intel/Linux add command-line option to set png_skip_frames when taking screenshot series --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 40 ++++++++++--------- .../InverseKinematicsExample.cpp | 2 +- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 6 ++- examples/OpenGLWindow/X11OpenGLWindow.cpp | 3 ++ 4 files changed, 29 insertions(+), 22 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 577d24c85..d0fae2254 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -157,6 +157,7 @@ void deleteDemo() } const char* gPngFileName = 0; +int gPngSkipFrames = 0; @@ -763,7 +764,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) loadCurrentSettings(startFileName, args); args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep); - + args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames); ///The OpenCL rigid body pipeline is experimental and ///most OpenCL drivers and OpenCL compilers have issues with our kernels. ///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers @@ -1101,24 +1102,6 @@ void OpenGLExampleBrowser::update(float deltaTime) //printf("---------------------------------------------------\n"); //printf("Framecount = %d\n",frameCount); - if (gPngFileName) - { - - static int skip = 0; - skip++; - if (skip>4) - { - skip=0; - //printf("gPngFileName=%s\n",gPngFileName); - static int s_frameCount = 100; - - sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++); - //b3Printf("Made screenshot %s",staticPngFileName); - s_app->dumpNextFrameToPng(staticPngFileName); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - } - } - if (gFixedTimeStep>0) { @@ -1153,6 +1136,25 @@ void OpenGLExampleBrowser::update(float deltaTime) } } + if (gPngFileName) + { + + static int skip = 0; + skip--; + if (skip<0) + { + skip=gPngSkipFrames; + //printf("gPngFileName=%s\n",gPngFileName); + static int s_frameCount = 100; + + sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++); + //b3Printf("Made screenshot %s",staticPngFileName); + s_app->dumpNextFrameToPng(staticPngFileName); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + } + } + + { if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera()) diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 8f902b008..4c36037a6 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -84,7 +84,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian) // Update target positions void UpdateTargets( double T2, Tree & treeY) { - double T = T2 / 20.; + double T = T2 / 5.; target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T)); } diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 1423f2841..971df3734 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -753,8 +753,10 @@ void SimpleOpenGL3App::swapBuffer() m_window->endRendering(); if (m_data->m_frameDumpPngFileName) { - writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(), - (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName, + int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); + int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(); + writeTextureToFile(width, + height,m_data->m_frameDumpPngFileName, m_data->m_ffmpegFile); m_data->m_renderTexture->disable(); if (m_data->m_ffmpegFile==0) diff --git a/examples/OpenGLWindow/X11OpenGLWindow.cpp b/examples/OpenGLWindow/X11OpenGLWindow.cpp index 68aa22720..789be7803 100644 --- a/examples/OpenGLWindow/X11OpenGLWindow.cpp +++ b/examples/OpenGLWindow/X11OpenGLWindow.cpp @@ -516,6 +516,9 @@ void X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) m_data->m_dpy = MyXOpenDisplay(NULL); + m_data->m_glWidth = ci.m_width; + m_data->m_glHeight = ci.m_height; + if(m_data->m_dpy == NULL) { printf("\n\tcannot connect to X server\n\n"); exit(0); From b4cfee87453f22b82647d1563aff34a3f59ee4ec Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Tue, 26 Jul 2016 16:15:08 -0700 Subject: [PATCH 11/36] set dft for kd to be 1.0. note: this is only applicable to CONTROL_MODE_VELOCITY --- examples/pybullet/pybullet.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0a1a205bb..d2b7a496c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -294,8 +294,6 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { - //TODO(matkelcey): should this just be three methods? - int size; int bodyIndex, jointIndex, controlMode; double targetValue=0; @@ -303,7 +301,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) double targetVelocity=0; double maxForce=100000; double kp=0.1; - double kd=0.1; + double kd=1.0; int valid = 0; if (0==sm) From daf91de77878858611c601ed89ff14651eb3fb44 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 27 Jul 2016 09:11:08 -0700 Subject: [PATCH 12/36] fix ffmpeg stream to create quicktime compatible videos --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 971df3734..7cfc75652 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -750,7 +750,7 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char* void SimpleOpenGL3App::swapBuffer() { - m_window->endRendering(); + if (m_data->m_frameDumpPngFileName) { int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); @@ -764,7 +764,8 @@ void SimpleOpenGL3App::swapBuffer() m_data->m_frameDumpPngFileName = 0; } } - m_window->startRendering(); + m_window->endRendering(); + m_window->startRendering(); } // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/ @@ -780,7 +781,7 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) #else sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " - "-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName); + "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName); #endif //sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " From 626a913866158e5956cfa2ae7f5f6350e99d783e Mon Sep 17 00:00:00 2001 From: MiCroN3000 Date: Thu, 28 Jul 2016 20:15:38 +0200 Subject: [PATCH 13/36] The kinematic character controller with various fixes and a few new features like, being able to set any vector for gravity/up, jumping in a certain direction, possibility to use collision masks, angular & linear velocity, angular & linear damping. --- .../btCharacterControllerInterface.h | 2 +- .../btKinematicCharacterController.cpp | 461 +++++++++++++----- .../btKinematicCharacterController.h | 60 ++- 3 files changed, 395 insertions(+), 128 deletions(-) diff --git a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h index dffb06dfe..c3a3ac6c8 100644 --- a/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -37,7 +37,7 @@ public: virtual void preStep ( btCollisionWorld* collisionWorld) = 0; virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; virtual bool canJump () const = 0; - virtual void jump () = 0; + virtual void jump(const btVector3& dir = btVector3()) = 0; virtual bool onGround () const = 0; virtual void setUpInterpolate (bool value) = 0; diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 31faf1df5..60e49b750 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector return direction - parallelComponent(direction, normal); } -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) +btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up) { - m_upAxis = upAxis; + m_up.setValue(0.0f, 0.0f, 1.0f); + m_jumpAxis.setValue(0.0f, 0.0f, 1.0f); + setUp(up); + setStepHeight(stepHeight); m_addedMargin = 0.02; - m_walkDirection.setValue(0,0,0); + m_walkDirection.setValue(0.0,0.0,0.0); + m_AngVel.setValue(0.0, 0.0, 0.0); m_useGhostObjectSweepTest = true; m_ghostObject = ghostObject; - m_stepHeight = stepHeight; m_turnAngle = btScalar(0.0); m_convexShape=convexShape; m_useWalkDirection = true; // use walk direction by default, legacy behavior m_velocityTimeInterval = 0.0; m_verticalVelocity = 0.0; m_verticalOffset = 0.0; - m_gravity = 9.8 * 3 ; // 3G acceleration. + m_gravity = 9.8 * 3.0 ; // 3G acceleration. m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. m_jumpSpeed = 10.0; // ? + m_SetjumpSpeed = m_jumpSpeed; m_wasOnGround = false; m_wasJumping = false; m_interpolateUp = true; setMaxSlope(btRadians(45.0)); - m_currentStepOffset = 0; + m_currentStepOffset = 0.0; + m_maxPenetrationDepth = 0.2; full_drop = false; bounce_fix = false; + m_linearDamping = btScalar(0.0); + m_angularDamping = btScalar(0.0); } btKinematicCharacterController::~btKinematicCharacterController () @@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - btScalar maxPen = btScalar(0.0); +// btScalar maxPen = btScalar(0.0); for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) { m_manifoldArray.resize(0); @@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) continue; + + if (!needsCollision(obj0, obj1)) + continue; if (collisionPair->m_algorithm) collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); @@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* btScalar dist = pt.getDistance(); - if (dist < 0.0) + if (dist < -m_maxPenetrationDepth) { - if (dist < maxPen) - { - maxPen = dist; - m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? + // TODO: cause problems on slopes, not sure if it is needed + //if (dist < maxPen) + //{ + // maxPen = dist; + // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? - } + //} m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); penetration = true; } else { @@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* void btKinematicCharacterController::stepUp ( btCollisionWorld* world) { + btScalar stepHeight = 0.0f; + if (m_verticalVelocity < 0.0) + stepHeight = m_stepHeight; + // phase 1: up btTransform start, end; - m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f)); start.setIdentity (); end.setIdentity (); /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); + start.setOrigin(m_currentPosition); + + m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f)); + m_currentPosition = m_targetPosition; + end.setOrigin (m_targetPosition); - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071)); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) } else { - world->convexSweepTest (m_convexShape, start, end, callback); + world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); } - - if (callback.hasHit()) + + if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // Only modify the position if the hit was a slope and not a wall or ceiling. - if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) + if (callback.m_hitNormalWorld.dot(m_up) > 0.0) { // we moved up only a fraction of the step height - m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; + m_currentStepOffset = stepHeight * callback.m_closestHitFraction; if (m_interpolateUp == true) m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); else m_currentPosition = m_targetPosition; } - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; + + btTransform& xform = m_ghostObject->getWorldTransform(); + xform.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(xform); + + // fix penetration if we hit a ceiling for example + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(world)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + m_targetPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_currentPosition = m_targetPosition; + + if (m_verticalOffset > 0) + { + m_verticalOffset = 0.0; + m_verticalVelocity = 0.0; + m_currentStepOffset = m_stepHeight; + } } else { - m_currentStepOffset = m_stepHeight; + m_currentStepOffset = stepHeight; m_currentPosition = m_targetPosition; } } +bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) +{ + bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0; + collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask); + return collides; +} + void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) { btVector3 movementDirection = m_targetPosition - m_currentPosition; @@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); // phase 2: forward and strafe btTransform start, end; + m_targetPosition = m_currentPosition + walkMove; start.setIdentity (); @@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); // printf("distance2=%f\n",distance2); - if (m_touchingContact) - { - if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) - { - //interferes with step movement - //updateTargetPositionBasedOnCollision (m_touchingNormal); - } - } - int maxIter = 10; while (fraction > btScalar(0.01) && maxIter-- > 0) @@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co end.setOrigin (m_targetPosition); btVector3 sweepDirNegative(m_currentPosition - m_targetPosition); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0)); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar margin = m_convexShape->getMargin(); m_convexShape->setMargin(margin + m_addedMargin); - - if (m_useGhostObjectSweepTest) + if (!(start == end)) { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + else + { + collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } } - m_convexShape->setMargin(margin); fraction -= callback.m_closestHitFraction; - if (callback.hasHit()) + if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // we moved only a fraction //btScalar hitDistance; @@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co } } else { - // we moved whole way - m_currentPosition = m_targetPosition; +// if (!m_wasJumping) + // we moved whole way + m_currentPosition = m_targetPosition; } + m_currentPosition = m_targetPosition; // if (callback.m_closestHitFraction == 0.f) // break; @@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld // phase 3: down /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); + btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep); btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; - btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; + btVector3 gravity_drop = m_up * downVelocity; m_targetPosition -= (step_drop + gravity_drop);*/ btVector3 orig_position = m_targetPosition; btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if (m_verticalVelocity > 0.0) + return; + if(downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) downVelocity = m_fallSpeed; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine); callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld start.setOrigin (m_currentPosition); end.setOrigin (m_targetPosition); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + //set double test for 2x the step drop, to check for a large drop vs small drop end_double.setOrigin (m_targetPosition - step_drop); @@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit()) + if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); @@ -471,30 +529,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit()) - { - //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) - collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } + if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) + { + //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) + collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } } btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; bool has_hit = false; if (bounce_fix == true) - has_hit = callback.hasHit() || callback2.hasHit(); + has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); else - has_hit = callback2.hasHit(); + has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); - if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false + btScalar stepHeight = 0.0f; + if (m_verticalVelocity < 0.0) + stepHeight = m_stepHeight; + + if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping)) { //redo the velocity calculation when falling a small amount, for fast stairs motion //for larger falls, use the smoother/slower interpolated movement by not touching the target position m_targetPosition = orig_position; - downVelocity = m_stepHeight; + downVelocity = stepHeight; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; continue; //re-run previous tests @@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld break; } - if (callback.hasHit() || runonce == true) + if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // we dropped a fraction of the height -> hit floor - btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); @@ -513,10 +574,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld if (bounce_fix == true) { if (full_drop == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - else - //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + else + //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); } else m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); @@ -528,7 +589,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld m_wasJumping = false; } else { // we dropped the full height - + full_drop = true; if (bounce_fix == true) @@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; - step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } @@ -579,21 +640,63 @@ btScalar timeInterval m_velocityTimeInterval += timeInterval; } +void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity) +{ + m_AngVel = velocity; +} + +const btVector3& btKinematicCharacterController::getAngularVelocity() const +{ + return m_AngVel; +} + +void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity) +{ + m_walkDirection = velocity; + + // HACK: if we are moving in the direction of the up, treat it as a jump :( + if (m_walkDirection.length2() > 0) + { + btVector3 w = velocity.normalized(); + btScalar c = w.dot(m_up); + if (c != 0) + { + //there is a component in walkdirection for vertical velocity + btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length()); + m_walkDirection -= upComponent; + m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length(); + + if (c > 0.0f) + { + m_wasJumping = true; + m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin(); + } + } + } + else + m_verticalVelocity = 0.0f; +} + +btVector3 btKinematicCharacterController::getLinearVelocity() const +{ + return m_walkDirection + (m_verticalVelocity * m_up); +} + void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) { - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; - m_wasOnGround = false; - m_wasJumping = false; - m_walkDirection.setValue(0,0,0); - m_velocityTimeInterval = 0.0; + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasOnGround = false; + m_wasJumping = false; + m_walkDirection.setValue(0,0,0); + m_velocityTimeInterval = 0.0; - //clear pair cache - btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); - while (cache->getOverlappingPairArray().size() > 0) - { - cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); - } + //clear pair cache + btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); + while (cache->getOverlappingPairArray().size() > 0) + { + cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); + } } void btKinematicCharacterController::warp (const btVector3& origin) @@ -607,62 +710,99 @@ void btKinematicCharacterController::warp (const btVector3& origin) void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) { - - int numPenetrationLoops = 0; - m_touchingContact = false; - while (recoverFromPenetration (collisionWorld)) - { - numPenetrationLoops++; - m_touchingContact = true; - if (numPenetrationLoops > 4) - { - //printf("character could not recover from penetration = %d\n", numPenetrationLoops); - break; - } - } - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); m_targetPosition = m_currentPosition; + + m_currentOrientation = m_ghostObject->getWorldTransform().getRotation(); + m_targetOrientation = m_currentOrientation; // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); - - } -#include - void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) { // printf("playerStep(): "); // printf(" dt = %f", dt); + if (m_AngVel.length2() > 0.0f) + { + m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt); + } + + // integrate for angular velocity + if (m_AngVel.length2() > 0.0f) + { + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + + btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt); + + btQuaternion orn = rot * xform.getRotation(); + + xform.setRotation(orn); + m_ghostObject->setWorldTransform(xform); + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; + m_currentOrientation = m_ghostObject->getWorldTransform().getRotation(); + m_targetOrientation = m_currentOrientation; + } + // quick check... - if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) { + if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) { // printf("\n"); return; // no motion } m_wasOnGround = onGround(); + //btVector3 lvel = m_walkDirection; + btScalar c = 0.0f; + + if (m_walkDirection.length2() > 0) + { + // apply damping + m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt); + } + + m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt); + // Update fall velocity. m_verticalVelocity -= m_gravity * dt; - if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) { m_verticalVelocity = m_jumpSpeed; } - if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) { m_verticalVelocity = -btFabs(m_fallSpeed); } m_verticalOffset = m_verticalVelocity * dt; - btTransform xform; - xform = m_ghostObject->getWorldTransform (); + xform = m_ghostObject->getWorldTransform(); // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); // printf("walkSpeed=%f\n",walkSpeed); - stepUp (collisionWorld); + stepUp(collisionWorld); + //todo: Experimenting with behavior of controller when it hits a ceiling.. + //bool hitUp = stepUp (collisionWorld); + //if (hitUp) + //{ + // m_verticalVelocity -= m_gravity * dt; + // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + // { + // m_verticalVelocity = m_jumpSpeed; + // } + // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + // { + // m_verticalVelocity = -btFabs(m_fallSpeed); + // } + // m_verticalOffset = m_verticalVelocity * dt; + + // xform = m_ghostObject->getWorldTransform(); + //} + if (m_useWalkDirection) { stepForwardAndStrafe (collisionWorld, m_walkDirection); } else { @@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo } stepDown (collisionWorld, dt); + //todo: Experimenting with max jump height + //if (m_wasJumping) + //{ + // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis]; + // if (ds > m_maxJumpHeight) + // { + // // substract the overshoot + // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight; + + // // max height was reached, so potential energy is at max + // // and kinematic energy is 0, thus velocity is 0. + // if (m_verticalVelocity > 0.0) + // m_verticalVelocity = 0.0; + // } + //} // printf("\n"); xform.setOrigin (m_currentPosition); m_ghostObject->setWorldTransform (xform); + + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(collisionWorld)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } } void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) @@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) { m_jumpSpeed = jumpSpeed; + m_SetjumpSpeed = m_jumpSpeed; } void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) @@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const return onGround(); } -void btKinematicCharacterController::jump () +void btKinematicCharacterController::jump(const btVector3& v) { - if (!canJump()) - return; - + m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length(); m_verticalVelocity = m_jumpSpeed; m_wasJumping = true; + m_jumpAxis = v.length2() == 0 ? m_up : v.normalized(); + + m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin(); + #if 0 currently no jumping. btTransform xform; @@ -727,14 +898,16 @@ void btKinematicCharacterController::jump () #endif } -void btKinematicCharacterController::setGravity(btScalar gravity) +void btKinematicCharacterController::setGravity(const btVector3& gravity) { - m_gravity = gravity; + if (gravity.length2() > 0) setUpVector(-gravity); + + m_gravity = gravity.length(); } -btScalar btKinematicCharacterController::getGravity() const +btVector3 btKinematicCharacterController::getGravity() const { - return m_gravity; + return -m_gravity * m_up; } void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) @@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const return m_maxSlopeRadians; } -bool btKinematicCharacterController::onGround () const +void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d) { - return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; + m_maxPenetrationDepth = d; } +btScalar btKinematicCharacterController::getMaxPenetrationDepth() const +{ + return m_maxPenetrationDepth; +} + +bool btKinematicCharacterController::onGround () const +{ + return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON); +} + +void btKinematicCharacterController::setStepHeight(btScalar h) +{ + m_stepHeight = h; +} btVector3* btKinematicCharacterController::getUpAxisDirections() { @@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value) { m_interpolateUp = value; } + +void btKinematicCharacterController::setUp(const btVector3& up) +{ + if (up.length2() > 0 && m_gravity > 0.0f) + { + setGravity(-m_gravity * up.normalized()); + return; + } + + setUpVector(up); +} + +void btKinematicCharacterController::setUpVector(const btVector3& up) +{ + if (m_up == up) + return; + + btVector3 u = m_up; + + if (up.length2() > 0) + m_up = up.normalized(); + else + m_up = btVector3(0.0, 0.0, 0.0); + + if (!m_ghostObject) return; + btQuaternion rot = getRotation(m_up, u); + + //set orientation with new up + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + btQuaternion orn = rot.inverse() * xform.getRotation(); + xform.setRotation(orn); + m_ghostObject->setWorldTransform(xform); +} + +btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const +{ + if (v0.length2() == 0.0f || v1.length2() == 0.0f) + { + btQuaternion q; + return q; + } + + return shortestArcQuatNormalize2(v0, v1); +} + diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h index add6f30a6..3d677e647 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -43,10 +43,12 @@ protected: btPairCachingGhostObject* m_ghostObject; btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast + btScalar m_maxPenetrationDepth; btScalar m_verticalVelocity; btScalar m_verticalOffset; btScalar m_fallSpeed; btScalar m_jumpSpeed; + btScalar m_SetjumpSpeed; btScalar m_maxJumpHeight; btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value) btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization) @@ -61,24 +63,34 @@ protected: ///this is the desired walk direction, set by the user btVector3 m_walkDirection; btVector3 m_normalizedDirection; + btVector3 m_AngVel; + + btVector3 m_jumpPosition; //some internal variables btVector3 m_currentPosition; btScalar m_currentStepOffset; btVector3 m_targetPosition; + btQuaternion m_currentOrientation; + btQuaternion m_targetOrientation; + ///keep track of the contact manifolds btManifoldArray m_manifoldArray; bool m_touchingContact; btVector3 m_touchingNormal; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_wasOnGround; bool m_wasJumping; bool m_useGhostObjectSweepTest; bool m_useWalkDirection; btScalar m_velocityTimeInterval; - int m_upAxis; + btVector3 m_up; + btVector3 m_jumpAxis; static btVector3* getUpAxisDirections(); bool m_interpolateUp; @@ -94,11 +106,18 @@ protected: void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); void stepDown (btCollisionWorld* collisionWorld, btScalar dt); + + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); + + void setUpVector(const btVector3& up); + + btQuaternion getRotation(btVector3& v0, btVector3& v1) const; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); + btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0)); ~btKinematicCharacterController (); @@ -112,14 +131,9 @@ public: ///btActionInterface interface void debugDraw(btIDebugDraw* debugDrawer); - void setUpAxis (int axis) - { - if (axis < 0) - axis = 0; - if (axis > 2) - axis = 2; - m_upAxis = axis; - } + void setUp(const btVector3& up); + + const btVector3& getUp() { return m_up; } /// This should probably be called setPositionIncrementPerSimulatorStep. /// This is neither a direction nor a velocity, but the amount to @@ -136,27 +150,47 @@ public: virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval); + virtual void setAngularVelocity(const btVector3& velocity); + virtual const btVector3& getAngularVelocity() const; + + virtual void setLinearVelocity(const btVector3& velocity); + virtual btVector3 getLinearVelocity() const; + + void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } + btScalar getLinearDamping() const { return m_linearDamping; } + void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } + btScalar getAngularDamping() const { return m_angularDamping; } + void reset ( btCollisionWorld* collisionWorld ); void warp (const btVector3& origin); void preStep ( btCollisionWorld* collisionWorld); void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); + void setStepHeight(btScalar h); + btScalar getStepHeight() const { return m_stepHeight; } void setFallSpeed (btScalar fallSpeed); + btScalar getFallSpeed() const { return m_fallSpeed; } void setJumpSpeed (btScalar jumpSpeed); + btScalar getJumpSpeed() const { return m_jumpSpeed; } void setMaxJumpHeight (btScalar maxJumpHeight); bool canJump () const; - void jump (); + void jump(const btVector3& v = btVector3()); - void setGravity(btScalar gravity); - btScalar getGravity() const; + void applyImpulse(const btVector3& v) { jump(v); } + + void setGravity(const btVector3& gravity); + btVector3 getGravity() const; /// The max slope determines the maximum angle that the controller can walk up. /// The slope angle is measured in radians. void setMaxSlope(btScalar slopeRadians); btScalar getMaxSlope() const; + void setMaxPenetrationDepth(btScalar d); + btScalar getMaxPenetrationDepth() const; + btPairCachingGhostObject* getGhostObject(); void setUseGhostSweepTest(bool useGhostObjectSweepTest) { From a7a9182953a5524c3ec49b6a96058583b9b8b0ef Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Thu, 28 Jul 2016 16:47:05 -0700 Subject: [PATCH 14/36] fix warning: extra tokens at end of #endif directive --- examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 2 +- examples/ThirdPartyLibs/BussIK/VectorRn.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 544dd921e..1d3eda690 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -399,4 +399,4 @@ inline void MatrixRmn::AddArrayScale( long length, const double* from, long from -#endif MATRIX_RMN_H \ No newline at end of file +#endif //MATRIX_RMN_H diff --git a/examples/ThirdPartyLibs/BussIK/VectorRn.h b/examples/ThirdPartyLibs/BussIK/VectorRn.h index 8357f9997..99bd67da5 100644 --- a/examples/ThirdPartyLibs/BussIK/VectorRn.h +++ b/examples/ThirdPartyLibs/BussIK/VectorRn.h @@ -235,4 +235,4 @@ inline double Dot( const VectorRn& u, const VectorRn& v ) return res; } -#endif VECTOR_RN_H \ No newline at end of file +#endif //VECTOR_RN_H From 72e329962ec94dec0e6ee324a157f31073195fcd Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 28 Jul 2016 18:06:03 -0700 Subject: [PATCH 15/36] fix ffmpeg mp4 generation under Windows as well. remove static variables, make them local, to avoid multithreading issues. --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 7 +++- .../Featherstone/btMultiBody.cpp | 42 +++++++++---------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 7cfc75652..25d283119 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -776,8 +776,11 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) char cmd[8192]; #ifdef _WIN32 - sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " - "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName); + sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " + "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName); + + //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " + // "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName); #else sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index c683a179e..d56f1db14 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -299,7 +299,7 @@ void btMultiBody::setupPlanar(int i, m_links[i].m_eVector = parentComToThisComOffset; // - static btVector3 vecNonParallelToRotAxis(1, 0, 0); + btVector3 vecNonParallelToRotAxis(1, 0, 0); if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) vecNonParallelToRotAxis.setValue(0, 1, 0); // @@ -714,15 +714,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btScalar * Y = &scratch_r[0]; // //aux variables - static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) - static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child - static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp - static btSpatialTransformationMatrix fromWorld; + btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) + btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; //spatial transform from parent to child + btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + btSpatialTransformationMatrix fromWorld; fromWorld.m_trnVec.setZero(); ///////////////// @@ -919,8 +919,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar case btMultibodyLink::eSpherical: case btMultibodyLink::ePlanar: { - static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); //unroll the loop? for(int row = 0; row < 3; ++row) @@ -1323,11 +1323,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar btScalar * Y = r_ptr; //////////////// //aux variables - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; ///////////////// // First 'upward' loop. @@ -1522,8 +1522,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd btScalar *pBaseQuat = pq ? pq : m_baseQuat; btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) // - static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); pQuatUpdateFun(baseOmega, baseQuat, true, dt); pBaseQuat[0] = baseQuat.x(); pBaseQuat[1] = baseQuat.y(); @@ -1557,8 +1557,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::eSpherical: { - static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); pQuatUpdateFun(jointVel, jointOri, false, dt); pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); break; From 80dfec170b5d28885f2841629a22aa7b04f42379 Mon Sep 17 00:00:00 2001 From: Mat Kelcey Date: Fri, 29 Jul 2016 14:43:55 -0700 Subject: [PATCH 16/36] fix setidentity bug --- examples/ThirdPartyLibs/BussIK/LinearR4.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h index 51f57a666..b0c542249 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR4.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h @@ -580,8 +580,11 @@ inline Matrix4x4::Matrix4x4 ( const Matrix4x4& A) inline void Matrix4x4::SetIdentity ( ) { - m11 = m22 = m33 = m44 = 1.0; - m12 = m13 = m14 = m21 = m23 = m24 = m13 = m23 = m41= m42 = m43 = 0.0; + m12 = m13 = m14 = + m21 = m23 = m24 = + m31 = m32 = m34 = + m41 = m42 = m43 = 0.0; + m11 = m22 = m33 = m44 = 1.0; } inline void Matrix4x4::Set( const VectorR4& u, const VectorR4& v, From b759ab8a913126697cbd4bc76dbb3cca4b245ecf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Aug 2016 23:45:14 -0700 Subject: [PATCH 17/36] fix overflow visual shape in COLLADA .dae file importer. --- .../ImportColladaDemo/LoadMeshFromCollada.cpp | 76 ++++++++++--------- 1 file changed, 41 insertions(+), 35 deletions(-) diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp index b9a9479e0..8b47f5552 100644 --- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp +++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp @@ -28,6 +28,7 @@ subject to the following restrictions: #include "btMatrix4x4.h" +#define MAX_VISUAL_SHAPES 512 struct VertexSource @@ -288,42 +289,47 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray; - visualShape.m_indices = new b3AlignedObjectArray; - int indexBase = 0; + if (shapeIndex; + visualShape.m_indices = new b3AlignedObjectArray; + int indexBase = 0; - btAssert(vertexNormals.size()==vertexPositions.size()); - for (int v=0;vpush_back(vtx); - } + btAssert(vertexNormals.size()==vertexPositions.size()); + for (int v=0;vpush_back(vtx); + } - for (int index=0;indexpush_back(indices[index]+indexBase); - } - - - //b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size()); - indexBase=visualShape.m_vertices->size(); - visualShape.m_numIndices = visualShape.m_indices->size(); - visualShape.m_numvertices = visualShape.m_vertices->size(); - } - //b3Printf("geometry name=%s\n",geometryName); - name2Shape.insert(geometryName,shapeIndex); - + for (int index=0;indexpush_back(indices[index]+indexBase); + } + + + //b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size()); + indexBase=visualShape.m_vertices->size(); + visualShape.m_numIndices = visualShape.m_indices->size(); + visualShape.m_numvertices = visualShape.m_vertices->size(); + } + //b3Printf("geometry name=%s\n",geometryName); + name2Shape.insert(geometryName,shapeIndex); + } else + { + b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES); + } }//for each geometry } @@ -557,7 +563,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray name2ShapeIndex; From a608f9bfddc21f47166e39c0ada5d93558e10dff Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Aug 2016 23:46:35 -0700 Subject: [PATCH 18/36] fix trailing space issue in tinyobjloader/mtl files. --- examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index 8479992d5..16b0522ea 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -366,6 +366,8 @@ std::string LoadMtl ( continue; } + linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1); + // Skip leading space. const char* token = linebuf.c_str(); token += strspn(token, " \t"); From 04cd9e18d6ec0023e8aca6a894ee26bf3ad7f7a3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 1 Aug 2016 23:53:38 -0700 Subject: [PATCH 19/36] add [] in error message of texture loading, to show leading/trailing spaces --- examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 67b64fd26..c042c42c0 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -67,7 +67,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& } else { - b3Warning("not found %s\n",relativeFileName); + b3Warning("not found [%s]\n",relativeFileName); } } } From 93db3d7ba6a5cda415ce738e1b1c8014a9d6c625 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 2 Aug 2016 09:30:33 -0700 Subject: [PATCH 20/36] fix CMD_INIT_POSE+INIT_POSE_HAS_INITIAL_ORIENTATION, setWorldToBaseRot takes inverse orientation (need to remove that API!) --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 469c178f8..8a54ba184 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1836,11 +1836,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); mb->setBaseOmega(btVector3(0,0,0)); - mb->setWorldToBaseRot(btQuaternion( - clientCmd.m_initPoseArgs.m_initialStateQ[3], + btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3], clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[5], - clientCmd.m_initPoseArgs.m_initialStateQ[6])); + clientCmd.m_initPoseArgs.m_initialStateQ[6]); + + mb->setWorldToBaseRot(invOrn.inverse()); } if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) { From f304fd76110fa90b110fc5e8f5f237f9de6cf3b4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 2 Aug 2016 11:12:23 -0700 Subject: [PATCH 21/36] add one more pybullet renderImage API and testrender.py example tweak Bullet Inverse Dynamics, work-around compiler issue --- examples/SharedMemory/PhysicsClientC_API.cpp | 62 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + examples/pybullet/pybullet.c | 31 ++++++++++ examples/pybullet/testrender.py | 40 ++++++++++++ .../details/IDLinearMathInterface.hpp | 9 +-- 5 files changed, 139 insertions(+), 4 deletions(-) create mode 100644 examples/pybullet/testrender.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4197544b2..23dc57826 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2,6 +2,8 @@ #include "PhysicsClientSharedMemory.h" #include "Bullet3Common/b3Scalar.h" #include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" + #include #include "SharedMemoryCommands.h" @@ -823,6 +825,66 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } +void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + b3Vector3 camUpVector; + b3Vector3 camForward; + b3Vector3 camPos; + b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]); + + int forwardAxis(-1); + switch (upAxis) + { + case 1: + forwardAxis = 2; + camUpVector = b3MakeVector3(0,1,0); + //gLightPos = b3MakeVector3(-50.f,100,30); + break; + case 2: + forwardAxis = 1; + camUpVector = b3MakeVector3(0,0,1); + //gLightPos = b3MakeVector3(-50.f,30,100); + break; + default: + { + //b3Assert(0); + return; + } + }; + + b3Vector3 eyePos = b3MakeVector3(0,0,0); + eyePos[forwardAxis] = -distance; + + camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + if (camForward.length2() < B3_EPSILON) + { + camForward.setValue(1.f,0.f,0.f); + } else + { + camForward.normalize(); + } + + b3Scalar rele = yaw * b3Scalar(0.01745329251994329547);// rads per deg + b3Scalar razi = pitch * b3Scalar(0.01745329251994329547);// rads per deg + b3Quaternion rot(camUpVector,razi); + + b3Vector3 right = camUpVector.cross(camForward); + b3Quaternion roll(right,-rele); + + eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos; + camPos = eyePos; + camPos += camTargetPos; + + float camPosf[4] = {camPos[0],camPos[1],camPos[2],0}; + float camPosTargetf[4] = {camTargetPos[0],camTargetPos[1],camTargetPos[2],0}; + float camUpf[4] = {camUpVector[0],camUpVector[1],camUpVector[2],0}; + + b3RequestCameraImageSetViewMatrix(commandHandle,camPosf,camPosTargetf,camUpf); + +} void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]) { b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2e2f126e2..588f810a8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -71,6 +71,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); +void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis); void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal); void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d2b7a496c..512956098 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1041,6 +1041,8 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) // renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal, fov) - // set resolution and initialize camera based on camera position, target // position, camera up, fulstrum near/far values and camera field of view. +// renderImage(w, h, targetPos, distance, yaw, pitch, upAxisIndex, nearVal, farVal, fov) + // // Note if the (w,h) is too small, the objects may not appear based on // where the camera has been set @@ -1151,6 +1153,35 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) aspect = width/height; b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); } + } + else if (size==10) + { + int upAxisIndex=1; + float camDistance,yaw,pitch; + + //sometimes more arguments are better :-) + if (PyArg_ParseTuple(args, "iiOfffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &upAxisIndex, &nearVal, &farVal, &fov)) + { + + if (pybullet_internalSetVector(objTargetPos, targetPos)) + { + printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); + + b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,upAxisIndex); + aspect = width/height; + b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); + } else + { + PyErr_SetString(SpamError, "Error parsing camera target pos"); + } + } else + { + PyErr_SetString(SpamError, "Error parsing arguments"); + } + + + + } else { diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py new file mode 100644 index 000000000..7ba07e4e7 --- /dev/null +++ b/examples/pybullet/testrender.py @@ -0,0 +1,40 @@ +import numpy as np +import matplotlib.pyplot as plt +import pybullet + +pybullet.connect(pybullet.GUI) +pybullet.loadURDF("r2d2.urdf") + +camTargetPos = [0,0,0] +#cameraUp = [0,0,1] +cameraPos = [3,3,3] +yaw = 40.0 +pitch = 0.0 +upAxisIndex = 1 +camDistance = 3 +pixelWidth = 640 +pixelHeight = 480 +nearPlane = 0.01 +farPlane = 1000 + +fov = 60 + +#img_arr = pybullet.renderImage(pixelWidth, pixelHeight) +#renderImage(w, h, view[16], projection[16]) +#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane) +img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, upAxisIndex, nearPlane, farPlane, fov) + +w=img_arr[0] #width of the image, in pixels +h=img_arr[1] #height of the image, in pixels +rgb=img_arr[2] #color data RGB +dep=img_arr[3] #depth data + + +# reshape creates np array +np_img_arr = np.reshape(rgb, (pixelHeight, pixelWidth, 4)) +np_img_arr = np_img_arr*(1./255.) + +#show +plt.imshow(np_img_arr,interpolation='none') +plt.show() +p.resetSimulation() diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index d81647b68..53f88c4b7 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -14,6 +14,7 @@ class vec3; class vecx; class mat33; typedef btMatrixX matxx; +typedef btVectorX vecxx; class vec3 : public btVector3 { public: @@ -46,11 +47,11 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s) inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -class vecx : public btVectorX { +class vecx : public vecxx { public: - vecx(int size) : btVectorX(size) {} - const vecx& operator=(const btVectorX& rhs) { - *static_cast*>(this) = rhs; + vecx(int size) : vecxx(size) {} + const vecx& operator=(const vecxx& rhs) { + *static_cast(this) = rhs; return *this; } From f5fca86d49b5de1e8968ca5260803f2c631dbfa8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 2 Aug 2016 11:14:21 -0700 Subject: [PATCH 22/36] remove debug printf --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 512956098..ba34093e8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1165,7 +1165,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) if (pybullet_internalSetVector(objTargetPos, targetPos)) { - printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); + //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,upAxisIndex); aspect = width/height; From b880ddf76bd45f78f43517a139b99e6908074a37 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 8 Aug 2016 14:23:44 -0700 Subject: [PATCH 23/36] add pybullet render API with yaw/pitch/roll option add testrender.py file allow option to enable OpenGL hardware renderer in multithreaded sim b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); --- build_and_run_cmake.sh | 2 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 2 +- examples/RoboticsLearning/b3RobotSimAPI.cpp | 64 ++++++++++-- examples/SharedMemory/PhysicsClientC_API.cpp | 99 ++++++++++++------- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../SharedMemory/PhysicsClientExample.cpp | 12 +-- .../PhysicsClientSharedMemory.cpp | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 4 +- .../SharedMemory/PhysicsServerExample.cpp | 55 ++++++++++- examples/pybullet/pybullet.c | 12 ++- examples/pybullet/testrender.py | 46 +++++---- 11 files changed, 219 insertions(+), 81 deletions(-) diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh index 28354357b..13cb76c62 100755 --- a/build_and_run_cmake.sh +++ b/build_and_run_cmake.sh @@ -2,6 +2,6 @@ rm CMakeCache.txt mkdir build_cmake cd build_cmake -cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release .. +cmake -DBUILD_PYBULLET=OFF -DCMAKE_BUILD_TYPE=Release .. make -j12 examples/ExampleBrowser/App_ExampleBrowser diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index d0fae2254..d6fab65a3 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -520,7 +520,7 @@ void MyStatusBarError(const char* msg) gui2->textOutput(msg); gui2->forceUpdateScrollBars(); } - btAssert(0); + btAssert(0); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index ce9f032b7..38914873e 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -47,6 +47,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eRobotSimGUIHelperCreateCollisionShapeGraphicsObject, eRobotSimGUIHelperCreateCollisionObjectGraphicsObject, eRobotSimGUIHelperRemoveAllGraphicsInstances, + eRobotSimGUIHelperCopyCameraImageData, }; #include @@ -152,7 +153,7 @@ void* RobotlsMemoryFunc() -ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface +ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper2 : public GUIHelperInterface { CommonGraphicsApp* m_app; @@ -185,7 +186,7 @@ public: int m_instanceId; - MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) + MultiThreadedOpenGLGuiHelper2(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) :m_app(app) ,m_cs(0), m_texels(0), @@ -195,7 +196,7 @@ public: } - virtual ~MultiThreadedOpenGLGuiHelper() + virtual ~MultiThreadedOpenGLGuiHelper2() { delete m_childGuiHelper; } @@ -345,10 +346,39 @@ public: { } + float m_viewMatrix[16]; + float m_projectionMatrix[16]; + unsigned char* m_pixelsRGBA; + int m_rgbaBufferSizeInPixels; + float* m_depthBuffer; + int m_depthBufferSizeInPixels; + int m_startPixelIndex; + int m_destinationWidth; + int m_destinationHeight; + int* m_numPixelsCopied; + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) { - if (numPixelsCopied) - *numPixelsCopied = 0; + m_cs->lock(); + for (int i=0;i<16;i++) + { + m_viewMatrix[i] = viewMatrix[i]; + m_projectionMatrix[i] = projectionMatrix[i]; + } + m_pixelsRGBA = pixelsRGBA; + m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels; + m_depthBuffer = depthBuffer; + m_depthBufferSizeInPixels = depthBufferSizeInPixels; + m_startPixelIndex = startPixelIndex; + m_destinationWidth = destinationWidth; + m_destinationHeight = destinationHeight; + m_numPixelsCopied = numPixelsCopied; + + m_cs->setSharedParam(1,eRobotSimGUIHelperCopyCameraImageData); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) + { + } } virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) @@ -375,7 +405,7 @@ struct b3RobotSimAPI_InternalData b3ThreadSupportInterface* m_threadSupport; RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; - MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper; + MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; bool m_connected; @@ -494,6 +524,24 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); break; } + case eRobotSimGUIHelperCopyCameraImageData: + { + m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix, + m_data->m_multiThreadedHelper->m_projectionMatrix, + m_data->m_multiThreadedHelper->m_pixelsRGBA, + m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels, + m_data->m_multiThreadedHelper->m_depthBuffer, + m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels, + m_data->m_multiThreadedHelper->m_startPixelIndex, + m_data->m_multiThreadedHelper->m_destinationWidth, + m_data->m_multiThreadedHelper->m_destinationHeight, + m_data->m_multiThreadedHelper->m_numPixelsCopied); + m_data->m_multiThreadedHelper->getCriticalSection()->lock(); + m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); + m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); + + break; + } case eRobotSimGUIHelperIdle: default: { @@ -669,9 +717,9 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) { - m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); - MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 23dc57826..fbb773854 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -825,7 +825,7 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } -void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis) +void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -834,47 +834,78 @@ void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandl b3Vector3 camForward; b3Vector3 camPos; b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]); - + b3Vector3 eyePos = b3MakeVector3(0,0,0); + int forwardAxis(-1); + + { + switch (upAxis) { - case 1: - forwardAxis = 2; - camUpVector = b3MakeVector3(0,1,0); - //gLightPos = b3MakeVector3(-50.f,100,30); - break; - case 2: - forwardAxis = 1; - camUpVector = b3MakeVector3(0,0,1); - //gLightPos = b3MakeVector3(-50.f,30,100); - break; - default: - { - //b3Assert(0); - return; - } - }; - - b3Vector3 eyePos = b3MakeVector3(0,0,0); - eyePos[forwardAxis] = -distance; - - camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); - if (camForward.length2() < B3_EPSILON) - { - camForward.setValue(1.f,0.f,0.f); - } else - { - camForward.normalize(); + + case 1: + { + + + forwardAxis = 0; + eyePos[forwardAxis] = -distance; + camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + if (camForward.length2() < B3_EPSILON) + { + camForward.setValue(1.f,0.f,0.f); + } else + { + camForward.normalize(); + } + b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547); + b3Quaternion rollRot(camForward,rollRad); + + camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,1,0)); + //gLightPos = b3MakeVector3(-50.f,100,30); + break; + } + case 2: + { + + + forwardAxis = 1; + eyePos[forwardAxis] = -distance; + camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]); + if (camForward.length2() < B3_EPSILON) + { + camForward.setValue(1.f,0.f,0.f); + } else + { + camForward.normalize(); + } + + b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547); + b3Quaternion rollRot(camForward,rollRad); + + camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,0,1)); + //gLightPos = b3MakeVector3(-50.f,30,100); + break; + } + default: + { + //b3Assert(0); + return; + } + }; } + - b3Scalar rele = yaw * b3Scalar(0.01745329251994329547);// rads per deg - b3Scalar razi = pitch * b3Scalar(0.01745329251994329547);// rads per deg - b3Quaternion rot(camUpVector,razi); + b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg + b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg + + b3Quaternion pitchRot(camUpVector,pitchRad); b3Vector3 right = camUpVector.cross(camForward); - b3Quaternion roll(right,-rele); + b3Quaternion yawRot(right,-yawRad); + + - eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos; + eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos; camPos = eyePos; camPos += camTargetPos; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 588f810a8..f55876fbd 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -71,7 +71,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); -void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis); +void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis); void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal); void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index b5788bd09..017e67a53 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -259,15 +259,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); //b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); - float viewMatrix[16]; - float projectionMatrix[16]; - m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); + float viewMatrix[16]; + float projectionMatrix[16]; + m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); - b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - break; + b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fa4362caf..fc807fabb 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -609,7 +609,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; - if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0) + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 511290778..2f1b1f413 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -225,7 +225,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) = rgbaPixelsReceived[i]; } - if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0) + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) { @@ -241,7 +241,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; } } - } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0); + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); return m_data->m_hasStatus; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 75e475738..53ef9ca31 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -43,6 +43,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperCreateCollisionShapeGraphicsObject, eGUIHelperCreateCollisionObjectGraphicsObject, eGUIHelperRemoveAllGraphicsInstances, + eGUIHelperCopyCameraImageData, }; #include @@ -392,11 +393,41 @@ public: { } - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied) + float m_viewMatrix[16]; + float m_projectionMatrix[16]; + unsigned char* m_pixelsRGBA; + int m_rgbaBufferSizeInPixels; + float* m_depthBuffer; + int m_depthBufferSizeInPixels; + int m_startPixelIndex; + int m_destinationWidth; + int m_destinationHeight; + int* m_numPixelsCopied; + + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) { - if (numPixelsCopied) - *numPixelsCopied = 0; + m_cs->lock(); + for (int i=0;i<16;i++) + { + m_viewMatrix[i] = viewMatrix[i]; + m_projectionMatrix[i] = projectionMatrix[i]; + } + m_pixelsRGBA = pixelsRGBA; + m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels; + m_depthBuffer = depthBuffer; + m_depthBufferSizeInPixels = depthBufferSizeInPixels; + m_startPixelIndex = startPixelIndex; + m_destinationWidth = destinationWidth; + m_destinationHeight = destinationHeight; + m_numPixelsCopied = numPixelsCopied; + + m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData); + m_cs->unlock(); + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + } } + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) { @@ -727,6 +758,24 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->getCriticalSection()->unlock(); break; } + + case eGUIHelperCopyCameraImageData: + { + m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix, + m_multiThreadedHelper->m_projectionMatrix, + m_multiThreadedHelper->m_pixelsRGBA, + m_multiThreadedHelper->m_rgbaBufferSizeInPixels, + m_multiThreadedHelper->m_depthBuffer, + m_multiThreadedHelper->m_depthBufferSizeInPixels, + m_multiThreadedHelper->m_startPixelIndex, + m_multiThreadedHelper->m_destinationWidth, + m_multiThreadedHelper->m_destinationHeight, + m_multiThreadedHelper->m_numPixelsCopied); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } case eGUIHelperIdle: default: { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ba34093e8..e00c5f0af 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1154,20 +1154,21 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); } } - else if (size==10) + else if (size==11) { int upAxisIndex=1; - float camDistance,yaw,pitch; + float camDistance,yaw,pitch,roll; //sometimes more arguments are better :-) - if (PyArg_ParseTuple(args, "iiOfffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &upAxisIndex, &nearVal, &farVal, &fov)) + if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov)) { + b3RequestCameraImageSetPixelResolution(command,width,height); if (pybullet_internalSetVector(objTargetPos, targetPos)) { //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); - b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,upAxisIndex); + b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,roll,upAxisIndex); aspect = width/height; b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); } else @@ -1193,6 +1194,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { b3SharedMemoryStatusHandle statusHandle; int statusType; + + //b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType==CMD_CAMERA_IMAGE_COMPLETED) diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py index 7ba07e4e7..da96bbbc5 100644 --- a/examples/pybullet/testrender.py +++ b/examples/pybullet/testrender.py @@ -6,14 +6,16 @@ pybullet.connect(pybullet.GUI) pybullet.loadURDF("r2d2.urdf") camTargetPos = [0,0,0] -#cameraUp = [0,0,1] -cameraPos = [3,3,3] -yaw = 40.0 -pitch = 0.0 -upAxisIndex = 1 -camDistance = 3 -pixelWidth = 640 -pixelHeight = 480 +cameraUp = [0,0,1] +cameraPos = [1,1,1] +yaw = 40 +pitch = 10.0 + +roll=0 +upAxisIndex = 2 +camDistance = 4 +pixelWidth = 320 +pixelHeight = 240 nearPlane = 0.01 farPlane = 1000 @@ -22,19 +24,23 @@ fov = 60 #img_arr = pybullet.renderImage(pixelWidth, pixelHeight) #renderImage(w, h, view[16], projection[16]) #img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane) -img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, upAxisIndex, nearPlane, farPlane, fov) +for pitch in range (0,360,10) : + img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov) -w=img_arr[0] #width of the image, in pixels -h=img_arr[1] #height of the image, in pixels -rgb=img_arr[2] #color data RGB -dep=img_arr[3] #depth data + w=img_arr[0] #width of the image, in pixels + h=img_arr[1] #height of the image, in pixels + rgb=img_arr[2] #color data RGB + dep=img_arr[3] #depth data + #print 'width = %d height = %d' % (w,h) -# reshape creates np array -np_img_arr = np.reshape(rgb, (pixelHeight, pixelWidth, 4)) -np_img_arr = np_img_arr*(1./255.) + # reshape creates np array + np_img_arr = np.reshape(rgb, (h, w, 4)) + np_img_arr = np_img_arr*(1./255.) -#show -plt.imshow(np_img_arr,interpolation='none') -plt.show() -p.resetSimulation() + #show + plt.imshow(np_img_arr,interpolation='none') + #plt.show() + plt.pause(0.01) + +pybullet.resetSimulation() From a9b1544a9f3a30f8537933f81f5a3c601a3cc41e Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 9 Aug 2016 18:40:12 -0700 Subject: [PATCH 24/36] Add premake support to build pybullet, Windows and Linux tested, will enable Mac in next commit. Expose inverse dynamics to Bullet shared memory API, through b3CalculateInverseDynamicsCommandInit and b3GetStatusInverseDynamicsJointForces command/status. See PhysicsClientExeample or pybullet for usage. Add option for Windows and Linux to set python_lib_dir and python_include_dir for premake and --enable_pybullet option Expose inverse dynamics to pybullet: [force] = p.calculateInverseDynamics(objectIndex,[q],[qdot],[acc]) Thanks to Jeff Bingham for the suggestion. --- build3/premake4.lua | 40 +++++- examples/SharedMemory/PhysicsClientC_API.cpp | 58 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 10 ++ .../SharedMemory/PhysicsClientExample.cpp | 58 +++++++++ .../PhysicsClientSharedMemory.cpp | 10 +- .../PhysicsServerCommandProcessor.cpp | 95 +++++++++++++- .../PhysicsServerCommandProcessor.h | 2 + examples/SharedMemory/SharedMemoryCommands.h | 19 +++ examples/SharedMemory/SharedMemoryPublic.h | 3 + examples/SharedMemory/premake4.lua | 6 +- examples/pybullet/CMakeLists.txt | 109 ++++++++-------- examples/pybullet/premake4.lua | 68 +++++++++- examples/pybullet/pybullet.c | 121 +++++++++++++++++- test/SharedMemory/CMakeLists.txt | 2 +- test/SharedMemory/premake4.lua | 92 ++++++------- 15 files changed, 575 insertions(+), 118 deletions(-) diff --git a/build3/premake4.lua b/build3/premake4.lua index 862dfc2a5..744a55214 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -85,12 +85,36 @@ newoption { - trigger = "python", - description = "Enable Python scripting (experimental, use Physics Server in Example Browser). " + trigger = "enable_pybullet", + description = "Enable high-level Python scripting of Bullet with URDF/SDF import and synthetic camera." } +if os.is("Linux") then + default_python_include_dir = "/usr/include/python2.7" + default_python_lib_dir = "/usr/local/lib/" +end + +if os.is("Windows") then + default_python_include_dir = "C:/Python34/include" + default_python_lib_dir = "C:/Python34/libs" +end + newoption + { + trigger = "python_include_dir", + value = default_python_include_dir, + description = "Python (2.x or 3.x) include directory" + } + + newoption + { + trigger = "python_lib_dir", + value = default_python_lib_dir, + description = "Python (2.x or 3.x) library directory " + } + + newoption { trigger = "targetdir", value = "path such as ../bin", @@ -140,7 +164,7 @@ platforms {"x32"} end else - platforms {"x32", "x64"} + platforms {"x64"} end configuration {"x32"} @@ -191,6 +215,14 @@ targetdir( _OPTIONS["targetdir"] or "../bin" ) location("./" .. act .. postfix) + if not _OPTIONS["python_include_dir"] then + _OPTIONS["python_include_dir"] = default_python_include_dir + end + + if not _OPTIONS["python_lib_dir"] then + _OPTIONS["python_lib_dir"] = default_python_lib_dir + end + projectRootDir = os.getcwd() .. "/../" print("Project root directory: " .. projectRootDir); @@ -222,7 +254,7 @@ if _OPTIONS["lua"] then include "../examples/ThirdPartyLibs/lua-5.2.3" end - if _OPTIONS["python"] then + if _OPTIONS["enable_pybullet"] then include "../examples/pybullet" end diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index fbb773854..8530c0c5a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -626,6 +626,7 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) return bodyId; } + int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* numDegreeOfFreedomQ, @@ -1097,3 +1098,60 @@ void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUn command->m_externalForceArguments.m_numForcesAndTorques++; } + + + +///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics +b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS; + command->m_updateFlags = 0; + command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex; + int numJoints = cl->getNumJoints(bodyIndex); + for (int i = 0; i < numJoints;i++) + { + command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; + command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i]; + command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i]; + } + + return (b3SharedMemoryCommandHandle)command; +} + +int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointForces) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED); + if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + return false; + + + if (dofCount) + { + *dofCount = status->m_inverseDynamicsResultArgs.m_dofCount; + } + if (bodyUniqueId) + { + *bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId; + } + if (jointForces) + { + for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++) + { + jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i]; + } + } + + + return true; +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f55876fbd..6f7b13647 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -97,6 +97,16 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics +b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, + const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); + +int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* dofCount, + double* jointForces); + + b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 017e67a53..801dd0d89 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -413,6 +413,33 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + if (m_selectedBody >= 0) + { + btAlignedObjectArray jointPositionsQ; + btAlignedObjectArray jointVelocitiesQdot; + btAlignedObjectArray jointAccelerations; + int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); + if (numJoints) + { + b3Printf("Compute inverse dynamics for joint accelerations:"); + jointPositionsQ.resize(numJoints); + jointVelocitiesQdot.resize(numJoints); + jointAccelerations.resize(numJoints); + for (int i = 0; i < numJoints; i++) + { + jointAccelerations[i] = 100; + b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]); + } + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle, + m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + + } + } + break; + } default: { b3Error("Unknown buttonId"); @@ -490,6 +517,7 @@ void PhysicsClientExample::createButtons() createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); + createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger); if (m_bodyUniqueIds.size()) { @@ -695,6 +723,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime) // b3Printf(msg); } + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(status, + &bodyUniqueId, + &dofCount, + 0); + + btAlignedObjectArray jointForces; + if (dofCount) + { + jointForces.resize(dofCount); + b3GetStatusInverseDynamicsJointForces(status, + 0, + 0, + &jointForces[0]); + for (int i = 0; i < dofCount; i++) + { + b3Printf("jointForces[%d]=%f", i, jointForces[i]); + } + } + + } + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_FAILED) + { + b3Warning("Inverse Dynamics computations failed"); + } + if (statusType == CMD_CAMERA_IMAGE_FAILED) { b3Warning("Camera image FAILED\n"); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fc807fabb..433515091 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -542,7 +542,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Camera image FAILED\n"); break; } - + case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED: + { + break; + } + case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED: + { + b3Warning("Inverse Dynamics computations failed"); + break; + } default: { b3Error("Unknown server status\n"); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8a54ba184..1937448ce 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4,13 +4,15 @@ #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "TinyRendererVisualShapeConverter.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" - +#include "LinearMath/btHashMap.h" +#include "BulletInverseDynamics/MultiBodyTree.hpp" #include "btBulletDynamicsCommon.h" @@ -383,6 +385,7 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; btAlignedObjectArray m_multiBodyJointFeedbacks; + btHashMap m_inverseDynamicsBodies; @@ -548,8 +551,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); } +void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() +{ + for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++) + { + btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.getAtIndex(i); + if (treePtrPtr) + { + btInverseDynamics::MultiBodyTree* tree = *treePtrPtr; + delete tree; + } + + } + m_data->m_inverseDynamicsBodies.clear(); +} + void PhysicsServerCommandProcessor::deleteDynamicsWorld() { + deleteCachedInverseDynamicsBodies(); + for (int i=0;im_multiBodyJointFeedbacks.size();i++) { @@ -1869,9 +1889,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_RESET_SIMULATION: { //clean up all data - - - + deleteCachedInverseDynamicsBodies(); + if (m_data && m_data->m_guiHelper) { m_data->m_guiHelper->removeAllGraphicsInstances(); @@ -2066,6 +2085,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + btInverseDynamics::MultiBodyTree** treePtrPtr = + m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody); + btInverseDynamics::MultiBodyTree* tree = 0; + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + + if (treePtrPtr) + { + tree = *treePtrPtr; + } + else + { + btInverseDynamics::btMultiBodyTreeCreator id_creator; + if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false)) + { + b3Error("error creating tree\n"); + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + else + { + tree = btInverseDynamics::CreateMultiBodyTree(id_creator); + m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree); + } + } + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); + for (int i = 0; i < num_dofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; + qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; + nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; + } + + if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; + for (int i = 0; i < num_dofs; i++) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs]; + } + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + + hasStatus = true; + break; + } + case CMD_APPLY_EXTERNAL_FORCE: { if (m_data->m_verboseOutput) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 7cbc87ab4..692bc2dd9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -29,6 +29,7 @@ protected: bool supportsJointMotor(class btMultiBody* body, int linkIndex); int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); + void deleteCachedInverseDynamicsBodies(); public: PhysicsServerCommandProcessor(); @@ -38,6 +39,7 @@ public: virtual void createEmptyDynamicsWorld(); virtual void deleteDynamicsWorld(); + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 64ae61991..ac5781639 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -348,6 +348,23 @@ enum EnumSdfRequestInfoFlags //SDF_REQUEST_INFO_CAMERA=2, }; + +struct CalculateInverseDynamicsArgs +{ + int m_bodyUniqueId; + + double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; + double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM]; + double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM]; +}; + +struct CalculateInverseDynamicsResultArgs +{ + int m_bodyUniqueId; + int m_dofCount; + double m_jointForces[MAX_DEGREE_OF_FREEDOM]; +}; + struct SharedMemoryCommand { int m_type; @@ -374,6 +391,7 @@ struct SharedMemoryCommand struct RequestPixelDataArgs m_requestPixelDataArguments; struct PickBodyArgs m_pickBodyArguments; struct ExternalForceArgs m_externalForceArguments; + struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; }; }; @@ -397,6 +415,7 @@ struct SharedMemoryStatus struct SendDebugLinesArgs m_sendDebugLinesArgs; struct SendPixelDataArgs m_sendPixelDataArguments; struct RigidBodyCreateArgs m_rigidBodyCreateArgs; + struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 10d87d008..0b6542806 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -27,6 +27,7 @@ enum EnumSharedMemoryClientCommand CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_REQUEST_CAMERA_IMAGE_DATA, CMD_APPLY_EXTERNAL_FORCE, + CMD_CALCULATE_INVERSE_DYNAMICS, CMD_MAX_CLIENT_COMMANDS }; @@ -59,6 +60,8 @@ enum EnumSharedMemoryServerStatus CMD_BODY_INFO_COMPLETED, CMD_BODY_INFO_FAILED, CMD_INVALID_STATUS, + CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, + CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index c2ceb1a05..ba761dd82 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -10,7 +10,7 @@ end includedirs {".","../../src", "../ThirdPartyLibs",} links { - "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath" + "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath" } language "C++" @@ -137,7 +137,7 @@ defines {"B3_USE_STANDALONE_EXAMPLE"} includedirs {"../../src"} links { - "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" + "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common" } initOpenGL() initGlew() @@ -211,7 +211,7 @@ if os.is("Windows") then } links { - "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api" + "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api" } diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 5540d10de..295499858 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -8,61 +8,60 @@ INCLUDE_DIRECTORIES( SET(pybullet_SRCS pybullet.c - ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp - ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp - ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h - ../../examples/OpenGLWindow/SimpleCamera.cpp - ../../examples/OpenGLWindow/SimpleCamera.h - ../../examples/TinyRenderer/geometry.cpp - ../../examples/TinyRenderer/model.cpp - ../../examples/TinyRenderer/tgaimage.cpp - ../../examples/TinyRenderer/our_gl.cpp - ../../examples/TinyRenderer/TinyRenderer.cpp - ../../examples/SharedMemory/InProcessMemory.cpp - ../../examples/SharedMemory/PhysicsClient.cpp - ../../examples/SharedMemory/PhysicsClient.h - ../../examples/SharedMemory/PhysicsServer.cpp - ../../examples/SharedMemory/PhysicsServer.h - ../../examples/SharedMemory/PhysicsServerExample.cpp - ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.h - ../../examples/SharedMemory/PhysicsDirect.cpp - ../../examples/SharedMemory/PhysicsDirect.h - ../../examples/SharedMemory/PhysicsDirectC_API.cpp - ../../examples/SharedMemory/PhysicsDirectC_API.h - ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp - ../../examples/SharedMemory/PhysicsServerCommandProcessor.h - ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory.h - ../../examples/SharedMemory/PhysicsClientC_API.cpp - ../../examples/SharedMemory/PhysicsClientC_API.h - ../../examples/SharedMemory/Win32SharedMemory.cpp - ../../examples/SharedMemory/Win32SharedMemory.h - ../../examples/SharedMemory/PosixSharedMemory.cpp - ../../examples/SharedMemory/PosixSharedMemory.h - ../../examples/Utils/b3ResourcePath.cpp - ../../examples/Utils/b3ResourcePath.h - ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp - ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h - ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp - ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp - ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp - ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp - ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp - ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp - ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp - ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp - ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp - ../../examples/MultiThreading/b3PosixThreadSupport.cpp - ../../examples/MultiThreading/b3Win32ThreadSupport.cpp - ../../examples/MultiThreading/b3ThreadSupportInterface.cpp - + ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp + ../../examples/SharedMemory/InProcessMemory.cpp + ../../examples/SharedMemory/PhysicsClient.cpp + ../../examples/SharedMemory/PhysicsClient.h + ../../examples/SharedMemory/PhysicsServer.cpp + ../../examples/SharedMemory/PhysicsServer.h + ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.h + ../../examples/SharedMemory/PhysicsDirect.cpp + ../../examples/SharedMemory/PhysicsDirect.h + ../../examples/SharedMemory/PhysicsDirectC_API.cpp + ../../examples/SharedMemory/PhysicsDirectC_API.h + ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp + ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.h + ../../examples/SharedMemory/PhysicsClientC_API.cpp + ../../examples/SharedMemory/PhysicsClientC_API.h + ../../examples/SharedMemory/Win32SharedMemory.cpp + ../../examples/SharedMemory/Win32SharedMemory.h + ../../examples/SharedMemory/PosixSharedMemory.cpp + ../../examples/SharedMemory/PosixSharedMemory.h + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Utils/b3ResourcePath.h + ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/MultiThreading/b3PosixThreadSupport.cpp + ../../examples/MultiThreading/b3Win32ThreadSupport.cpp + ../../examples/MultiThreading/b3ThreadSupportInterface.cpp ) IF(WIN32) diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 39092b889..a0585a128 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -1,7 +1,6 @@ project ("pybullet") - language "C++" kind "SharedLib" targetsuffix ("") @@ -12,7 +11,7 @@ project ("pybullet") defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} hasCL = findOpenCL("clew") - links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} + links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} initOpenGL() initGlew() @@ -20,10 +19,8 @@ project ("pybullet") ".", "../../src", "../ThirdPartyLibs", - "/usr/include/python2.7", } - if os.is("MacOSX") then links{"Cocoa.framework","Python"} end @@ -40,8 +37,69 @@ project ("pybullet") files { "pybullet.c", - "../../examples/ExampleBrowser/ExampleEntries.cpp", + "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/OpenGLWindow/SimpleCamera.cpp", + "../../examples/OpenGLWindow/SimpleCamera.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", + "../../examples/SharedMemory/InProcessMemory.cpp", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsDirect.cpp", + "../../examples/SharedMemory/PhysicsDirect.h", + "../../examples/SharedMemory/PhysicsDirectC_API.cpp", + "../../examples/SharedMemory/PhysicsDirectC_API.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/MultiThreading/b3PosixThreadSupport.cpp", + "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", } + + includedirs { + _OPTIONS["python_include_dir"], + } + libdirs { + _OPTIONS["python_lib_dir"] + } + if os.is("Linux") then initX11() end diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e00c5f0af..511767ccc 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1163,7 +1163,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov)) { - b3RequestCameraImageSetPixelResolution(command,width,height); + b3RequestCameraImageSetPixelResolution(command,width,height); if (pybullet_internalSetVector(objTargetPos, targetPos)) { //printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); @@ -1521,6 +1521,123 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) return Py_None; } +///Given an object id, joint positions, joint velocities and joint accelerations, +///compute the joint forces using Inverse Dynamics +static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args) +{ + int size; + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size = PySequence_Size(args); + if (size==4) + { + + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + + if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations)) + { + int szObPos = PySequence_Size(objPositionsQ); + int szObVel = PySequence_Size(objVelocitiesQdot); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyIndex); + if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints)) + { + int szInBytes = sizeof(double)*numJoints; + int i; + PyObject* pylist = 0; + double* jointPositionsQ = (double*)malloc(szInBytes); + double* jointVelocitiesQdot = (double*)malloc(szInBytes); + double* jointAccelerations = (double*)malloc(szInBytes); + double* jointForcesOutput = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) + { + jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i); + jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); + jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i); + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm, + bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, + &bodyUniqueId, + &dofCount, + 0); + + if (dofCount) + { + b3GetStatusInverseDynamicsJointForces(statusHandle, + 0, + 0, + jointForcesOutput); + { + { + + int i; + pylist = PyTuple_New(dofCount); + for (i = 0; i Date: Thu, 11 Aug 2016 11:20:31 -0700 Subject: [PATCH 25/36] set the camera, even in multi-threaded mode --- examples/SharedMemory/PhysicsServerExample.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 53ef9ca31..8a0099525 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -391,6 +391,7 @@ public: } virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) { + m_childGuiHelper->resetCamera(camDist,pitch,yaw,camPosX,camPosY,camPosZ); } float m_viewMatrix[16]; From 3c30e2f8211a019181b240148c89cd76f3f1e1f6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 11 Aug 2016 14:55:30 -0700 Subject: [PATCH 26/36] add segmentation mask rendering to TinyRenderer and shared memory API similar to the zbuffer, but storing the object index (int) instead of float depth --- .../CommonGUIHelperInterface.h | 13 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 7 +- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 7 +- examples/ExampleBrowser/OpenGLGuiHelper.h | 7 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 17 ++- .../ImportURDFDemo/BulletUrdfImporter.h | 5 +- .../LinkVisualShapesConverter.h | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 2 +- .../ImportURDFDemo/URDFImporterInterface.h | 6 +- .../RenderingExamples/TinyRendererSetup.cpp | 5 +- examples/RoboticsLearning/b3RobotSimAPI.cpp | 13 +- .../SharedMemory/PhysicsClientExample.cpp | 144 +++++++++++++++--- .../PhysicsClientSharedMemory.cpp | 11 +- examples/SharedMemory/PhysicsDirect.cpp | 11 +- .../PhysicsServerCommandProcessor.cpp | 18 ++- .../SharedMemory/PhysicsServerExample.cpp | 13 +- examples/SharedMemory/SharedMemoryPublic.h | 3 +- .../TinyRendererVisualShapeConverter.cpp | 23 ++- .../TinyRendererVisualShapeConverter.h | 4 +- examples/TinyRenderer/TinyRenderer.cpp | 9 +- examples/TinyRenderer/TinyRenderer.h | 6 +- examples/TinyRenderer/our_gl.cpp | 3 +- examples/TinyRenderer/our_gl.h | 2 +- 23 files changed, 276 insertions(+), 55 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 726303aed..f871a7e0e 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -46,7 +46,11 @@ struct GUIHelperInterface virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0; - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0; + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0; virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0; @@ -107,7 +111,12 @@ struct DummyGUIHelper : public GUIHelperInterface { } - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied) + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int width, int height, int* numPixelsCopied) + { if (numPixelsCopied) *numPixelsCopied = 0; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index d6fab65a3..e29eb6996 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -634,10 +634,12 @@ struct QuickCanvas : public Common2dCanvasInterface MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS]; GraphingTexture* m_gt[MAX_GRAPH_WINDOWS]; int m_curNumGraphWindows; + int m_curXpos; QuickCanvas(GL3TexLoader* myTexLoader) :m_myTexLoader(myTexLoader), - m_curNumGraphWindows(0) + m_curNumGraphWindows(0), + m_curXpos(0) { for (int i=0;igetInternalData()); input.m_width=width; input.m_height=height; - input.m_xPos = 10000;//GUI will clamp it to the right//300; + input.m_xPos = m_curXpos;//GUI will clamp it to the right//300; + m_curXpos+=width+20; input.m_yPos = 10000;//GUI will clamp it to bottom input.m_name=canvasName; input.m_texName = canvasName; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index a5d7e0c3d..d4aa246dc 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -338,7 +338,12 @@ void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float c } -void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) +void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int destinationWidth, + int destinationHeight, int* numPixelsCopied) { int sourceWidth = m_data->m_glApp->m_window->getWidth()*m_data->m_glApp->m_window->getRetinaScale(); int sourceHeight = m_data->m_glApp->m_window->getHeight()*m_data->m_glApp->m_window->getRetinaScale(); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 3ebdc4572..2fac0313e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -44,7 +44,12 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ); - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied); + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int destinationWidth, + int destinationHeight, int* numPixelsCopied); virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 425620c1a..992840850 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -45,6 +45,7 @@ struct BulletURDFInternalData UrdfParser m_urdfParser; struct GUIHelperInterface* m_guiHelper; char m_pathPrefix[1024]; + int m_bodyId; btHashMap m_linkColors; btAlignedObjectArray m_allocatedCollisionShapes; @@ -208,6 +209,18 @@ const char* BulletURDFImporter::getPathPrefix() return m_data->m_pathPrefix; } + +void BulletURDFImporter::setBodyUniqueId(int bodyId) +{ + m_data->m_bodyId =bodyId; +} + + +int BulletURDFImporter::getBodyUniqueId() const +{ + return m_data->m_bodyId; +} + BulletURDFImporter::~BulletURDFImporter() { @@ -1017,13 +1030,13 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& return false; } -void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const +void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const { if (m_data->m_customVisualShapesConverter) { const UrdfModel& model = m_data->m_urdfParser.getModel(); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex); } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index ea945aa33..127ed25b7 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -25,7 +25,8 @@ public: virtual bool loadSDF(const char* fileName, bool forceFixedBase = false); virtual int getNumModels() const; virtual void activateModel(int modelIndex); - + virtual void setBodyUniqueId(int bodyId); + virtual int getBodyUniqueId() const; const char* getPathPrefix(); void printTree(); //for debugging @@ -50,7 +51,7 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const; + virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h index a9c941dc0..814838ced 100644 --- a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h +++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h @@ -3,7 +3,7 @@ struct LinkVisualShapesConverter { - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj)=0; + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0; }; #endif //LINK_VISUAL_SHAPES_CONVERTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 597f1a4d9..367e674da 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -397,7 +397,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat u2b.getLinkColor(urdfLinkIndex,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); - u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col); + u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId()); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index e3299eda5..ce688ec90 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -48,8 +48,10 @@ public: ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const { } - + virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { } + virtual void setBodyUniqueId(int bodyId) {} + virtual int getBodyUniqueId() const { return 0;} + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0; }; diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index fd3f80e41..89fd1437d 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -23,6 +23,7 @@ struct TinyRendererSetupInternalData TGAImage m_rgbColorBuffer; b3AlignedObjectArray m_depthBuffer; + b3AlignedObjectArray m_segmentationMaskBuffer; int m_width; @@ -185,7 +186,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_internalData->m_shapePtr.push_back(0); TinyRenderObjectData* ob = new TinyRenderObjectData( m_internalData->m_rgbColorBuffer, - m_internalData->m_depthBuffer); + m_internalData->m_depthBuffer, + m_internalData->m_segmentationMaskBuffer, + m_internalData->m_renderObjects.size(),0); //ob->loadModel("cube.obj"); const int* indices = &meshData.m_gfxShape->m_indices->at(0); ob->registerMeshShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 38914873e..adbef7a20 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -352,12 +352,19 @@ public: int m_rgbaBufferSizeInPixels; float* m_depthBuffer; int m_depthBufferSizeInPixels; + int* m_segmentationMaskBuffer; + int m_segmentationMaskBufferSizeInPixels; int m_startPixelIndex; int m_destinationWidth; int m_destinationHeight; int* m_numPixelsCopied; - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int destinationWidth, + int destinationHeight, int* numPixelsCopied) { m_cs->lock(); for (int i=0;i<16;i++) @@ -369,6 +376,8 @@ public: m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels; m_depthBuffer = depthBuffer; m_depthBufferSizeInPixels = depthBufferSizeInPixels; + m_segmentationMaskBuffer = segmentationMaskBuffer; + m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels; m_startPixelIndex = startPixelIndex; m_destinationWidth = destinationWidth; m_destinationHeight = destinationHeight; @@ -532,6 +541,8 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels, m_data->m_multiThreadedHelper->m_depthBuffer, m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels, + m_data->m_multiThreadedHelper->m_segmentationMaskBuffer, + m_data->m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, m_data->m_multiThreadedHelper->m_startPixelIndex, m_data->m_multiThreadedHelper->m_destinationWidth, m_data->m_multiThreadedHelper->m_destinationHeight, diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 801dd0d89..718ad5967 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -44,7 +44,9 @@ protected: int m_selectedBody; int m_prevSelectedBody; struct Common2dCanvasInterface* m_canvas; - int m_canvasIndex; + int m_canvasRGBIndex; + int m_canvasDepthIndex; + int m_canvasSegMaskIndex; void createButton(const char* name, int id, bool isTrigger ); @@ -248,7 +250,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -460,7 +462,11 @@ m_prevSelectedBody(-1), m_numMotors(0), m_options(options), m_isOptionalServerConnected(false), -m_canvas(0) +m_canvas(0), +m_canvasRGBIndex(-1), +m_canvasDepthIndex(-1), +m_canvasSegMaskIndex(-1) + { b3Printf("Started PhysicsClientExample\n"); } @@ -479,9 +485,15 @@ PhysicsClientExample::~PhysicsClientExample() m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); } - if (m_canvas && (m_canvasIndex>=0)) + if (m_canvas) { - m_canvas->destroyCanvas(m_canvasIndex); + if (m_canvasRGBIndex>=0) + m_canvas->destroyCanvas(m_canvasRGBIndex); + if (m_canvasDepthIndex>=0) + m_canvas->destroyCanvas(m_canvasDepthIndex); + if (m_canvasSegMaskIndex>=0) + m_canvas->destroyCanvas(m_canvasSegMaskIndex); + } b3Printf("~PhysicsClientExample\n"); } @@ -613,9 +625,10 @@ void PhysicsClientExample::initPhysics() m_canvas = m_guiHelper->get2dCanvasInterface(); if (m_canvas) { - - - m_canvasIndex = m_canvas->createCanvas("Synthetic Camera",camVisualizerWidth, camVisualizerHeight); + + m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight); + m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight); + m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight); for (int i=0;isetPixel(m_canvasIndex,i,j,red,green,blue,alpha); + m_canvas->setPixel(m_canvasRGBIndex,i,j,red,green,blue,alpha); + m_canvas->setPixel(m_canvasDepthIndex,i,j,red,green,blue,alpha); + m_canvas->setPixel(m_canvasSegMaskIndex,i,j,red,green,blue,alpha); + } } - m_canvas->refreshImageData(m_canvasIndex); + m_canvas->refreshImageData(m_canvasRGBIndex); + m_canvas->refreshImageData(m_canvasDepthIndex); + m_canvas->refreshImageData(m_canvasSegMaskIndex); + } @@ -698,8 +717,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime) // sprintf(msg,"Camera image %d OK\n",counter++); b3CameraImageData imageData; b3GetCameraImageData(m_physicsClientHandle,&imageData); - if (m_canvas && m_canvasIndex >=0) + if (m_canvas) { + + //compute depth image range + float minDepthValue = 1e20f; + float maxDepthValue = -1e20f; + + for (int i=0;i=0) + { + int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth); + float depthValue = imageData.m_depthValues4[depthPixelIndex]; + //todo: rescale the depthValue to [0..255] + if (depthValue>-1e20) + { + maxDepthValue = btMax(maxDepthValue,depthValue); + minDepthValue = btMin(minDepthValue,depthValue); + } + } + } + } + for (int i=0;isetPixel(m_canvasIndex,i,j, - imageData.m_rgbColorData[pixelIndex], - imageData.m_rgbColorData[pixelIndex+1], - imageData.m_rgbColorData[pixelIndex+2], - 255); //alpha set to 255 + if (m_canvasRGBIndex >=0) + { + int rgbPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel; + m_canvas->setPixel(m_canvasRGBIndex,i,j, + imageData.m_rgbColorData[rgbPixelIndex], + imageData.m_rgbColorData[rgbPixelIndex+1], + imageData.m_rgbColorData[rgbPixelIndex+2], + 255); //alpha set to 255 + } + + if (m_canvasDepthIndex >=0) + { + int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth); + float depthValue = imageData.m_depthValues4[depthPixelIndex]; + //todo: rescale the depthValue to [0..255] + if (depthValue>-1e20) + { + int rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue))); + if (rgb<0 || rgb>255) + { + + printf("rgb=%d\n",rgb); + } + + m_canvas->setPixel(m_canvasDepthIndex,i,j, + rgb, + rgb, + 255, 255); //alpha set to 255 + } else + { + m_canvas->setPixel(m_canvasDepthIndex,i,j, + 0, + 0, + 0, 255); //alpha set to 255 + } + } + if (m_canvasSegMaskIndex>=0 && (0!=imageData.m_segmentationMaskValues)) + { + int segmentationMaskPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth); + int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex]; + btVector4 palette[4] = {btVector4(32,255,32,255), + btVector4(32,32,255,255), + btVector4(255,255,32,255), + btVector4(32,255,255,255)}; + if (segmentationMask>=0) + { + btVector4 rgb = palette[segmentationMask&3]; + m_canvas->setPixel(m_canvasSegMaskIndex,i,j, + rgb.x(), + rgb.y(), + rgb.z(), 255); //alpha set to 255 + } else + { + m_canvas->setPixel(m_canvasSegMaskIndex,i,j, + 0, + 0, + 0, 255); //alpha set to 255 + } + } } } - m_canvas->refreshImageData(m_canvasIndex); + if (m_canvasRGBIndex >=0) + m_canvas->refreshImageData(m_canvasRGBIndex); + if (m_canvasDepthIndex >=0) + m_canvas->refreshImageData(m_canvasDepthIndex); + if (m_canvasSegMaskIndex >=0) + m_canvas->refreshImageData(m_canvasSegMaskIndex); + + + } // b3Printf(msg); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 433515091..e7002b04f 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -36,6 +36,7 @@ struct PhysicsClientSharedMemoryInternalData { int m_cachedCameraPixelsHeight; btAlignedObjectArray m_cachedCameraPixelsRGBA; btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMaskBuffer; btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -514,6 +515,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels); m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); @@ -522,12 +524,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { // printf("pixel = %d\n", rgbaPixelsReceived[0]); float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]); for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; } + for (int i=0;im_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] @@ -704,8 +712,9 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c { cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; - cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_depthValues4 = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0; } const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 2f1b1f413..d913954c5 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -37,6 +37,8 @@ struct PhysicsDirectInternalData int m_cachedCameraPixelsHeight; btAlignedObjectArray m_cachedCameraPixelsRGBA; btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMask; + PhysicsServerCommandProcessor* m_commandProcessor; @@ -205,6 +207,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMask.resize(numTotalPixels); m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); @@ -212,6 +215,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]); // printf("pixel = %d\n", rgbaPixelsReceived[0]); @@ -219,6 +223,10 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) { m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; } + for (int i=0;im_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] @@ -456,7 +464,8 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) { cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; - cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_depthValues4 = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1937448ce..d3ada5bca 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -751,7 +751,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); - + u2b.setBodyUniqueId(bodyUniqueId); { btScalar mass = 0; bodyHandle->m_rootLocalInertialFrame.setIdentity(); @@ -845,6 +845,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (bodyUniqueIdPtr) *bodyUniqueIdPtr= bodyUniqueId; + u2b.setBodyUniqueId(bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); { @@ -1165,15 +1166,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (numRemainingPixels>0) { - int maxNumPixels = bufferSizeInBytes/8-1; + int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask + int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1; unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); + int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8); if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) { - m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,width,height,&numPixelsCopied); + m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); } else { @@ -1194,7 +1201,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } - m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); + m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,&width,&height,&numPixelsCopied); } //each pixel takes 4 RGBA values and 1 float = 8 bytes diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 8a0099525..ca868b29f 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -400,12 +400,19 @@ public: int m_rgbaBufferSizeInPixels; float* m_depthBuffer; int m_depthBufferSizeInPixels; + int* m_segmentationMaskBuffer; + int m_segmentationMaskBufferSizeInPixels; int m_startPixelIndex; int m_destinationWidth; int m_destinationHeight; int* m_numPixelsCopied; - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int destinationWidth, + int destinationHeight, int* numPixelsCopied) { m_cs->lock(); for (int i=0;i<16;i++) @@ -417,6 +424,8 @@ public: m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels; m_depthBuffer = depthBuffer; m_depthBufferSizeInPixels = depthBufferSizeInPixels; + m_segmentationMaskBuffer = segmentationMaskBuffer; + m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels; m_startPixelIndex = startPixelIndex; m_destinationWidth = destinationWidth; m_destinationHeight = destinationHeight; @@ -768,6 +777,8 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_rgbaBufferSizeInPixels, m_multiThreadedHelper->m_depthBuffer, m_multiThreadedHelper->m_depthBufferSizeInPixels, + m_multiThreadedHelper->m_segmentationMaskBuffer, + m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, m_multiThreadedHelper->m_startPixelIndex, m_multiThreadedHelper->m_destinationWidth, m_multiThreadedHelper->m_destinationHeight, diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 0b6542806..9fee50a66 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -122,7 +122,8 @@ struct b3CameraImageData int m_pixelWidth; int m_pixelHeight; const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes - const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats + const float* m_depthValues4;//m_pixelWidth*m_pixelHeight floats + const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints }; ///b3LinkState provides extra information such as the Cartesian world coordinates diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 9457652a6..1cce9ef57 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -66,6 +66,8 @@ struct TinyRendererVisualShapeConverterInternalData int m_swHeight; TGAImage m_rgbColorBuffer; b3AlignedObjectArray m_depthBuffer; + b3AlignedObjectArray m_segmentationMaskBuffer; + SimpleCamera m_camera; TinyRendererVisualShapeConverterInternalData() @@ -75,6 +77,7 @@ struct TinyRendererVisualShapeConverterInternalData m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB) { m_depthBuffer.resize(m_swWidth*m_swHeight); + m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1); } }; @@ -440,7 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref -void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj) +void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex) { @@ -487,7 +490,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const if (vertices.size() && indices.size()) { - TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, m_data->m_segmentationMaskBuffer, objectIndex,0); unsigned char* textureImage=0; int textureWidth=0; int textureHeight=0; @@ -535,6 +538,7 @@ void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor) { m_data->m_rgbColorBuffer.set(x,y,clearColor); m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f; + m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1; } } @@ -624,7 +628,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo // printf("flipped!\n"); m_data->m_rgbColorBuffer.flip_vertically(); - //flip z-buffer + //flip z-buffer and segmentation Buffer { int half = m_data->m_swHeight>>1; for (int j=0; jm_swWidth;i++) { btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]); + btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]); } } } @@ -652,11 +657,16 @@ void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height) m_data->m_swHeight = height; m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight); + m_data->m_segmentationMaskBuffer.resize(m_data->m_swWidth*m_data->m_swHeight); m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB); + } -void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) +void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, + int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) { int w = m_data->m_rgbColorBuffer.get_width(); int h = m_data->m_rgbColorBuffer.get_height(); @@ -682,6 +692,11 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels { depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex]; } + if (segmentationMaskBuffer) + { + segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex]; + } + if (pixelsRGBA) { pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0]; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 6baff2491..0602fe29c 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -13,7 +13,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter virtual ~TinyRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape); + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex); void setUpAxis(int axis); @@ -26,7 +26,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void getWidthAndHeight(int& width, int& height); void setWidthAndHeight(int width, int height); - void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); + void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); void render(); void render(const float viewMat[16], const float projMat[16]); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 650948a1a..75518abd5 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -88,12 +88,14 @@ struct Shader : public IShader { }; -TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray& segmentationMaskBuffer, int objectIndex,int objectIndex2) :m_rgbColorBuffer(rgbColorBuffer), m_depthBuffer(depthBuffer), +m_segmentationMaskBuffer(segmentationMaskBuffer), m_model(0), m_userData(0), -m_userIndex(-1) +m_userIndex(-1), +m_objectIndex(objectIndex) { Vec3f eye(1,1,3); Vec3f center(0,0,0); @@ -247,6 +249,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) renderData.m_viewportMatrix = viewport(0,0,width, height); b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; + b3AlignedObjectArray& segmentationMaskBuffer = renderData.m_segmentationMaskBuffer; TGAImage& frame = renderData.m_rgbColorBuffer; @@ -264,7 +267,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) for (int j=0; j<3; j++) { shader.vertex(i, j); } - triangle(shader.varying_tri, shader, frame, &zbuffer[0], renderData.m_viewportMatrix); + triangle(shader.varying_tri, shader, frame, &zbuffer[0], &segmentationMaskBuffer[0], renderData.m_viewportMatrix, renderData.m_objectIndex); } } diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index c6f4f9821..550134be7 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -28,8 +28,9 @@ struct TinyRenderObjectData TGAImage& m_rgbColorBuffer; b3AlignedObjectArray& m_depthBuffer; - - TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); + b3AlignedObjectArray& m_segmentationMaskBuffer; + + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray& segmentationMaskBuffer,int objectIndex,int objectIndex2); virtual ~TinyRenderObjectData(); void loadModel(const char* fileName); @@ -41,6 +42,7 @@ struct TinyRenderObjectData void* m_userData; int m_userIndex; + int m_objectIndex; }; diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index 4f23b73e7..53e8cd021 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -59,7 +59,7 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) { return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator } -void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) { +void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) { mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points @@ -100,6 +100,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb bool discard = shader.fragment(bc_clip, color); if (!discard) { zbuffer[P.x+P.y*image.get_width()] = frag_depth; + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex; image.set(P.x, P.y, color); } } diff --git a/examples/TinyRenderer/our_gl.h b/examples/TinyRenderer/our_gl.h index d80156904..60d5c400f 100644 --- a/examples/TinyRenderer/our_gl.h +++ b/examples/TinyRenderer/our_gl.h @@ -17,6 +17,6 @@ struct IShader { }; //void triangle(Vec4f *pts, IShader &shader, TGAImage &image, float *zbuffer); -void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix); +void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex); #endif //__OUR_GL_H__ From 4bb488f65e0804570fd7849b90a4500c449a2919 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 11 Aug 2016 15:02:00 -0700 Subject: [PATCH 27/36] reset canvas location --- examples/ExampleBrowser/OpenGLExampleBrowser.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index e29eb6996..5866e0ae4 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -680,6 +680,7 @@ struct QuickCanvas : public Common2dCanvasInterface } virtual void destroyCanvas(int canvasId) { + m_curXpos = 0; btAssert(canvasId>=0); delete m_gt[canvasId]; m_gt[canvasId] = 0; From 4abe083388dfede13c17fb3aceb5049757de6419 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 11 Aug 2016 15:58:51 -0700 Subject: [PATCH 28/36] fix pybullet --- examples/pybullet/pybullet.c | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 511767ccc..f9d4fa898 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1205,11 +1205,13 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth PyObject *pylistRGB; PyObject* pylistDep; + PyObject* pylistSeg; + int i, j, p; b3GetCameraImageData(sm, &imageData); //TODO(hellojas): error handling if image size is 0 - pyResultList = PyTuple_New(4); + pyResultList = PyTuple_New(5); PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); @@ -1221,15 +1223,23 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; pylistRGB = PyTuple_New(num); pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); - + pylistSeg = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); for (i=0;i Date: Fri, 12 Aug 2016 13:55:37 -0700 Subject: [PATCH 29/36] revert accidently renaming of m_depthvalues4 --- examples/SharedMemory/PhysicsClientExample.cpp | 4 ++-- examples/SharedMemory/PhysicsClientSharedMemory.cpp | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 2 +- examples/pybullet/pybullet.c | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 718ad5967..8dc2f962d 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -736,7 +736,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_canvasDepthIndex >=0) { int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth); - float depthValue = imageData.m_depthValues4[depthPixelIndex]; + float depthValue = imageData.m_depthValues[depthPixelIndex]; //todo: rescale the depthValue to [0..255] if (depthValue>-1e20) { @@ -770,7 +770,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_canvasDepthIndex >=0) { int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth); - float depthValue = imageData.m_depthValues4[depthPixelIndex]; + float depthValue = imageData.m_depthValues[depthPixelIndex]; //todo: rescale the depthValue to [0..255] if (depthValue>-1e20) { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e7002b04f..16b435f61 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -712,7 +712,7 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c { cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; - cameraData->m_depthValues4 = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index d913954c5..05c8c0eed 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -464,7 +464,7 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) { cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; - cameraData->m_depthValues4 = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9fee50a66..fb5d41364 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -122,7 +122,7 @@ struct b3CameraImageData int m_pixelWidth; int m_pixelHeight; const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes - const float* m_depthValues4;//m_pixelWidth*m_pixelHeight floats + const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f9d4fa898..915de51f1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1231,7 +1231,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) // TODO(hellojas): validate depth values make sense int depIndex = i+j*imageData.m_pixelWidth; { - item = PyFloat_FromDouble(imageData.m_depthValues4[depIndex]); + item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]); PyTuple_SetItem(pylistDep, depIndex, item); } { From ceceaa16be3f173ee0ee1f459e23664b0f84bc3a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 12 Aug 2016 14:18:46 -0700 Subject: [PATCH 30/36] make some changes to make the previous example code changes backward compatible --- .../CommonInterfaces/CommonGUIHelperInterface.h | 12 ++++++++++++ examples/RenderingExamples/TinyRendererSetup.cpp | 6 +++--- .../TinyRendererVisualShapeConverter.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 14 ++++++++++---- examples/TinyRenderer/TinyRenderer.h | 7 ++++--- examples/TinyRenderer/our_gl.cpp | 10 +++++++++- examples/TinyRenderer/our_gl.h | 2 +- 7 files changed, 40 insertions(+), 13 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index f871a7e0e..baea8daf4 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -47,6 +47,18 @@ struct GUIHelperInterface virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0; virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) + { + copyCameraImageData(viewMatrix,projectionMatrix,pixelsRGBA,rgbaBufferSizeInPixels, + depthBuffer,depthBufferSizeInPixels, + 0,0, + startPixelIndex,destinationWidth, + destinationHeight,numPixelsCopied); + } + + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 89fd1437d..9114d0889 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -187,9 +187,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) TinyRenderObjectData* ob = new TinyRenderObjectData( m_internalData->m_rgbColorBuffer, m_internalData->m_depthBuffer, - m_internalData->m_segmentationMaskBuffer, - m_internalData->m_renderObjects.size(),0); - //ob->loadModel("cube.obj"); + &m_internalData->m_segmentationMaskBuffer, + m_internalData->m_renderObjects.size()); + const int* indices = &meshData.m_gfxShape->m_indices->at(0); ob->registerMeshShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 1cce9ef57..be35c9626 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -490,7 +490,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const if (vertices.size() && indices.size()) { - TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, m_data->m_segmentationMaskBuffer, objectIndex,0); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex); unsigned char* textureImage=0; int textureWidth=0; int textureHeight=0; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 75518abd5..a382c1564 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -87,11 +87,17 @@ struct Shader : public IShader { } }; +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) +:TinyRenderObjectData(rgbColorBuffer,depthBuffer,0,0) +{ + +} -TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray& segmentationMaskBuffer, int objectIndex,int objectIndex2) + +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* segmentationMaskBuffer, int objectIndex) :m_rgbColorBuffer(rgbColorBuffer), m_depthBuffer(depthBuffer), -m_segmentationMaskBuffer(segmentationMaskBuffer), +m_segmentationMaskBufferPtr(segmentationMaskBuffer), m_model(0), m_userData(0), m_userIndex(-1), @@ -249,7 +255,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) renderData.m_viewportMatrix = viewport(0,0,width, height); b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; - b3AlignedObjectArray& segmentationMaskBuffer = renderData.m_segmentationMaskBuffer; + int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0); TGAImage& frame = renderData.m_rgbColorBuffer; @@ -267,7 +273,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) for (int j=0; j<3; j++) { shader.vertex(i, j); } - triangle(shader.varying_tri, shader, frame, &zbuffer[0], &segmentationMaskBuffer[0], renderData.m_viewportMatrix, renderData.m_objectIndex); + triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex); } } diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 550134be7..1819790a6 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -27,10 +27,11 @@ struct TinyRenderObjectData //Output TGAImage& m_rgbColorBuffer; - b3AlignedObjectArray& m_depthBuffer; - b3AlignedObjectArray& m_segmentationMaskBuffer; + b3AlignedObjectArray& m_depthBuffer;//required, hence a reference + b3AlignedObjectArray* m_segmentationMaskBufferPtr;//optional, hence a pointer - TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray& segmentationMaskBuffer,int objectIndex,int objectIndex2); + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer); + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* segmentationMaskBuffer,int objectIndex); virtual ~TinyRenderObjectData(); void loadModel(const char* fileName); diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp index 53e8cd021..639db9b0b 100644 --- a/examples/TinyRenderer/our_gl.cpp +++ b/examples/TinyRenderer/our_gl.cpp @@ -59,6 +59,11 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) { return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator } +void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix, int objectIndex) +{ + triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0); +} + void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) { mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points @@ -100,7 +105,10 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb bool discard = shader.fragment(bc_clip, color); if (!discard) { zbuffer[P.x+P.y*image.get_width()] = frag_depth; - segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex; + if (segmentationMaskBuffer) + { + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex; + } image.set(P.x, P.y, color); } } diff --git a/examples/TinyRenderer/our_gl.h b/examples/TinyRenderer/our_gl.h index 60d5c400f..378324cbd 100644 --- a/examples/TinyRenderer/our_gl.h +++ b/examples/TinyRenderer/our_gl.h @@ -16,7 +16,7 @@ struct IShader { virtual bool fragment(Vec3f bar, TGAColor &color) = 0; }; -//void triangle(Vec4f *pts, IShader &shader, TGAImage &image, float *zbuffer); +void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix); void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex); #endif //__OUR_GL_H__ From 238ba8c642a4c5f4de392dfc7fb6fb4dddd51571 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 12 Aug 2016 14:28:02 -0700 Subject: [PATCH 31/36] remote a c++11-style constructor --- examples/TinyRenderer/TinyRenderer.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index a382c1564..b81ce57ed 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -88,12 +88,25 @@ struct Shader : public IShader { }; TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) -:TinyRenderObjectData(rgbColorBuffer,depthBuffer,0,0) +:m_rgbColorBuffer(rgbColorBuffer), +m_depthBuffer(depthBuffer), +m_segmentationMaskBufferPtr(0), +m_model(0), +m_userData(0), +m_userIndex(-1), +m_objectIndex(-1) { - + Vec3f eye(1,1,3); + Vec3f center(0,0,0); + Vec3f up(0,0,1); + m_lightDirWorld.setValue(0,0,0); + m_localScaling.setValue(1,1,1); + m_modelMatrix = Matrix::identity(); + } + TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer, b3AlignedObjectArray* segmentationMaskBuffer, int objectIndex) :m_rgbColorBuffer(rgbColorBuffer), m_depthBuffer(depthBuffer), From 3bdcf23a05e1e2dc2a2466242a7d036ca45dbf26 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Sat, 13 Aug 2016 12:21:18 -0700 Subject: [PATCH 32/36] Add sleep to avoid 100% busy CPU loop in PhysicsServerExample Added btClock::usleep Fix broken TinyRenderer example code. --- build3/premake4.lua | 6 +++--- build_visual_studio.bat | 3 ++- .../InProcessExampleBrowser.cpp | 2 ++ .../ExampleBrowser/OpenGLExampleBrowser.cpp | 18 ++++++++++++------ .../SharedMemory/PhysicsServerExample.cpp | 14 +++++++++++++- .../main_tinyrenderer_single_example.cpp | 7 ++++++- examples/Utils/b3Clock.cpp | 19 +++++++++++++++++++ examples/Utils/b3Clock.h | 4 ++++ examples/pybullet/premake4.lua | 1 - 9 files changed, 61 insertions(+), 13 deletions(-) diff --git a/build3/premake4.lua b/build3/premake4.lua index 744a55214..d8826a348 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -96,8 +96,8 @@ end if os.is("Windows") then - default_python_include_dir = "C:/Python34/include" - default_python_lib_dir = "C:/Python34/libs" + default_python_include_dir = "C:\Python-3.5.2/include" + default_python_lib_dir = "C:/Python-3.5.2/libs" end newoption @@ -164,7 +164,7 @@ end platforms {"x32"} end else - platforms {"x64"} + platforms {"x32","x64"} end configuration {"x32"} diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 0da0089b1..5f33e30e7 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -2,7 +2,8 @@ rem premake4 --with-pe vs2010 rem premake4 --bullet2demos vs2010 cd build3 -premake4 --enable_openvr --targetdir="../bin" vs2010 +premake4 --enable_openvr --targetdir="../bin" vs2010 +rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010 rem premake4 --targetdir="../server2bin" vs2010 rem cd vs2010 rem rename 0_Bullet3Solution.sln 0_server.sln diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index e562b5d52..4924a5da9 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -332,6 +332,7 @@ btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc, while (data->m_args.m_cs->getSharedParam(0)==eExampleBrowserIsUnInitialized) { + b3Clock::usleep(1000); } return data; @@ -366,6 +367,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data) } else { // printf("polling.."); + b3Clock::usleep(1000); } }; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 5866e0ae4..6a8c87355 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -107,7 +107,7 @@ static CommonExampleInterface* sCurrentDemo = 0; static b3AlignedObjectArray allNames; static float gFixedTimeStep = 0; bool gAllowRetina = true; - +bool gDisableDemoSelection = false; static class ExampleEntries* gAllExamples=0; bool sUseOpenGL2 = false; bool drawGUI=true; @@ -556,9 +556,11 @@ struct MyMenuItemHander :public Gwen::Event::Handler Gwen::String laa = Gwen::Utility::UnicodeToString(la); //const char* ha = laa.c_str(); - - selectDemo(sCurrentHightlighted); - saveCurrentSettings(sCurrentDemoIndex, startFileName); + if (!gDisableDemoSelection ) + { + selectDemo(sCurrentHightlighted); + saveCurrentSettings(sCurrentDemoIndex, startFileName); + } } void onButtonC(Gwen::Controls::Base* pControl) { @@ -580,8 +582,11 @@ struct MyMenuItemHander :public Gwen::Event::Handler */ // printf("onKeyReturn ! \n"); - selectDemo(sCurrentHightlighted); - saveCurrentSettings(sCurrentDemoIndex, startFileName); + if (!gDisableDemoSelection ) + { + selectDemo(sCurrentHightlighted); + saveCurrentSettings(sCurrentDemoIndex, startFileName); + } } @@ -1228,5 +1233,6 @@ void OpenGLExampleBrowser::update(float deltaTime) void OpenGLExampleBrowser::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem) { + gDisableDemoSelection = true; sSharedMem = sharedMem; } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ca868b29f..59e81ac4c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -107,6 +107,7 @@ struct MotionThreadLocalStorage }; int skip = 0; +int skip1 = 0; void MotionThreadFunc(void* userPtr,void* lsMemory) { @@ -134,8 +135,15 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) if (deltaTimeInSeconds<(1./5000.)) { skip++; + skip1++; + if (0==(skip1&0x3)) + { + b3Clock::usleep(250); + } } else { + skip1=0; + //process special controller commands, such as //VR controller button press/release and controller motion @@ -186,8 +194,9 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) } - printf("finished, #skip = %d\n",skip); + printf("finished, #skip = %d, skip1 = %d\n",skip,skip1); skip=0; + skip1=0; //do nothing } @@ -638,8 +647,10 @@ void PhysicsServerExample::initPhysics() int index = 0; m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w); + while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized) { + b3Clock::usleep(1000); } } @@ -669,6 +680,7 @@ void PhysicsServerExample::exitPhysics() } else { + b3Clock::usleep(1000); } }; diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 5babcb023..6ca0efa36 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -348,7 +348,12 @@ struct TinyRendererGUIHelper : public GUIHelperInterface } - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied) + + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied) { if (numPixelsCopied) *numPixelsCopied = 0; diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index 6802726d9..d2d5d0930 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -36,6 +36,7 @@ const T& b3ClockMin(const T& a, const T& b) #else //_WIN32 #include +#include #endif //_WIN32 @@ -227,3 +228,21 @@ double b3Clock::getTimeInSeconds() return double(getTimeMicroseconds()/1.e6); } +void b3Clock::usleep(int microSeconds) +{ +#ifdef _WIN32 + int millis = microSeconds/1000; + if (millis < 1) + { + millis = 1; + } + Sleep(millis); +#else + + usleep(microSeconds); / + //struct timeval tv; + //tv.tv_sec = microSeconds/1000000L; + //tv.tv_usec = microSeconds%1000000L; + //return select(0, 0, 0, 0, &tv); +#endif +} diff --git a/examples/Utils/b3Clock.h b/examples/Utils/b3Clock.h index cb1c0c29d..10a36686a 100644 --- a/examples/Utils/b3Clock.h +++ b/examples/Utils/b3Clock.h @@ -28,6 +28,10 @@ public: /// the Clock was created. double getTimeInSeconds(); + ///Sleep for 'microSeconds', to yield to other threads and not waste 100% CPU cycles. + ///Note that some operating systems may sleep a longer time. + static void usleep(int microSeconds); + private: struct b3ClockData* m_data; }; diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index a0585a128..cacb7218e 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -5,7 +5,6 @@ project ("pybullet") kind "SharedLib" targetsuffix ("") targetprefix ("") - targetextension (".so") includedirs {"../../src", "../../examples", "../../examples/ThirdPartyLibs"} defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} From 985a6c203f6d36626b5285e15cc5bb2021e32f95 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 13 Aug 2016 12:38:40 -0700 Subject: [PATCH 33/36] fix typo --- examples/Utils/b3Clock.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index d2d5d0930..7482c302d 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -239,7 +239,7 @@ void b3Clock::usleep(int microSeconds) Sleep(millis); #else - usleep(microSeconds); / + usleep(microSeconds); //struct timeval tv; //tv.tv_sec = microSeconds/1000000L; //tv.tv_usec = microSeconds%1000000L; From 9c4cfde3d68f2629cbffd0b3f3f84a0c7efc30c3 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 15 Aug 2016 13:19:00 -0700 Subject: [PATCH 34/36] Fixed constraint for btMultiBody and btRigidBody. --- examples/MultiBody/MultiDofDemo.cpp | 17 +- src/BulletDynamics/CMakeLists.txt | 2 + .../btGeneric6DofSpring2Constraint.h | 16 +- .../Featherstone/btMultiBody.cpp | 10 + src/BulletDynamics/Featherstone/btMultiBody.h | 6 +- .../Featherstone/btMultiBodyConstraint.cpp | 668 +++++++++--------- .../Featherstone/btMultiBodyConstraint.h | 12 +- .../btMultiBodyFixedConstraint.cpp | 211 ++++++ .../Featherstone/btMultiBodyFixedConstraint.h | 94 +++ .../btMultiBodyJointLimitConstraint.cpp | 2 +- .../Featherstone/btMultiBodyJointMotor.cpp | 2 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 2 +- 12 files changed, 708 insertions(+), 334 deletions(-) create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp index f4dc2f673..a040702bc 100644 --- a/examples/MultiBody/MultiDofDemo.cpp +++ b/examples/MultiBody/MultiDofDemo.cpp @@ -10,6 +10,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" #include "../OpenGLWindow/GLInstancingRenderer.h" #include "BulletCollision/CollisionShapes/btShapeHull.h" @@ -134,7 +135,8 @@ void MultiDofDemo::initPhysics() bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool multibodyOnly = false; bool canSleep = true; - bool selfCollide = false; + bool selfCollide = false; + bool multibodyConstraint = true; btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); @@ -236,7 +238,18 @@ void MultiDofDemo::initPhysics() m_dynamicsWorld->addRigidBody(body);//,1,1+2); - + if (multibodyConstraint) { + btVector3 pointInA = -linkHalfExtents; + btVector3 pointInB = halfExtents; + btMatrix3x3 frameInA; + btMatrix3x3 frameInB; + frameInA.setIdentity(); + frameInB.setIdentity(); + btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB); + //btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB); + p2p->setMaxAppliedImpulse(2.0); + m_dynamicsWorld->addMultiBodyConstraint(p2p); + } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index b8c37b0d2..e511e6372 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -32,6 +32,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyJointLimitConstraint.cpp Featherstone/btMultiBodyConstraint.cpp Featherstone/btMultiBodyPoint2Point.cpp + Featherstone/btMultiBodyFixedConstraint.cpp Featherstone/btMultiBodyJointMotor.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp @@ -89,6 +90,7 @@ SET(Featherstone_HDRS Featherstone/btMultiBodyJointLimitConstraint.h Featherstone/btMultiBodyConstraint.h Featherstone/btMultiBodyPoint2Point.h + Featherstone/btMultiBodyFixedConstraint.h Featherstone/btMultiBodyJointMotor.h ) diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index 738d52ce8..57b4e1983 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -319,14 +319,6 @@ protected: const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false); - static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); - static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz); - public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -489,6 +481,14 @@ public: //If no axis is provided, it uses the default axis for this constraint. virtual void setParam(int num, btScalar value, int axis = -1); virtual btScalar getParam(int num, int axis = -1) const; + + static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); + static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz); }; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index d56f1db14..edef315b3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -466,6 +466,16 @@ btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const } } +btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const +{ + btMatrix3x3 result = local_frame; + btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0)); + btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1)); + btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2)); + result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]); + return result; +} + void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const { int num_links = getNumLinks(); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index f2251a1e3..a676c0227 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -272,7 +272,11 @@ public: btVector3 localDirToWorld(int i, const btVector3 &vec) const; btVector3 worldPosToLocal(int i, const btVector3 &vec) const; btVector3 worldDirToLocal(int i, const btVector3 &vec) const; - + + // + // transform a frame in local coordinate to a frame in world coordinate + // + btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const; // // calculate kinetic energy and angular momentum diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 12997d2e3..119a24c60 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -53,323 +53,359 @@ void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala } btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& constraintNormalAng, + const btVector3& constraintNormalLin, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint, + btScalar relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - - - solverConstraint.m_multiBodyA = m_bodyA; - solverConstraint.m_multiBodyB = m_bodyB; - solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); - btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary - if (bodyA) - rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); - - if (multiBodyA) - { - if (solverConstraint.m_linkA<0) - { - rel_pos1 = posAworld - multiBodyA->getBasePos(); - } else - { - rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); - } - - const int ndofA = multiBodyA->getNumDofs() + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom - //resize.. - solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - //copy/determine - if(jacOrgA) - { - for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - //determine.. - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - else //if(rb0) - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - if (solverConstraint.m_linkB<0) - { - rel_pos2 = posBworld - multiBodyB->getBasePos(); - } else - { - rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); - } - - const int ndofB = multiBodyB->getNumDofs() + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom - //resize.. - solverConstraint.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - //copy/determine.. - if(jacOrgB) - { - for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - //determine.. - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - - } - else //if(rb1) - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* deltaVelA = 0; - btScalar* deltaVelB = 0; - int ndofA = 0; - //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l = deltaVelA[i]; - denom0 += j*l; - } - } - else if(rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - // - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l = deltaVelB[i]; - denom1 += j*l; - } - - } - else if(rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } - - // - btScalar d = denom0+denom1; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } - else - { - //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - } - - - //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else if(rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumDofs() + 6; - btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - else if(rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary + if (bodyA) + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); + + if (multiBodyA) + { + if (solverConstraint.m_linkA<0) + { + rel_pos1 = posAworld - multiBodyA->getBasePos(); + } else + { + rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom + //resize.. + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + //copy/determine + if(jacOrgA) + { + for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + //determine.. + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + + btVector3 torqueAxis0; + if (angConstraint) { + torqueAxis0 = constraintNormalAng; + } + else { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + + } + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormalLin; + } + else //if(rb0) + { + btVector3 torqueAxis0; + if (angConstraint) { + torqueAxis0 = constraintNormalAng; + } + else { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormalLin; + } + + if (multiBodyB) + { + if (solverConstraint.m_linkB<0) + { + rel_pos2 = posBworld - multiBodyB->getBasePos(); + } else + { + rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom + //resize.. + solverConstraint.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + //copy/determine.. + if(jacOrgB) + { + for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + //determine.. + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + + btVector3 torqueAxis1; + if (angConstraint) { + torqueAxis1 = constraintNormalAng; + } + else { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormalLin; + } + else //if(rb1) + { + btVector3 torqueAxis1; + if (angConstraint) { + torqueAxis1 = constraintNormalAng; + } + else { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormalLin; + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 0; + int ndofA = 0; + //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l = deltaVelA[i]; + denom0 += j*l; + } + } + else if(rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + if (angConstraint) { + denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec); + } + else { + denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); + } + } + // + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l = deltaVelB[i]; + denom1 += j*l; + } + + } + else if(rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + if (angConstraint) { + denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec); + } + else { + denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); + } + } + + // + btScalar d = denom0+denom1; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + } + + + //compute rhs and remaining solverConstraint fields + btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else if(rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + else if(rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + + solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + } + + + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - positionalError = -penetration * erp/infoGlobal.m_timeStep; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; - + } + } + } else + */ + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + positionalError = -penetration * erp/infoGlobal.m_timeStep; + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; + } + + return rel_vel; + } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 137b34d87..74c6f5a81 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -66,15 +66,19 @@ protected: btAlignedObjectArray m_data; void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& constraintNormalAng, + + const btVector3& constraintNormalLin, const btVector3& posAworld, const btVector3& posBworld, btScalar posError, const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint = false, + btScalar relaxation = 1.f, bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp new file mode 100644 index 000000000..0f0d9f67b --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -0,0 +1,211 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyFixedConstraint.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +#define BTMBFIXEDCONSTRAINT_DIM 6 + +btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) +{ + m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses +} + +btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) +{ + m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses +} + +void btMultiBodyFixedConstraint::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() +{ +} + + +int btMultiBodyFixedConstraint::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyFixedConstraint::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + +void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) +{ + int numDim = BTMBFIXEDCONSTRAINT_DIM; + for (int i=0;igetCompanionId(); + pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; + frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation()); + + } else + { + if (m_bodyA) { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + } + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation()); + + } else + { + if (m_bodyB) { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld); + } + } + + btMatrix3x3 relRot = frameAworld.inverse()*frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff); + + btVector3 constraintNormalLin(0,0,0); + btVector3 constraintNormalAng(0,0,0); + btScalar posError = 0.0; + if (i < 3) { + constraintNormalLin[i] = -1; + posError = (pivotAworld-pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); + } + else { //i>=3 + constraintNormalAng = frameAworld.getColumn(i%3); + posError = angleDiff[i%3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true + ); + } + } +} + +void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h new file mode 100644 index 000000000..26e28a74e --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -0,0 +1,94 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H +#define BT_MULTIBODY_FIXED_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyFixedConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + +public: + + btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyFixedConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer); + +}; + +#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 3f05aa4d5..707817673 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -122,7 +122,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint const btScalar posError = 0; //why assume it's zero? const btVector3 dummy(0, 0, 0); - btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); + btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); { //expect either prismatic or revolute joint type for now diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index ebc1042b4..326a6ac48 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -128,7 +128,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; - fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs); + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 2ccb9827d..3e28f80df 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -159,7 +159,7 @@ int numDim = BTMBP2PCONSTRAINT_DIM; #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - fillMultiBodyConstraint(constraintRow, data, 0, 0, + fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0), contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" posError, infoGlobal, From 4bc31394a0790187a58425b5e5edd20f1fac63bb Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 16 Aug 2016 16:57:48 -0700 Subject: [PATCH 35/36] Allow choosing loaded as btRigidBody with RobotSimAPI. --- data/cube_small.sdf | 35 +++++++++++++++++++ .../RoboticsLearning/GripperGraspExample.cpp | 6 ++-- examples/RoboticsLearning/b3RobotSimAPI.cpp | 6 +++- examples/RoboticsLearning/b3RobotSimAPI.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 12 +++++-- .../PhysicsServerCommandProcessor.cpp | 2 +- 6 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 data/cube_small.sdf diff --git a/data/cube_small.sdf b/data/cube_small.sdf new file mode 100644 index 000000000..828b74df1 --- /dev/null +++ b/data/cube_small.sdf @@ -0,0 +1,35 @@ + + + + 0 0 0.107 0 0 0 + + + 0.1 + + 1.0 + 0 + 0 + 1.0 + 0 + 1.0 + + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + cube.obj + + + + + + + diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index ebdc39acb..514ac970d 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -82,7 +82,6 @@ public: m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; @@ -132,9 +131,11 @@ public: { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; + args.m_fileName = "cube_small.sdf"; args.m_startPosition.setValue(0,0,.107); args.m_startOrientation.setEulerZYX(0,0,0); + args.m_useMultiBody = false; + args.m_fileType = 2; m_robotSim.loadFile(args,results); } if (1) @@ -144,6 +145,7 @@ public: args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; + args.m_useMultiBody = true; b3RobotSimLoadFileResults results; m_robotSim.loadFile(args,results); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index adbef7a20..634ba3cd2 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -211,7 +211,10 @@ public: return m_cs; } - virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){} + virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) + { + createCollisionObjectGraphicsObject((btCollisionObject*)body, color); + } btCollisionObject* m_obj; btVector3 m_color2; @@ -685,6 +688,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS { b3LoadUrdfCommandSetUseFixedBase(command,true); } + b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); statusType = b3GetStatusType(statusHandle); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 3666700d2..9a4a14831 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -23,6 +23,7 @@ struct b3RobotSimLoadFileArgs b3Vector3 m_startPosition; b3Quaternion m_startOrientation; bool m_forceOverrideFixedBase; + bool m_useMultiBody; int m_fileType; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8530c0c5a..e60a4f5d2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -55,6 +55,16 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie return (b3SharedMemoryCommandHandle) command; } +int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY; + command->m_urdfArguments.m_useMultiBody = useMultiBody; + + return 0; +} int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) { @@ -67,8 +77,6 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, return 0; } - - int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3ada5bca..f1436516b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -877,6 +877,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } btMultiBody* mb = creation.getBulletMultiBody(); + if (useMultiBody) { @@ -944,7 +945,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } else { - btAssert(0); return true; } From 591f922d97b0bbed3433ab542fe912cc763c19e7 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 16 Aug 2016 17:56:30 -0700 Subject: [PATCH 36/36] Support loading Urdf as btRigidBody with RobotSimAPI. Loading Sdf as btRigidBody is work in progress. --- examples/RoboticsLearning/GripperGraspExample.cpp | 4 ++-- examples/RoboticsLearning/b3RobotSimAPI.cpp | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 11 +++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsServerCommandProcessor.cpp | 6 +++--- examples/SharedMemory/PhysicsServerCommandProcessor.h | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 1 + 7 files changed, 20 insertions(+), 6 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 514ac970d..84b5fc7db 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -86,6 +86,7 @@ public: b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; args.m_fileType = B3_SDF_FILE; + args.m_useMultiBody = true; b3RobotSimLoadFileResults results; if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) @@ -131,11 +132,10 @@ public: { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.sdf"; + args.m_fileName = "cube_small.urdf"; args.m_startPosition.setValue(0,0,.107); args.m_startOrientation.setEulerZYX(0,0,0); args.m_useMultiBody = false; - args.m_fileType = 2; m_robotSim.loadFile(args,results); } if (1) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 634ba3cd2..15cc08a21 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -703,6 +703,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str()); + b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); statusType = b3GetStatusType(statusHandle); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e60a4f5d2..98d6d2e19 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -66,6 +66,17 @@ int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, return 0; } +int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_SDF); + command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY; + command->m_sdfArguments.m_useMultiBody = useMultiBody; + + return 0; +} + int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6f7b13647..545b70b9e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -96,6 +96,7 @@ int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f1436516b..5e2970b83 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -711,7 +711,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) } -bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes) +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody) { btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) @@ -768,7 +768,6 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe MyMultiBodyCreator creation(m_data->m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - bool useMultiBody = true; ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); @@ -1244,8 +1243,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? sdfArgs.m_useMultiBody : true; - bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody); if (completedOk) { //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 692bc2dd9..6526fe030 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -21,7 +21,7 @@ protected: - bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes); + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ac5781639..f7a9812ca 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -64,6 +64,7 @@ enum EnumSdfArgsUpdateFlags struct SdfArgs { char m_sdfFileName[MAX_URDF_FILENAME_LENGTH]; + int m_useMultiBody; }; enum EnumUrdfArgsUpdateFlags