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 977687da3..cac015d61 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,25 @@ 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, "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), + 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 68626554c..577d24c85 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -802,6 +802,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..8f902b008 --- /dev/null +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -0,0 +1,385 @@ +#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_DLS_SVD: + jacob->CalcDeltaThetasDLSwithSVD(); + 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..845925ef6 --- /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 , IK_DLS_SVD}; + +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..2b0454ea8 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -0,0 +1,611 @@ +/* +* +* 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://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html +* +* +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 "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::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..b1c683f8a --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -0,0 +1,131 @@ + +/* +* +* 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://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html +* +* +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 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..51f57a666 --- /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 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..6aa696d55 --- /dev/null +++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp @@ -0,0 +1,153 @@ +/* +* +* 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 + + +/**************************************************************** + 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 cross( float [3], float [3], float [3] ); +float dot( float [3], float [3] ); +float unit( float [3], float [3] ); + + + + + +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..6891f0601 --- /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://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html +* +* +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..673f41c9e --- /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://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html +* +* +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;