From 75e86051c2724f159cecf12bd6b683ab8b83b82b Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Sun, 24 Jul 2016 22:22:42 -0700 Subject: [PATCH 1/4] 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 2/4] 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 3/4] 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 4/4] 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);