Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' of https://github.com/YunfeiBai/bullet3

Browse Source
This commit is contained in:
YunfeiBai
2016-08-19 12:01:09 -07:00
parent d26c424e11 591f922d97
commit 4611f5f097
106 changed files with 12975 additions and 856 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/InverseDynamics/DillCreator.cpp
View File

@@ -44,7 +44,7 @@ int DillCreator::getNumBodies(int* num_bodies) const {
return 0;
}
int DillCreator::getBody(int body_index, int* parent_index, JointType* joint_type,
int DillCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {

2
Extras/InverseDynamics/DillCreator.hpp
View File

@@ -22,7 +22,7 @@ public:
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(int body_index, int* parent_index, JointType* joint_type,
int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;

2
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
View File

@@ -237,7 +237,7 @@ int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const {
return 0;
}
int btMultiBodyTreeCreator::getBody(int body_index, int *parent_index, JointType *joint_type,
int btMultiBodyTreeCreator::getBody(const int body_index, int *parent_index, JointType *joint_type,
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass,
vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,

2
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
View File

@@ -25,7 +25,7 @@ public:
/// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int *num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
int getBody(int body_index, int *parent_index, JointType *joint_type,
int getBody(const int body_index, int *parent_index, JointType *joint_type,
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion,
idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
void **user_ptr) const;

40
build3/premake4.lua
View File

@@ -85,10 +85,34 @@
newoption
{
trigger = "python",
description = "Enable Python scripting (experimental, use Physics Server in Example Browser). "
trigger = "enable_pybullet",
description = "Enable high-level Python scripting of Bullet with URDF/SDF import and synthetic camera."
}
if os.is("Linux") then
default_python_include_dir = "/usr/include/python2.7"
default_python_lib_dir = "/usr/local/lib/"
end
if os.is("Windows") then
default_python_include_dir = "C:\Python-3.5.2/include"
default_python_lib_dir = "C:/Python-3.5.2/libs"
end
newoption
{
trigger = "python_include_dir",
value = default_python_include_dir,
description = "Python (2.x or 3.x) include directory"
}
newoption
{
trigger = "python_lib_dir",
value = default_python_lib_dir,
description = "Python (2.x or 3.x) library directory "
}
newoption {
@@ -140,7 +164,7 @@
platforms {"x32"}
end
else
platforms {"x32", "x64"}
platforms {"x32","x64"}
end
configuration {"x32"}
@@ -191,6 +215,14 @@
targetdir( _OPTIONS["targetdir"] or "../bin" )
location("./" .. act .. postfix)
if not _OPTIONS["python_include_dir"] then
_OPTIONS["python_include_dir"] = default_python_include_dir
end
if not _OPTIONS["python_lib_dir"] then
_OPTIONS["python_lib_dir"] = default_python_lib_dir
end
projectRootDir = os.getcwd() .. "/../"
print("Project root directory: " .. projectRootDir);
@@ -222,7 +254,7 @@
if _OPTIONS["lua"] then
include "../examples/ThirdPartyLibs/lua-5.2.3"
end
if _OPTIONS["python"] then
if _OPTIONS["enable_pybullet"] then
include "../examples/pybullet"
end

2
build_and_run_cmake.sh
View File

@@ -2,6 +2,6 @@
rm CMakeCache.txt
mkdir build_cmake
cd build_cmake
cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release ..
cmake -DBUILD_PYBULLET=OFF -DCMAKE_BUILD_TYPE=Release ..
make -j12
examples/ExampleBrowser/App_ExampleBrowser

3
build_visual_studio.bat
View File

@@ -2,7 +2,8 @@
rem premake4 --with-pe vs2010
rem premake4 --bullet2demos vs2010
cd build3
premake4 --enable_openvr --targetdir="../bin" vs2010
premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010
rem premake4 --targetdir="../server2bin" vs2010
rem cd vs2010
rem rename 0_Bullet3Solution.sln 0_server.sln

35
data/cube_small.sdf Normal file
View File

@@ -0,0 +1,35 @@
<sdf version='1.6'>
<world name='default'>
<model name='unit_box_0'>
<pose frame=''>0 0 0.107 0 0 0</pose>
<link name='unit_box_0::link'>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>0.05 0.05 0.05</scale>
<uri>cube.obj</uri>
</mesh>
</geometry>
</visual>
</link>
</model>
</world>
</sdf>

33
data/cube_small.urdf Normal file
View File

@@ -0,0 +1,33 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale=".05 .05 .05"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
</collision>
</link>
</robot>

BIN
data/gripper/meshes/GUIDE_WSG50_110.stl Normal file
View File

Binary file not shown.

BIN
data/gripper/meshes/WSG-FMF.stl Normal file
View File

Binary file not shown.

BIN
data/gripper/meshes/WSG50_110.stl Normal file
View File

Binary file not shown.

BIN
data/gripper/meshes/l_gripper_tip_scaled.stl Normal file
View File

Binary file not shown.

298
data/gripper/wsg50_with_r2d2_gripper.sdf Normal file
View File

@@ -0,0 +1,298 @@
<?xml version="1.0" ?>
<sdf version='1.6'>
<world name='default'>
<model name='wsg50_with_gripper'>
<pose frame=''>0 0 0.27 3.14 0 0</pose>
<link name='world'>
</link>
<joint name='base_joint' type='prismatic'>
<parent>world</parent>
<child>base_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.5</lower>
<upper>10</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>100</damping>
<friction>100</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='base_link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>1.2</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<collision name='base_link_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.2 0.05 0.05 </size>
</box>
</geometry>
</collision>
<visual name='base_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/WSG50_110.stl</uri>
</mesh>
</geometry>
<material>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<self_collide>0</self_collide>
</link>
<link name='gripper_left'>
<pose frame=''>-0.055 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='gripper_left_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</collision>
<collision name='gripper_left_fixed_joint_lump__finger_left_collision_1'>
<pose frame=''>0 0 0.023 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='gripper_left_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
<pose frame=''>0 0 0.023 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='base_joint_gripper_left' type='prismatic'>
<child>gripper_left</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.001</lower>
<upper>0.055</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>100</damping>
<friction>100</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='gripper_right'>
<pose frame=''>0.055 0 0 0 -0 3.14159</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='gripper_right_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</collision>
<collision name='gripper_right_fixed_joint_lump__finger_right_collision_1'>
<pose frame=''>0 0 0.023 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='gripper_right_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
<pose frame=''>0 0 0.023 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='base_joint_gripper_right' type='prismatic'>
<child>gripper_right</child>
<parent>base_link</parent>
<axis>
<xyz>-1 0 0</xyz>
<limit>
<lower>-0.055</lower>
<upper>0.001</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>100</damping>
<friction>100</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='finger_right'>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='finger_right_collision'>
<pose frame=''>0 0 0.042 0 0 0 </pose>
<geometry>
<box>
<size>0.02 0.02 0.15 </size>
</box>
</geometry>
</collision>
<visual name='finger_right_visual'>
<pose frame=''>0 0 0 0 0 0 </pose>
<geometry>
<mesh>
<scale>1 1 1 </scale>
<uri>meshes/l_gripper_tip_scaled.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_finger_right' type='fixed'>
<parent>gripper_right</parent>
<child>finger_right</child>
</joint>
<link name='finger_left'>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<collision name='finger_left_collision'>
<pose frame=''>0 0 0.042 0 0 0 </pose>
<geometry>
<box>
<size>0.02 0.02 0.15 </size>
</box>
</geometry>
</collision>
<visual name='finger_left_visual'>
<pose frame=''>0 0 0 0 0 0 </pose>
<geometry>
<mesh>
<scale>1 1 1 </scale>
<uri>meshes/l_gripper_tip_scaled.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_finger_left' type='fixed'>
<parent>gripper_left</parent>
<child>finger_left</child>
</joint>
</model>
</world>
</sdf>

25
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -46,7 +46,23 @@ struct GUIHelperInterface
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
{
copyCameraImageData(viewMatrix,projectionMatrix,pixelsRGBA,rgbaBufferSizeInPixels,
depthBuffer,depthBufferSizeInPixels,
0,0,
startPixelIndex,destinationWidth,
destinationHeight,numPixelsCopied);
}
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0;
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;
@@ -107,7 +123,12 @@ struct DummyGUIHelper : public GUIHelperInterface
{
}
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied)
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int width, int height, int* numPixelsCopied)
{
if (numPixelsCopied)
*numPixelsCopied = 0;

15
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -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
@@ -208,6 +210,8 @@ SET(BulletExampleBrowser_SRCS
../RenderingExamples/TimeSeriesCanvas.h
../RenderingExamples/TimeSeriesFontData.cpp
../RenderingExamples/TimeSeriesFontData.h
../RoboticsLearning/GripperGraspExample.cpp
../RoboticsLearning/GripperGraspExample.h
../RoboticsLearning/b3RobotSimAPI.cpp
../RoboticsLearning/b3RobotSimAPI.h
../RoboticsLearning/R2D2GraspExample.cpp
@@ -299,6 +303,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
../ThirdPartyLibs/tinyxml/tinyxml.cpp

25
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -46,6 +46,8 @@
#include "../MultiThreading/MultiThreadingExample.h"
#include "../InverseDynamics/InverseDynamicsExample.h"
#include "../RoboticsLearning/R2D2GraspExample.h"
#include "../RoboticsLearning/GripperGraspExample.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
#ifdef ENABLE_LUA
#include "../LuaDemo/LuaPhysicsSetup.h"
@@ -95,7 +97,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 +125,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),
@@ -249,6 +261,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc),

2
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -332,6 +332,7 @@ btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,
while (data->m_args.m_cs->getSharedParam(0)==eExampleBrowserIsUnInitialized)
{
b3Clock::usleep(1000);
}
return data;
@@ -366,6 +367,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data)
} else
{
// printf("polling..");
b3Clock::usleep(1000);
}
};

69
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -107,7 +107,7 @@ static CommonExampleInterface* sCurrentDemo = 0;
static b3AlignedObjectArray<const char*> allNames;
static float gFixedTimeStep = 0;
bool gAllowRetina = true;
bool gDisableDemoSelection = false;
static class ExampleEntries* gAllExamples=0;
bool sUseOpenGL2 = false;
bool drawGUI=true;
@@ -157,6 +157,7 @@ void deleteDemo()
}
const char* gPngFileName = 0;
int gPngSkipFrames = 0;
@@ -519,6 +520,8 @@ void MyStatusBarError(const char* msg)
gui2->textOutput(msg);
gui2->forceUpdateScrollBars();
}
btAssert(0);
}
struct MyMenuItemHander :public Gwen::Event::Handler
@@ -553,9 +556,11 @@ struct MyMenuItemHander :public Gwen::Event::Handler
Gwen::String laa = Gwen::Utility::UnicodeToString(la);
//const char* ha = laa.c_str();
selectDemo(sCurrentHightlighted);
saveCurrentSettings(sCurrentDemoIndex, startFileName);
if (!gDisableDemoSelection )
{
selectDemo(sCurrentHightlighted);
saveCurrentSettings(sCurrentDemoIndex, startFileName);
}
}
void onButtonC(Gwen::Controls::Base* pControl)
{
@@ -577,8 +582,11 @@ struct MyMenuItemHander :public Gwen::Event::Handler
*/
// printf("onKeyReturn ! \n");
selectDemo(sCurrentHightlighted);
saveCurrentSettings(sCurrentDemoIndex, startFileName);
if (!gDisableDemoSelection )
{
selectDemo(sCurrentHightlighted);
saveCurrentSettings(sCurrentDemoIndex, startFileName);
}
}
@@ -631,10 +639,12 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS];
GraphingTexture* m_gt[MAX_GRAPH_WINDOWS];
int m_curNumGraphWindows;
int m_curXpos;
QuickCanvas(GL3TexLoader* myTexLoader)
:m_myTexLoader(myTexLoader),
m_curNumGraphWindows(0)
m_curNumGraphWindows(0),
m_curXpos(0)
{
for (int i=0;i<MAX_GRAPH_WINDOWS;i++)
{
@@ -658,7 +668,8 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphInput input(gui2->getInternalData());
input.m_width=width;
input.m_height=height;
input.m_xPos = 10000;//GUI will clamp it to the right//300;
input.m_xPos = m_curXpos;//GUI will clamp it to the right//300;
m_curXpos+=width+20;
input.m_yPos = 10000;//GUI will clamp it to bottom
input.m_name=canvasName;
input.m_texName = canvasName;
@@ -674,6 +685,7 @@ struct QuickCanvas : public Common2dCanvasInterface
}
virtual void destroyCanvas(int canvasId)
{
m_curXpos = 0;
btAssert(canvasId>=0);
delete m_gt[canvasId];
m_gt[canvasId] = 0;
@@ -761,7 +773,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
loadCurrentSettings(startFileName, args);
args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep);
args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames);
///The OpenCL rigid body pipeline is experimental and
///most OpenCL drivers and OpenCL compilers have issues with our kernels.
///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers
@@ -800,6 +812,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
{
@@ -1098,24 +1111,6 @@ void OpenGLExampleBrowser::update(float deltaTime)
//printf("---------------------------------------------------\n");
//printf("Framecount = %d\n",frameCount);
if (gPngFileName)
{
static int skip = 0;
skip++;
if (skip>4)
{
skip=0;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);
s_app->dumpNextFrameToPng(staticPngFileName);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
}
if (gFixedTimeStep>0)
{
@@ -1150,6 +1145,25 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
}
if (gPngFileName)
{
static int skip = 0;
skip--;
if (skip<0)
{
skip=gPngSkipFrames;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);
s_app->dumpNextFrameToPng(staticPngFileName);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
}
{
if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())
@@ -1219,5 +1233,6 @@ void OpenGLExampleBrowser::update(float deltaTime)
void OpenGLExampleBrowser::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
{
gDisableDemoSelection = true;
sSharedMem = sharedMem;
}

7
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -338,7 +338,12 @@ void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float c
}
void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied)
{
int sourceWidth = m_data->m_glApp->m_window->getWidth()*m_data->m_glApp->m_window->getRetinaScale();
int sourceHeight = m_data->m_glApp->m_window->getHeight()*m_data->m_glApp->m_window->getRetinaScale();

7
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -44,7 +44,12 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied);
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied);
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ;

4
examples/ExampleBrowser/premake4.lua
View File

@@ -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",

72
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "btMatrix4x4.h"
#define MAX_VISUAL_SHAPES 512
struct VertexSource
@@ -288,42 +289,47 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
}//for each mesh
int shapeIndex = visualShapes.size();
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
if (shapeIndex<MAX_VISUAL_SHAPES)
{
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
btAssert(vertexNormals.size()==vertexPositions.size());
for (int v=0;v<vertexPositions.size();v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
btAssert(vertexNormals.size()==vertexPositions.size());
for (int v=0;v<vertexPositions.size();v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
for (int index=0;index<indices.size();index++)
{
visualShape.m_indices->push_back(indices[index]+indexBase);
}
for (int index=0;index<indices.size();index++)
{
visualShape.m_indices->push_back(indices[index]+indexBase);
}
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase=visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName,shapeIndex);
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase=visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName,shapeIndex);
} else
{
b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES);
}
}//for each geometry
}
@@ -557,7 +563,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
// GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(32);
visualShapes.reserve(MAX_VISUAL_SHAPES);
float extraScaling = 1;//0.01;
btHashMap<btHashString, int> name2ShapeIndex;

2
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -67,7 +67,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
} else
{
b3Warning("not found %s\n",relativeFileName);
b3Warning("not found [%s]\n",relativeFileName);
}
}
}

17
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -45,6 +45,7 @@ struct BulletURDFInternalData
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
char m_pathPrefix[1024];
int m_bodyId;
btHashMap<btHashInt,btVector4> m_linkColors;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
@@ -209,6 +210,18 @@ const char* BulletURDFImporter::getPathPrefix()
}
void BulletURDFImporter::setBodyUniqueId(int bodyId)
{
m_data->m_bodyId =bodyId;
}
int BulletURDFImporter::getBodyUniqueId() const
{
return m_data->m_bodyId;
}
BulletURDFImporter::~BulletURDFImporter()
{
delete m_data;
@@ -1017,13 +1030,13 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
return false;
}
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const
{
if (m_data->m_customVisualShapesConverter)
{
const UrdfModel& model = m_data->m_urdfParser.getModel();
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex);
}
}

5
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -25,7 +25,8 @@ public:
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
const char* getPathPrefix();
void printTree(); //for debugging
@@ -50,7 +51,7 @@ public:
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation

2
examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
View File

@@ -3,7 +3,7 @@
struct LinkVisualShapesConverter
{
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj)=0;
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
};
#endif //LINK_VISUAL_SHAPES_CONVERTER_H

9
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -232,6 +232,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (mass)
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
//temporary inertia scaling until we load inertia from URDF
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
{
localInertiaDiagonal*=contactInfo.m_inertiaScaling;
}
}
btRigidBody* linkRigidBody = 0;
@@ -390,7 +397,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);

4
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -48,7 +48,9 @@ public:
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const { }
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
virtual void setBodyUniqueId(int bodyId) {}
virtual int getBodyUniqueId() const { return 0;}
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
};

5
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -17,7 +17,8 @@ enum URDF_LinkContactFlags
{
URDF_CONTACT_HAS_LATERAL_FRICTION=1,
URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_INERTIA_SCALING=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8
};
@@ -25,6 +26,7 @@ struct URDFLinkContactInfo
{
btScalar m_lateralFriction;
btScalar m_rollingFriction;
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
int m_flags;
@@ -32,6 +34,7 @@ struct URDFLinkContactInfo
URDFLinkContactInfo()
:m_lateralFriction(0.5),
m_rollingFriction(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0)
{

23
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -606,6 +606,28 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
@@ -623,6 +645,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
}

21
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -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<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> 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());
}
}
}

385
examples/InverseKinematics/InverseKinematicsExample.cpp Normal file
View File

@@ -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 / 5.;
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<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> 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);
}

8
examples/InverseKinematics/InverseKinematicsExample.h Normal file
View File

@@ -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

15
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -10,6 +10,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
@@ -135,6 +136,7 @@ void MultiDofDemo::initPhysics()
bool multibodyOnly = false;
bool canSleep = true;
bool selfCollide = false;
bool multibodyConstraint = true;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
@@ -236,7 +238,18 @@ void MultiDofDemo::initPhysics()
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
if (multibodyConstraint) {
btVector3 pointInA = -linkHalfExtents;
btVector3 pointInB = halfExtents;
btMatrix3x3 frameInA;
btMatrix3x3 frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB);
//btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
p2p->setMaxAppliedImpulse(2.0);
m_dynamicsWorld->addMultiBodyConstraint(p2p);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

24
examples/MultiThreading/b3PosixThreadSupport.cpp
View File

@@ -44,8 +44,6 @@ b3PosixThreadSupport::~b3PosixThreadSupport()
#define NAMED_SEMAPHORES
#endif
// this semaphore will signal, if and how many threads are finished with their work
static sem_t* mainSemaphore=0;
static sem_t* createSem(const char* baseName)
{
@@ -100,12 +98,12 @@ static void *threadFunction(void *argument)
b3Assert(status->m_status);
status->m_userThreadFunc(userPtr,status->m_lsMemory);
status->m_status = 2;
checkPThreadFunction(sem_post(mainSemaphore));
checkPThreadFunction(sem_post(status->m_mainSemaphore));
status->threadUsed++;
} else {
//exit Thread
status->m_status = 3;
checkPThreadFunction(sem_post(mainSemaphore));
checkPThreadFunction(sem_post(status->m_mainSemaphore));
printf("Thread with taskId %i exiting\n",status->m_taskId);
break;
}
@@ -160,13 +158,14 @@ bool b3PosixThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1,
b3Assert(m_activeThreadStatus.size());
// wait for any of the threads to finish
int result = sem_trywait(mainSemaphore);
int result = sem_trywait(m_mainSemaphore);
if (result==0)
{
// get at least one thread which has finished
size_t last = -1;
for(size_t t=0; t < size_t(m_activeThreadStatus.size()); ++t) {
int last = -1;
int status = -1;
for(int t=0; t < int (m_activeThreadStatus.size()); ++t) {
status = m_activeThreadStatus[t].m_status;
if(2 == m_activeThreadStatus[t].m_status) {
last = t;
break;
@@ -200,7 +199,7 @@ void b3PosixThreadSupport::waitForResponse( int *puiArgument0, int *puiArgument
b3Assert(m_activeThreadStatus.size());
// wait for any of the threads to finish
checkPThreadFunction(sem_wait(mainSemaphore));
checkPThreadFunction(sem_wait(m_mainSemaphore));
// get at least one thread which has finished
size_t last = -1;
@@ -231,7 +230,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
printf("%s creating %i threads.\n", __FUNCTION__, threadConstructionInfo.m_numThreads);
m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads);
mainSemaphore = createSem("main");
m_mainSemaphore = createSem("main");
//checkPThreadFunction(sem_wait(mainSemaphore));
for (int i=0;i < threadConstructionInfo.m_numThreads;i++)
@@ -249,6 +248,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
spuStatus.m_taskId = i;
spuStatus.m_commandId = 0;
spuStatus.m_status = 0;
spuStatus.m_mainSemaphore = m_mainSemaphore;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
spuStatus.threadUsed = 0;
@@ -271,7 +271,7 @@ void b3PosixThreadSupport::stopThreads()
spuStatus.m_userPtr = 0;
checkPThreadFunction(sem_post(spuStatus.startSemaphore));
checkPThreadFunction(sem_wait(mainSemaphore));
checkPThreadFunction(sem_wait(m_mainSemaphore));
printf("destroy semaphore\n");
destroySem(spuStatus.startSemaphore);
@@ -280,7 +280,7 @@ void b3PosixThreadSupport::stopThreads()
}
printf("destroy main semaphore\n");
destroySem(mainSemaphore);
destroySem(m_mainSemaphore);
printf("main semaphore destroyed\n");
m_activeThreadStatus.clear();
}

12
examples/MultiThreading/b3PosixThreadSupport.h
View File

@@ -57,14 +57,22 @@ public:
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc
pthread_t thread;
sem_t* startSemaphore;
pthread_t thread;
//each tread will wait until this signal to start its work
sem_t* startSemaphore;
// this is a copy of m_mainSemaphore,
//each tread will signal once it is finished with its work
sem_t* m_mainSemaphore;
unsigned long threadUsed;
};
private:
b3AlignedObjectArray<b3ThreadStatus> m_activeThreadStatus;
// m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work
sem_t* m_mainSemaphore;
public:
///Setup and initialize SPU/CELL/Libspe2

20
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -750,11 +750,13 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
void SimpleOpenGL3App::swapBuffer()
{
m_window->endRendering();
if (m_data->m_frameDumpPngFileName)
{
writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
(int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight();
writeTextureToFile(width,
height,m_data->m_frameDumpPngFileName,
m_data->m_ffmpegFile);
m_data->m_renderTexture->disable();
if (m_data->m_ffmpegFile==0)
@@ -762,7 +764,8 @@ void SimpleOpenGL3App::swapBuffer()
m_data->m_frameDumpPngFileName = 0;
}
}
m_window->startRendering();
m_window->endRendering();
m_window->startRendering();
}
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
@@ -773,12 +776,15 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
char cmd[8192];
#ifdef _WIN32
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
//sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
#else
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName);
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "

3
examples/OpenGLWindow/X11OpenGLWindow.cpp
View File

@@ -516,6 +516,9 @@ void X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_data->m_dpy = MyXOpenDisplay(NULL);
m_data->m_glWidth = ci.m_width;
m_data->m_glHeight = ci.m_height;
if(m_data->m_dpy == NULL) {
printf("\n\tcannot connect to X server\n\n");
exit(0);

7
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -23,6 +23,7 @@ struct TinyRendererSetupInternalData
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> m_depthBuffer;
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
int m_width;
@@ -185,8 +186,10 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_internalData->m_shapePtr.push_back(0);
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer);
//ob->loadModel("cube.obj");
m_internalData->m_depthBuffer,
&m_internalData->m_segmentationMaskBuffer,
m_internalData->m_renderObjects.size());
const int* indices = &meshData.m_gfxShape->m_indices->at(0);
ob->registerMeshShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,

233
examples/RoboticsLearning/GripperGraspExample.cpp Normal file
View File

@@ -0,0 +1,233 @@
#include "GripperGraspExample.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
static btScalar sGripperVerticalVelocity = -0.2f;
static btScalar sGripperClosingTargetVelocity = 0.5f;
class GripperGraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_options;
int m_r2d2Index;
int m_gripperIndex;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
GripperGraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_r2d2Index(-1),
m_gripperIndex(-1),
m_x(0),
m_y(0),
m_z(0)
{
m_app->setUpAxis(2);
}
virtual ~GripperGraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
{
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
slider.m_minVal=-2;
slider.m_maxVal=2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
);
slider.m_minVal=-1;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
args.m_fileType = B3_SDF_FILE;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
m_gripperIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
/*
int fingerJointIndices[2]={1,3};
double fingerTargetPositions[2]={-0.04,0.04};
for (int i=0;i<2;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_targetPosition = fingerTargetPositions[i];
controlArgs.m_kp = 5.0;
controlArgs.m_kd = 3.0;
controlArgs.m_maxTorqueValue = 1.0;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
*/
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={-0.2,.5,-.5};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0,0,.107);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = false;
m_robotSim.loadFile(args,results);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
if ((m_gripperIndex>=0))
{
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
,-sGripperClosingTargetVelocity
};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
//m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 1.5;
float pitch = 12;
float yaw = -10;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new GripperGraspExample(options.m_guiHelper, options.m_option);
}

23
examples/RoboticsLearning/GripperGraspExample.h Normal file
View File

@@ -0,0 +1,23 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GRIPPER_GRASP_EXAMPLE_H
#define GRIPPER_GRASP_EXAMPLE_H
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //GRIPPER_GRASP_EXAMPLE_H

86
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -47,6 +47,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eRobotSimGUIHelperCreateCollisionShapeGraphicsObject,
eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
eRobotSimGUIHelperRemoveAllGraphicsInstances,
eRobotSimGUIHelperCopyCameraImageData,
};
#include <stdio.h>
@@ -152,7 +153,7 @@ void* RobotlsMemoryFunc()
ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper2 : public GUIHelperInterface
{
CommonGraphicsApp* m_app;
@@ -185,7 +186,7 @@ public:
int m_instanceId;
MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
MultiThreadedOpenGLGuiHelper2(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
:m_app(app)
,m_cs(0),
m_texels(0),
@@ -195,7 +196,7 @@ public:
}
virtual ~MultiThreadedOpenGLGuiHelper()
virtual ~MultiThreadedOpenGLGuiHelper2()
{
delete m_childGuiHelper;
}
@@ -210,7 +211,10 @@ public:
return m_cs;
}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
{
createCollisionObjectGraphicsObject((btCollisionObject*)body, color);
}
btCollisionObject* m_obj;
btVector3 m_color2;
@@ -345,10 +349,48 @@ public:
{
}
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
float m_viewMatrix[16];
float m_projectionMatrix[16];
unsigned char* m_pixelsRGBA;
int m_rgbaBufferSizeInPixels;
float* m_depthBuffer;
int m_depthBufferSizeInPixels;
int* m_segmentationMaskBuffer;
int m_segmentationMaskBufferSizeInPixels;
int m_startPixelIndex;
int m_destinationWidth;
int m_destinationHeight;
int* m_numPixelsCopied;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied)
{
if (numPixelsCopied)
*numPixelsCopied = 0;
m_cs->lock();
for (int i=0;i<16;i++)
{
m_viewMatrix[i] = viewMatrix[i];
m_projectionMatrix[i] = projectionMatrix[i];
}
m_pixelsRGBA = pixelsRGBA;
m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
m_depthBuffer = depthBuffer;
m_depthBufferSizeInPixels = depthBufferSizeInPixels;
m_segmentationMaskBuffer = segmentationMaskBuffer;
m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
m_startPixelIndex = startPixelIndex;
m_destinationWidth = destinationWidth;
m_destinationHeight = destinationHeight;
m_numPixelsCopied = numPixelsCopied;
m_cs->setSharedParam(1,eRobotSimGUIHelperCopyCameraImageData);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
@@ -375,7 +417,7 @@ struct b3RobotSimAPI_InternalData
b3ThreadSupportInterface* m_threadSupport;
RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper;
bool m_connected;
@@ -494,6 +536,26 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperCopyCameraImageData:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix,
m_data->m_multiThreadedHelper->m_projectionMatrix,
m_data->m_multiThreadedHelper->m_pixelsRGBA,
m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
m_data->m_multiThreadedHelper->m_depthBuffer,
m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels,
m_data->m_multiThreadedHelper->m_segmentationMaskBuffer,
m_data->m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
m_data->m_multiThreadedHelper->m_startPixelIndex,
m_data->m_multiThreadedHelper->m_destinationWidth,
m_data->m_multiThreadedHelper->m_destinationHeight,
m_data->m_multiThreadedHelper->m_numPixelsCopied);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperIdle:
default:
{
@@ -560,6 +622,7 @@ void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
@@ -625,8 +688,10 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
{
b3LoadUrdfCommandSetUseFixedBase(command,true);
}
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
results.m_uniqueObjectIds.push_back(robotUniqueId);
@@ -638,6 +703,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
@@ -667,9 +733,9 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
{
m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);

3
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -23,6 +23,7 @@ struct b3RobotSimLoadFileArgs
b3Vector3 m_startPosition;
b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase;
bool m_useMultiBody;
int m_fileType;
@@ -61,7 +62,7 @@ struct b3JointMotorArgs
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.1),
m_kd(0.9),
m_maxTorqueValue(1000)
{
}

178
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2,6 +2,8 @@
#include "PhysicsClientSharedMemory.h"
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include <string.h>
#include "SharedMemoryCommands.h"
@@ -53,6 +55,27 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
return (b3SharedMemoryCommandHandle) command;
}
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY;
command->m_urdfArguments.m_useMultiBody = useMultiBody;
return 0;
}
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_SDF);
command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY;
command->m_sdfArguments.m_useMultiBody = useMultiBody;
return 0;
}
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
{
@@ -65,8 +88,6 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
return 0;
}
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -624,6 +645,7 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
return bodyId;
}
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
@@ -702,10 +724,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
}
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getJointInfo(bodyIndex, linkIndex,*info);
return cl->getJointInfo(bodyIndex, jointIndex, *info);
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
@@ -823,6 +845,97 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3Vector3 camUpVector;
b3Vector3 camForward;
b3Vector3 camPos;
b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]);
b3Vector3 eyePos = b3MakeVector3(0,0,0);
int forwardAxis(-1);
{
switch (upAxis)
{
case 1:
{
forwardAxis = 0;
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f,0.f,0.f);
} else
{
camForward.normalize();
}
b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward,rollRad);
camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,1,0));
//gLightPos = b3MakeVector3(-50.f,100,30);
break;
}
case 2:
{
forwardAxis = 1;
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f,0.f,0.f);
} else
{
camForward.normalize();
}
b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward,rollRad);
camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,0,1));
//gLightPos = b3MakeVector3(-50.f,30,100);
break;
}
default:
{
//b3Assert(0);
return;
}
};
}
b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion pitchRot(camUpVector,pitchRad);
b3Vector3 right = camUpVector.cross(camForward);
b3Quaternion yawRot(right,-yawRad);
eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos;
camPos = eyePos;
camPos += camTargetPos;
float camPosf[4] = {camPos[0],camPos[1],camPos[2],0};
float camPosTargetf[4] = {camTargetPos[0],camTargetPos[1],camTargetPos[2],0};
float camUpf[4] = {camUpVector[0],camUpVector[1],camUpVector[2],0};
b3RequestCameraImageSetViewMatrix(commandHandle,camPosf,camPosTargetf,camUpf);
}
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
@@ -1004,3 +1117,60 @@ void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUn
command->m_externalForceArguments.m_numForcesAndTorques++;
}
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex;
int numJoints = cl->getNumJoints(bodyIndex);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
return (b3SharedMemoryCommandHandle)command;
}
int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointForces)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
return false;
if (dofCount)
{
*dofCount = status->m_inverseDynamicsResultArgs.m_dofCount;
}
if (bodyUniqueId)
{
*bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId;
}
if (jointForces)
{
for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++)
{
jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i];
}
}
return true;
}

16
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -56,8 +56,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
@@ -71,6 +71,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
@@ -95,6 +96,17 @@ int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle,
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointForces);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);

218
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -44,7 +44,9 @@ protected:
int m_selectedBody;
int m_prevSelectedBody;
struct Common2dCanvasInterface* m_canvas;
int m_canvasIndex;
int m_canvasRGBIndex;
int m_canvasDepthIndex;
int m_canvasSegMaskIndex;
void createButton(const char* name, int id, bool isTrigger );
@@ -171,10 +173,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)
@@ -248,7 +250,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_LOAD_SDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf");
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
@@ -259,15 +261,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
//b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
@@ -413,6 +415,33 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
if (m_selectedBody >= 0)
{
btAlignedObjectArray<double> jointPositionsQ;
btAlignedObjectArray<double> jointVelocitiesQdot;
btAlignedObjectArray<double> jointAccelerations;
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
if (numJoints)
{
b3Printf("Compute inverse dynamics for joint accelerations:");
jointPositionsQ.resize(numJoints);
jointVelocitiesQdot.resize(numJoints);
jointAccelerations.resize(numJoints);
for (int i = 0; i < numJoints; i++)
{
jointAccelerations[i] = 100;
b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]);
}
b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle,
m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
}
break;
}
default:
{
b3Error("Unknown buttonId");
@@ -433,7 +462,11 @@ m_prevSelectedBody(-1),
m_numMotors(0),
m_options(options),
m_isOptionalServerConnected(false),
m_canvas(0)
m_canvas(0),
m_canvasRGBIndex(-1),
m_canvasDepthIndex(-1),
m_canvasSegMaskIndex(-1)
{
b3Printf("Started PhysicsClientExample\n");
}
@@ -452,9 +485,15 @@ PhysicsClientExample::~PhysicsClientExample()
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas && (m_canvasIndex>=0))
if (m_canvas)
{
m_canvas->destroyCanvas(m_canvasIndex);
if (m_canvasRGBIndex>=0)
m_canvas->destroyCanvas(m_canvasRGBIndex);
if (m_canvasDepthIndex>=0)
m_canvas->destroyCanvas(m_canvasDepthIndex);
if (m_canvasSegMaskIndex>=0)
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
b3Printf("~PhysicsClientExample\n");
}
@@ -490,6 +529,7 @@ void PhysicsClientExample::createButtons()
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
if (m_bodyUniqueIds.size())
{
@@ -586,8 +626,9 @@ void PhysicsClientExample::initPhysics()
if (m_canvas)
{
m_canvasIndex = m_canvas->createCanvas("Synthetic Camera",camVisualizerWidth, camVisualizerHeight);
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight);
for (int i=0;i<camVisualizerWidth;i++)
{
@@ -603,10 +644,16 @@ void PhysicsClientExample::initPhysics()
green=0;
blue=0;
}
m_canvas->setPixel(m_canvasIndex,i,j,red,green,blue,alpha);
m_canvas->setPixel(m_canvasRGBIndex,i,j,red,green,blue,alpha);
m_canvas->setPixel(m_canvasDepthIndex,i,j,red,green,blue,alpha);
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,red,green,blue,alpha);
}
}
m_canvas->refreshImageData(m_canvasIndex);
m_canvas->refreshImageData(m_canvasRGBIndex);
m_canvas->refreshImageData(m_canvasDepthIndex);
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
@@ -670,8 +717,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
if (m_canvas)
{
//compute depth image range
float minDepthValue = 1e20f;
float maxDepthValue = -1e20f;
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<camVisualizerHeight;j++)
{
int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
if (m_canvasDepthIndex >=0)
{
int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue>-1e20)
{
maxDepthValue = btMax(maxDepthValue,depthValue);
minDepthValue = btMin(minDepthValue,depthValue);
}
}
}
}
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<camVisualizerHeight;j++)
@@ -682,19 +757,110 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4; //RGBA
int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,i,j,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255); //alpha set to 255
if (m_canvasRGBIndex >=0)
{
int rgbPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasRGBIndex,i,j,
imageData.m_rgbColorData[rgbPixelIndex],
imageData.m_rgbColorData[rgbPixelIndex+1],
imageData.m_rgbColorData[rgbPixelIndex+2],
255); //alpha set to 255
}
if (m_canvasDepthIndex >=0)
{
int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue>-1e20)
{
int rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue)));
if (rgb<0 || rgb>255)
{
printf("rgb=%d\n",rgb);
}
m_canvas->setPixel(m_canvasDepthIndex,i,j,
rgb,
rgb,
255, 255); //alpha set to 255
} else
{
m_canvas->setPixel(m_canvasDepthIndex,i,j,
0,
0,
0, 255); //alpha set to 255
}
}
if (m_canvasSegMaskIndex>=0 && (0!=imageData.m_segmentationMaskValues))
{
int segmentationMaskPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex];
btVector4 palette[4] = {btVector4(32,255,32,255),
btVector4(32,32,255,255),
btVector4(255,255,32,255),
btVector4(32,255,255,255)};
if (segmentationMask>=0)
{
btVector4 rgb = palette[segmentationMask&3];
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
rgb.x(),
rgb.y(),
rgb.z(), 255); //alpha set to 255
} else
{
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
0,
0,
0, 255); //alpha set to 255
}
}
}
}
m_canvas->refreshImageData(m_canvasIndex);
if (m_canvasRGBIndex >=0)
m_canvas->refreshImageData(m_canvasRGBIndex);
if (m_canvasDepthIndex >=0)
m_canvas->refreshImageData(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >=0)
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
// b3Printf(msg);
}
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
{
int bodyUniqueId;
int dofCount;
b3GetStatusInverseDynamicsJointForces(status,
&bodyUniqueId,
&dofCount,
0);
btAlignedObjectArray<double> jointForces;
if (dofCount)
{
jointForces.resize(dofCount);
b3GetStatusInverseDynamicsJointForces(status,
0,
0,
&jointForces[0]);
for (int i = 0; i < dofCount; i++)
{
b3Printf("jointForces[%d]=%f", i, jointForces[i]);
}
}
}
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_FAILED)
{
b3Warning("Inverse Dynamics computations failed");
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Warning("Camera image FAILED\n");

21
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -36,6 +36,7 @@ struct PhysicsClientSharedMemoryInternalData {
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -514,6 +515,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
@@ -522,12 +524,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
@@ -542,7 +550,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Camera image FAILED\n");
break;
}
case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
{
break;
}
case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED:
{
b3Warning("Inverse Dynamics computations failed");
break;
}
default: {
b3Error("Unknown server status\n");
btAssert(0);
@@ -609,7 +625,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
{
@@ -698,6 +714,7 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {

13
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -37,6 +37,8 @@ struct PhysicsDirectInternalData
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMask;
PhysicsServerCommandProcessor* m_commandProcessor;
@@ -205,6 +207,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedSegmentationMask.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
@@ -212,6 +215,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
(unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
@@ -219,13 +223,17 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
= rgbaPixelsReceived[i];
}
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
{
@@ -241,7 +249,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
}
}
} while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
} while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied);
return m_data->m_hasStatus;
@@ -458,5 +466,6 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
}
}

147
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4,13 +4,15 @@
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
#include "TinyRendererVisualShapeConverter.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "btBulletDynamicsCommon.h"
@@ -383,6 +385,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_physicsDeltaTime;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
@@ -548,8 +551,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
{
for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
{
btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.getAtIndex(i);
if (treePtrPtr)
{
btInverseDynamics::MultiBodyTree* tree = *treePtrPtr;
delete tree;
}
}
m_data->m_inverseDynamicsBodies.clear();
}
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
{
deleteCachedInverseDynamicsBodies();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{
@@ -691,7 +711,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
}
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes)
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody)
{
btAssert(m_data->m_dynamicsWorld);
if (!m_data->m_dynamicsWorld)
@@ -731,7 +751,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
u2b.setBodyUniqueId(bodyUniqueId);
{
btScalar mass = 0;
bodyHandle->m_rootLocalInertialFrame.setIdentity();
@@ -748,7 +768,6 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
MyMultiBodyCreator creation(m_data->m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);
bool useMultiBody = true;
ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true);
@@ -825,6 +844,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
{
@@ -856,6 +876,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
}
btMultiBody* mb = creation.getBulletMultiBody();
if (useMultiBody)
{
@@ -923,7 +944,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
} else
{
btAssert(0);
return true;
}
@@ -1145,15 +1165,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (numRemainingPixels>0)
{
int maxNumPixels = bufferSizeInBytes/8-1;
int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask
int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1;
unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels);
float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4);
int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8);
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
{
m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,width,height,&numPixelsCopied);
m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,width,height,&numPixelsCopied);
} else
{
@@ -1174,7 +1200,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied);
m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,&width,&height,&numPixelsCopied);
}
//each pixel takes 4 RGBA values and 1 float = 8 bytes
@@ -1214,18 +1243,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName);
}
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? sdfArgs.m_useMultiBody : true;
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes);
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++)
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody);
if (completedOk)
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
}
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
for (int i=0;i<maxBodies;i++)
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
}
serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
} else
{
serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
}
hasStatus = true;
break;
}
@@ -1831,11 +1866,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
mb->setBaseOmega(btVector3(0,0,0));
mb->setWorldToBaseRot(btQuaternion(
clientCmd.m_initPoseArgs.m_initialStateQ[3],
btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]));
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
mb->setWorldToBaseRot(invOrn.inverse());
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
{
@@ -1863,8 +1899,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
//clean up all data
deleteCachedInverseDynamicsBodies();
if (m_data && m_data->m_guiHelper)
{
@@ -2060,6 +2095,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
if (treePtrPtr)
{
tree = *treePtrPtr;
}
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
{
b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
}
}
if (tree)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++)
{
q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
for (int i = 0; i < num_dofs; i++)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs];
}
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
else
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
}
}
else
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
hasStatus = true;
break;
}
case CMD_APPLY_EXTERNAL_FORCE:
{
if (m_data->m_verboseOutput)

4
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -21,7 +21,7 @@ protected:
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
@@ -29,6 +29,7 @@ protected:
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
void deleteCachedInverseDynamicsBodies();
public:
PhysicsServerCommandProcessor();
@@ -39,6 +40,7 @@ public:
virtual void createEmptyDynamicsWorld();
virtual void deleteDynamicsWorld();
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes );
virtual void renderScene();

81
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -43,6 +43,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperCreateCollisionShapeGraphicsObject,
eGUIHelperCreateCollisionObjectGraphicsObject,
eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData,
};
#include <stdio.h>
@@ -106,6 +107,7 @@ struct MotionThreadLocalStorage
};
int skip = 0;
int skip1 = 0;
void MotionThreadFunc(void* userPtr,void* lsMemory)
{
@@ -133,8 +135,15 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
if (deltaTimeInSeconds<(1./5000.))
{
skip++;
skip1++;
if (0==(skip1&0x3))
{
b3Clock::usleep(250);
}
} else
{
skip1=0;
//process special controller commands, such as
//VR controller button press/release and controller motion
@@ -185,8 +194,9 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
}
printf("finished, #skip = %d\n",skip);
printf("finished, #skip = %d, skip1 = %d\n",skip,skip1);
skip=0;
skip1=0;
//do nothing
}
@@ -390,14 +400,54 @@ public:
}
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
{
m_childGuiHelper->resetCamera(camDist,pitch,yaw,camPosX,camPosY,camPosZ);
}
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied)
float m_viewMatrix[16];
float m_projectionMatrix[16];
unsigned char* m_pixelsRGBA;
int m_rgbaBufferSizeInPixels;
float* m_depthBuffer;
int m_depthBufferSizeInPixels;
int* m_segmentationMaskBuffer;
int m_segmentationMaskBufferSizeInPixels;
int m_startPixelIndex;
int m_destinationWidth;
int m_destinationHeight;
int* m_numPixelsCopied;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied)
{
if (numPixelsCopied)
*numPixelsCopied = 0;
m_cs->lock();
for (int i=0;i<16;i++)
{
m_viewMatrix[i] = viewMatrix[i];
m_projectionMatrix[i] = projectionMatrix[i];
}
m_pixelsRGBA = pixelsRGBA;
m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
m_depthBuffer = depthBuffer;
m_depthBufferSizeInPixels = depthBufferSizeInPixels;
m_segmentationMaskBuffer = segmentationMaskBuffer;
m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
m_startPixelIndex = startPixelIndex;
m_destinationWidth = destinationWidth;
m_destinationHeight = destinationHeight;
m_numPixelsCopied = numPixelsCopied;
m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
{
}
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
@@ -597,8 +647,10 @@ void PhysicsServerExample::initPhysics()
int index = 0;
m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w);
while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized)
{
b3Clock::usleep(1000);
}
}
@@ -628,6 +680,7 @@ void PhysicsServerExample::exitPhysics()
} else
{
b3Clock::usleep(1000);
}
};
@@ -727,6 +780,26 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperCopyCameraImageData:
{
m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix,
m_multiThreadedHelper->m_projectionMatrix,
m_multiThreadedHelper->m_pixelsRGBA,
m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
m_multiThreadedHelper->m_depthBuffer,
m_multiThreadedHelper->m_depthBufferSizeInPixels,
m_multiThreadedHelper->m_segmentationMaskBuffer,
m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
m_multiThreadedHelper->m_startPixelIndex,
m_multiThreadedHelper->m_destinationWidth,
m_multiThreadedHelper->m_destinationHeight,
m_multiThreadedHelper->m_numPixelsCopied);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperIdle:
default:
{

20
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -64,6 +64,7 @@ enum EnumSdfArgsUpdateFlags
struct SdfArgs
{
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
int m_useMultiBody;
};
enum EnumUrdfArgsUpdateFlags
@@ -348,6 +349,23 @@ enum EnumSdfRequestInfoFlags
//SDF_REQUEST_INFO_CAMERA=2,
};
struct CalculateInverseDynamicsArgs
{
int m_bodyUniqueId;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
};
struct CalculateInverseDynamicsResultArgs
{
int m_bodyUniqueId;
int m_dofCount;
double m_jointForces[MAX_DEGREE_OF_FREEDOM];
};
struct SharedMemoryCommand
{
int m_type;
@@ -374,6 +392,7 @@ struct SharedMemoryCommand
struct RequestPixelDataArgs m_requestPixelDataArguments;
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
};
};
@@ -397,6 +416,7 @@ struct SharedMemoryStatus
struct SendDebugLinesArgs m_sendDebugLinesArgs;
struct SendPixelDataArgs m_sendPixelDataArguments;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
};
};

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -27,6 +27,7 @@ enum EnumSharedMemoryClientCommand
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_MAX_CLIENT_COMMANDS
};
@@ -59,6 +60,8 @@ enum EnumSharedMemoryServerStatus
CMD_BODY_INFO_COMPLETED,
CMD_BODY_INFO_FAILED,
CMD_INVALID_STATUS,
CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
CMD_MAX_SERVER_COMMANDS
};
@@ -120,6 +123,7 @@ struct b3CameraImageData
int m_pixelHeight;
const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
};
///b3LinkState provides extra information such as the Cartesian world coordinates

23
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -66,6 +66,8 @@ struct TinyRendererVisualShapeConverterInternalData
int m_swHeight;
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> m_depthBuffer;
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
SimpleCamera m_camera;
TinyRendererVisualShapeConverterInternalData()
@@ -75,6 +77,7 @@ struct TinyRendererVisualShapeConverterInternalData
m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB)
{
m_depthBuffer.resize(m_swWidth*m_swHeight);
m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1);
}
};
@@ -440,7 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj)
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex)
{
@@ -487,7 +490,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex);
unsigned char* textureImage=0;
int textureWidth=0;
int textureHeight=0;
@@ -535,6 +538,7 @@ void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor)
{
m_data->m_rgbColorBuffer.set(x,y,clearColor);
m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f;
m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1;
}
}
@@ -624,7 +628,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
// printf("flipped!\n");
m_data->m_rgbColorBuffer.flip_vertically();
//flip z-buffer
//flip z-buffer and segmentation Buffer
{
int half = m_data->m_swHeight>>1;
for (int j=0; j<half; j++)
@@ -634,6 +638,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
for (int i=0;i<m_data->m_swWidth;i++)
{
btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]);
btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]);
}
}
}
@@ -652,11 +657,16 @@ void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
m_data->m_swHeight = height;
m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_segmentationMaskBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
}
void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,
int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
{
int w = m_data->m_rgbColorBuffer.get_width();
int h = m_data->m_rgbColorBuffer.get_height();
@@ -682,6 +692,11 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
{
depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
}
if (segmentationMaskBuffer)
{
segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
}
if (pixelsRGBA)
{
pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];

4
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@@ -13,7 +13,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape);
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
void setUpAxis(int axis);
@@ -26,7 +26,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
void getWidthAndHeight(int& width, int& height);
void setWidthAndHeight(int width, int height);
void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
void render();
void render(const float viewMat[16], const float projMat[16]);

6
examples/SharedMemory/premake4.lua
View File

@@ -10,7 +10,7 @@ end
includedirs {".","../../src", "../ThirdPartyLibs",}
links {
"Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath"
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath"
}
language "C++"
@@ -137,7 +137,7 @@ defines {"B3_USE_STANDALONE_EXAMPLE"}
includedirs {"../../src"}
links {
"BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
"BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
}
initOpenGL()
initGlew()
@@ -211,7 +211,7 @@ if os.is("Windows") then
}
links {
"Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
}

7
examples/StandaloneMain/main_tinyrenderer_single_example.cpp
View File

@@ -348,7 +348,12 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
}
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied)
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
{
if (numPixelsCopied)
*numPixelsCopied = 0;

611
examples/ThirdPartyLibs/BussIK/Jacobian.cpp Normal file
View File

@@ -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 <stdlib.h>
#include <math.h>
#include <assert.h>
#include <iostream>
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 && i<nEffector && 0<=j && j<nJoint );
if ( m->IsFrozen() ) {
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<MatrixRmn&>(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; i<diagLength; i++ ) {
double dotProdCol = U.DotProductColumn( dS, i ); // Dot product with i-th column of U
double alpha = *(wPtr++);
if ( fabs(alpha)>pseudoInverseThreshold ) {
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; i<diagLength; i++ ) {
double dotProdCol = U.DotProductColumn( dS, i ); // Dot product with i-th column of U
double alpha = *(wPtr++);
alpha = alpha/(Square(alpha)+DampingLambdaSq);
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>MaxAngleDLS ) {
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; i<nRows; i++ ) {
double wiInv = w[i];
if ( NearZero(wiInv,1.0e-10) ) {
continue;
}
wiInv = 1.0/wiInv;
double N = 0.0; // N is the quasi-1-norm of the i-th column of U
double alpha = 0.0; // alpha is the dot product of dT and the i-th column of U
const double *dTx = dT1.GetPtr();
const double *ux = U.GetColumnPtr(i);
long j;
for ( j=numEndEffectors; j>0; 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 ( N<M ) {
gamma *= N/M; // Scale back maximum permissable joint angle
}
// Calculate the dTheta from pure pseudoinverse considerations
double scale = alpha*wiInv; // This times i-th column of V is the psuedoinverse response
dPreTheta.LoadScaled( V.GetColumnPtr(i), scale );
// Now rescale the dTheta values.
double max = dPreTheta.MaxAbs();
double rescale = (gamma)/(gamma+max);
dTheta.AddScaled(dPreTheta,rescale);
/*if ( gamma<max) {
dTheta.AddScaled( dPreTheta, gamma/max );
}
else {
dTheta += dPreTheta;
}*/
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleSDLS ) {
dTheta *= MaxAngleSDLS/(MaxAngleSDLS+maxChange);
//dTheta *= MaxAngleSDLS/maxChange;
}
}
void Jacobian::CalcdTClampedFromdS()
{
long len = dS.GetLength();
long j = 0;
for ( long i=0; i<len; i+=3, j++ ) {
double normSq = Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]);
if ( normSq>Square(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; i<len; i++ ) {
double v1 = e1[i];
double v2 = e2[i];
if ( v1<v2 ) {
ret1 += v1/v2;
ret2 += 1.0;
}
else if ( v1 != 0.0 ) {
ret1 += 1.0;
ret2 += v2/v1;
}
else {
ret1 += 0.0;
ret2 += 0.0;
}
}
*weightedDist1 = ret1;
*weightedDist2 = ret2;
}
void Jacobian::CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies )
{
const VectorRn& e1 = j1.errorArray;
const VectorRn& e2 = j2.errorArray;
int b1=0, b2=0, tie=0;
int len = e1.GetLength();
for ( long i=0; i<len; i++ ) {
double v1 = e1[i];
double v2 = e2[i];
if ( v1<v2 ) {
b1++;
}
else if ( v2<v1 ) {
b2++;
}
else {
tie++;
}
}
*numBetter1 = b1;
*numBetter2 = b2;
*numTies = tie;
}
/* THIS VERSION IS NOT AS GOOD. DO NOT USE!
void Jacobian::CalcDeltaThetasSDLSrev2()
{
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);
}
// Loop over each singular vector
for ( i=0; i<nRows; i++ ) {
double wiInv = w[i];
if ( NearZero(wiInv,1.0e-10) ) {
continue;
}
double N = 0.0; // N is the quasi-1-norm of the i-th column of U
double alpha = 0.0; // alpha is the dot product of dS and the i-th column of U
const double *dSx = dS.GetPtr();
const double *ux = U.GetColumnPtr(i);
long j;
for ( j=numEndEffectors; j>0; 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<P ) {
lambda -= N/P; // Scale back maximum permissable joint angle
}
lambda *= lambda;
lambda *= DampingLambdaSDLS;
// Calculate the dTheta from pure pseudoinverse considerations
double scale = alpha*wiInv/(Square(wiInv)+Square(lambda)); // This times i-th column of V is the SDLS response
MatrixRmn::AddArrayScale(nCols, V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, scale );
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleSDLS ) {
dTheta *= MaxAngleSDLS/maxChange;
}
} */

131
examples/ThirdPartyLibs/BussIK/Jacobian.h Normal file
View File

@@ -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

101
examples/ThirdPartyLibs/BussIK/LinearR2.cpp Normal file
View File

@@ -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 <assert.h>
// ******************************************************
// * 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 << ">");
}

981
examples/ThirdPartyLibs/BussIK/LinearR2.h Normal file
View File

@@ -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 <math.h>
#include <assert.h>
#include <iostream>
#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

822
examples/ThirdPartyLibs/BussIK/LinearR3.cpp Normal file
View File

@@ -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);
}

2027
examples/ThirdPartyLibs/BussIK/LinearR3.h Normal file
View File

File diff suppressed because it is too large Load Diff

467
examples/ThirdPartyLibs/BussIK/LinearR4.cpp Normal file
View File

@@ -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 <assert.h>
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<rotmat.Column1().Norm()
&& rotmat.Column1().Norm()<1.0001 && 0.9999<rotmat.Column1().Norm()
&& (rotmat.Column1()^rotmat.Column2()) < 0.001
&& (rotmat.Column1()^rotmat.Column2()) > -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 << ">");
}

1102
examples/ThirdPartyLibs/BussIK/LinearR4.h Normal file
View File

File diff suppressed because it is too large Load Diff

384
examples/ThirdPartyLibs/BussIK/MathMisc.h Normal file
View File

@@ -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 <math.h>
//
// 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<class T> inline T Min ( T x, T y )
{
return (x<y ? x : y);
}
template<class T> inline T Max ( T x, T y )
{
return (y<x ? x : y);
}
template<class T> inline T ClampRange ( T x, T min, T max)
{
if ( x<min ) {
return min;
}
if ( x>max ) {
return max;
}
return x;
}
template<class T> inline bool ClampRange ( T *x, T min, T max)
{
if ( (*x)<min ) {
(*x) = min;
return false;
}
else if ( (*x)>max ) {
(*x) = max;
return false;
}
else {
return true;
}
}
template<class T> inline bool ClampMin ( T *x, T min)
{
if ( (*x)<min ) {
(*x) = min;
return false;
}
return true;
}
template<class T> inline bool ClampMax ( T *x, T max)
{
if ( (*x)>max ) {
(*x) = max;
return false;
}
return true;
}
template<class T> inline T& UpdateMin ( const T& x, T& y )
{
if ( x<y ) {
y = x;
}
return y;
}
template<class T> inline T& UpdateMax ( const T& x, T& y )
{
if ( x>y ) {
y = x;
}
return y;
}
template<class T> inline bool SameSignNonzero( T x, T y )
{
if ( x<0 ) {
return (y<0);
}
else if ( 0<x ) {
return (0<y);
}
else {
return false;
}
}
inline double Mag ( double x ) {
return fabs(x);
}
inline double Dist ( double x, double y ) {
return fabs(x-y);
}
template <class T>
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<x && x<1.0e-15 );
}
}
inline bool GreaterOrEqualFuzzy ( double x, double y )
{
return LessOrEqualFuzzy( y, x );
}
inline bool UpdateMaxAbs( double *maxabs, double updateval )
{
if ( updateval > *maxabs ) {
*maxabs = updateval;
return true;
}
else if ( -updateval > *maxabs ) {
*maxabs = -updateval;
return true;
}
else {
return false;
}
}
// **********************************************************
// Combinations and averages. *
// **********************************************************
template <class T>
void averageOf ( const T& a, const T &b, T&c ) {
c = a;
c += b;
c *= 0.5;
}
template <class T>
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 <class T>
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<class T> inline T Square ( T x )
{
return (x*x);
}
// Cube(x) returns x*x*x, of course!
template<class T> 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<class T> inline int Sign( T x)
{
if ( x<0 ) {
return -1;
}
else if ( x==0 ) {
return 0;
}
else {
return 1;
}
}
#endif // #ifndef MATH_MISC_H

984
examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp Normal file
View File

@@ -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<NumRows && startCol>=0 && startCol<NumCols );
assert( startRow+(length-1)*deltaRow>=0 && startRow+(length-1)*deltaRow<NumRows );
assert( startCol+(length-1)*deltaCol>=0 && startCol+(length-1)*deltaCol<NumCols );
double *to = x + startRow + NumRows*startCol;
double *from = d.x;
long stride = deltaRow + NumRows*deltaCol;
for ( ; length>0; 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 && idx<NumCols );
double *colA = x + idx*NumRows;
double *colB = colA + NumRows;
for ( long i = NumRows; i>0; 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 && idx1<NumCols && 0<=idx2 && idx2<NumCols );
double *colA = x + idx1*NumRows;
double *colB = x + idx2*NumRows;
for ( long i = NumRows; i>0; 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<lastBidiagIdx-1) ? Square(superDiag[lastBidiagIdx-2]): 0.0;
double BSq = Square(w[lastBidiagIdx-1]);
A += BSq; // The "A" entry of M^T * M
double C = Square(superDiag[lastBidiagIdx-1]);
BSq *= C; // The squared "B" entry
C += Square(w[lastBidiagIdx]); // The "C" entry
double lambda; // lambda will hold the estimated eigenvalue
lambda = sqrt( Square((A-C)*0.5) + BSq ); // Use the lambda value that is closest to C.
if ( A > 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<lastBidiagIdx-1; i++ ) {
// Push non-zero from M[i+1,i] to M[i,i+2]
CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr+1, sdPtr+1 );
U.PostApplyGivens( c, -s, i );
// Push non-zero from M[i,i+2] to M[1+2,i+1]
CalcGivensValues( *sdPtr, extraOffDiag, &c, &s );
ApplyGivensCBTD( c, s, sdPtr, wPtr+1, &extraOffDiag, extraOffDiag, sdPtr+1, wPtr+2 );
V.PostApplyGivens( c, -s, i+1 );
wPtr++;
sdPtr++;
}
// Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
ApplyGivensCBTD( c, s, wPtr, &extraOffDiag, sdPtr, wPtr+1 );
U.PostApplyGivens( c, -s, i );
// DEBUG
// DebugCalcBidiagCheck( V, w, superDiag, U );
}
}
// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry on the same row.
// We use Givens rotations to "chase" the non-zero entry across the row; when it reaches the last
// column, it is finally zeroed away.
// wPtr points to the zero entry on the diagonal. sdPtr points to the non-zero superdiagonal entry on the same row.
void MatrixRmn::ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx, MatrixRmn& U, double *wPtr, double *sdPtr, double eps )
{
double curSd = *sdPtr; // Value being chased across the row
*sdPtr = 0.0;
long i=firstBidiagIdx+1;
while (true) {
// Rotate row i and row firstBidiagIdx (Givens rotation)
double c, s;
CalcGivensValues( *(++wPtr), curSd, &c, &s );
U.PostApplyGivens( c, -s, i, firstBidiagIdx );
*wPtr = c*(*wPtr) - s*curSd;
if ( i==lastBidiagIdx ) {
break;
}
curSd = s*(*(++sdPtr)); // New value pops up one column over to the right
*sdPtr = c*(*sdPtr);
i++;
}
}
// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry in the same column.
// We use Givens rotations to "chase" the non-zero entry up the column; when it reaches the last
// column, it is finally zeroed away.
// wPtr points to the zero entry on the diagonal. sdPtr points to the non-zero superdiagonal entry in the same column.
void MatrixRmn::ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps )
{
double curSd = *sdPtr; // Value being chased up the column
*sdPtr = 0.0;
long i = endIdx-1;
while ( true ) {
double c, s;
CalcGivensValues( *(--wPtr), curSd, &c, &s );
V.PostApplyGivens( c, -s, i, endIdx );
*wPtr = c*(*wPtr) - s*curSd;
if ( i==0 ) {
break;
}
curSd = s*(*(--sdPtr)); // New value pops up one row above
if ( NearZero( curSd, eps ) ) {
break;
}
*sdPtr = c*(*sdPtr);
i--;
}
}
// Matrix A is ( ( a c ) ( b d ) ), i.e., given in column order.
// Mult's G[c,s] times A, replaces A.
void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d )
{
double temp = *a;
*a = cosine*(*a) - sine*(*b);
*b = sine*temp + cosine*(*b);
temp = *c;
*c = cosine*(*c) - sine*(*d);
*d = sine*temp + cosine*(*d);
}
// Now matrix A given in row order, A = ( ( a b c ) ( d e f ) ).
// Return G[c,s] * A, replace A. d becomes zero, no need to return.
// Also, it is certain the old *c value is taken to be zero!
void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c,
double d, double *e, double *f )
{
*a = cosine*(*a) - sine*d;
double temp = *b;
*b = cosine*(*b) - sine*(*e);
*e = sine*temp + cosine*(*e);
*c = -sine*(*f);
*f = cosine*(*f);
}
// Helper routine for SVD conversion from bidiagonal to diagonal
bool MatrixRmn::UpdateBidiagIndices( long *firstBidiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps )
{
long lastIdx = *lastBidiagIdx;
double* sdPtr = superDiag.GetPtr( lastIdx-1 ); // Entry above the last diagonal entry
while ( NearZero(*sdPtr, eps) ) {
*(sdPtr--) = 0.0;
lastIdx--;
if ( lastIdx == 0 ) {
return false;
}
}
*lastBidiagIdx = lastIdx;
long firstIdx = lastIdx-1;
double* wPtr = w.GetPtr( firstIdx );
while ( firstIdx > 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;
}

402
examples/ThirdPartyLibs/BussIK/MatrixRmn.h Normal file
View File

@@ -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 <math.h>
#include <assert.h>
#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 ( i<NumRows && j<NumCols );
return *(x+j*NumRows+i);
}
// Return a VectorR3 out of a column. Starts at row 3*i, in column j.
inline void MatrixRmn::GetTriple( long i, long j, VectorR3 *retValue ) const
{
long ii = 3*i;
assert ( 0<=i && ii+2<NumRows && 0<=j && j<NumCols );
retValue->Load( 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 && i<NumRows && 0<=j &&j<NumCols );
return (x+j*NumRows+i);
}
// Get a pointer to the (i,j) entry.
// The entries are in column order.
// This version gives pointer to writable data
inline double* MatrixRmn::GetPtr( long i, long j )
{
assert ( i<NumRows && j<NumCols );
return (x+j*NumRows+i);
}
// Get a pointer to the j-th column.
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetColumnPtr( long j ) const
{
assert ( 0<=j && j<NumCols );
return (x+j*NumRows);
}
// Get a pointer to the j-th column.
// This version gives pointer to writable data
inline double* MatrixRmn::GetColumnPtr( long j )
{
assert ( 0<=j && j<NumCols );
return (x+j*NumRows);
}
/// Get a pointer to the i-th row
// The entries are in column order.
// This version gives read-only pointer
inline const double* MatrixRmn::GetRowPtr( long i ) const
{
assert ( 0<=i && i<NumRows );
return (x+i);
}
// Get a pointer to the i-th row
// This version gives pointer to writable data
inline double* MatrixRmn::GetRowPtr( long i )
{
assert ( 0<=i && i<NumRows );
return (x+i);
}
// Set the (i,j) entry of the matrix
inline void MatrixRmn::Set( long i, long j, double val )
{
assert( i<NumRows && j<NumCols );
*(x+j*NumRows+i) = val;
}
// Set the i-th triple in the j-th column to u's three values
inline void MatrixRmn::SetTriple( long i, long j, const VectorR3& u )
{
long ii = 3*i;
assert ( 0<=i && ii+2<NumRows && 0<=j && j<NumCols );
u.Dump( x+j*NumRows + ii );
}
// Set to be equal to the identity matrix
inline void MatrixRmn::SetIdentity()
{
assert ( NumRows==NumCols );
SetZero();
SetDiagonalEntries(1.0);
}
inline MatrixRmn& MatrixRmn::operator*=( double mult )
{
double* aPtr = x;
for ( long i=NumRows*NumCols; i>0; 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

153
examples/ThirdPartyLibs/BussIK/Misc.cpp Normal file
View File

@@ -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 <math.h>
#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 );
}

90
examples/ThirdPartyLibs/BussIK/Node.cpp Normal file
View File

@@ -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 <math.h>
#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;
}

101
examples/ThirdPartyLibs/BussIK/Node.h Normal file
View File

@@ -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

298
examples/ThirdPartyLibs/BussIK/Spherical.h Normal file
View File

@@ -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

217
examples/ThirdPartyLibs/BussIK/Tree.cpp Normal file
View File

@@ -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 <iostream>
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);
}

92
examples/ThirdPartyLibs/BussIK/Tree.h Normal file
View File

@@ -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

46
examples/ThirdPartyLibs/BussIK/VectorRn.cpp Normal file
View File

@@ -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;
}

238
examples/ThirdPartyLibs/BussIK/VectorRn.h Normal file
View File

@@ -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 <math.h>
#include <assert.h>
#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 && i<length ); return *(x+i); }
double& operator[]( long i ) { assert ( 0<=i && i<length ); return *(x+i); }
double Get( long i ) const { assert ( 0<=i && i<length ); return *(x+i); }
// Use GetPtr to get pointer into the array (efficient)
// Is friendly in that anyone can change the array contents (be careful!)
const double* GetPtr( long i ) const { assert(0<=i && i<length); return (x+i); }
double* GetPtr( long i ) { assert(0<=i && i<length); return (x+i); }
const double* GetPtr() const { return x; }
double* GetPtr() { return x; }
void Set( long i, double val ) { assert(0<=i && i<length), *(x+i) = val; }
void SetTriple( long i, const VectorR3& u );
VectorRn& operator+=( const VectorRn& src );
VectorRn& operator-=( const VectorRn& src );
void AddScaled (const VectorRn& src, double scaleFactor );
VectorRn& operator*=( double f );
double NormSq() const;
double Norm() const { return sqrt(NormSq()); }
double MaxAbs() const;
private:
long length; // Logical or actual length
long AllocLength; // Allocated length
double *x; // Array of vector entries
static VectorRn WorkVector; // Serves as a temporary vector
static VectorRn& GetWorkVector() { return WorkVector; }
static VectorRn& GetWorkVector( long len ) { WorkVector.SetLength(len); return WorkVector; }
};
inline VectorRn::VectorRn()
{
length = 0;
AllocLength = 0;
x = 0;
}
inline VectorRn::VectorRn( long initLength )
{
length = 0;
AllocLength = 0;
x = 0;
SetLength( initLength );
}
inline VectorRn::~VectorRn()
{
delete x;
}
// Resize.
// If the array is shortened, the information about the allocated length is lost.
inline void VectorRn::SetLength( long newLength )
{
assert ( newLength>0 );
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

2
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -366,6 +366,8 @@ std::string LoadMtl (
continue;
}
linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
// Skip leading space.
const char* token = linebuf.c_str();
token += strspn(token, " \t");

28
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -87,13 +87,34 @@ struct Shader : public IShader {
}
};
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_segmentationMaskBufferPtr(0),
m_model(0),
m_userData(0),
m_userIndex(-1)
m_userIndex(-1),
m_objectIndex(-1)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_segmentationMaskBufferPtr(segmentationMaskBuffer),
m_model(0),
m_userData(0),
m_userIndex(-1),
m_objectIndex(objectIndex)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
@@ -247,6 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
int* segmentationMaskBufferPtr = &renderData.m_segmentationMaskBufferPtr->at(0);
TGAImage& frame = renderData.m_rgbColorBuffer;
@@ -264,7 +286,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
for (int j=0; j<3; j++) {
shader.vertex(i, j);
}
triangle(shader.varying_tri, shader, frame, &zbuffer[0], renderData.m_viewportMatrix);
triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}

7
examples/TinyRenderer/TinyRenderer.h
View File

@@ -27,9 +27,11 @@ struct TinyRenderObjectData
//Output
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;//required, hence a reference
b3AlignedObjectArray<int>* m_segmentationMaskBufferPtr;//optional, hence a pointer
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);
@@ -41,6 +43,7 @@ struct TinyRenderObjectData
void* m_userData;
int m_userIndex;
int m_objectIndex;
};

11
examples/TinyRenderer/our_gl.cpp
View File

@@ -59,7 +59,12 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) {
return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator
}
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) {
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix, int objectIndex)
{
triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0);
}
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
@@ -100,6 +105,10 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
bool discard = shader.fragment(bc_clip, color);
if (!discard) {
zbuffer[P.x+P.y*image.get_width()] = frag_depth;
if (segmentationMaskBuffer)
{
segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
}
image.set(P.x, P.y, color);
}
}

2
examples/TinyRenderer/our_gl.h
View File

@@ -16,7 +16,7 @@ struct IShader {
virtual bool fragment(Vec3f bar, TGAColor &color) = 0;
};
//void triangle(Vec4f *pts, IShader &shader, TGAImage &image, float *zbuffer);
void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix);
void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex);
#endif //__OUR_GL_H__

6
examples/TinyRenderer/tgaimage.h
View File

@@ -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) {

19
examples/Utils/b3Clock.cpp
View File

@@ -36,6 +36,7 @@ const T& b3ClockMin(const T& a, const T& b)
#else //_WIN32
#include <sys/time.h>
#include <unistd.h>
#endif //_WIN32
@@ -227,3 +228,21 @@ double b3Clock::getTimeInSeconds()
return double(getTimeMicroseconds()/1.e6);
}
void b3Clock::usleep(int microSeconds)
{
#ifdef _WIN32
int millis = microSeconds/1000;
if (millis < 1)
{
millis = 1;
}
Sleep(millis);
#else
usleep(microSeconds);
//struct timeval tv;
//tv.tv_sec = microSeconds/1000000L;
//tv.tv_usec = microSeconds%1000000L;
//return select(0, 0, 0, 0, &tv);
#endif
}

4
examples/Utils/b3Clock.h
View File

@@ -28,6 +28,10 @@ public:
/// the Clock was created.
double getTimeInSeconds();
///Sleep for 'microSeconds', to yield to other threads and not waste 100% CPU cycles.
///Note that some operating systems may sleep a longer time.
static void usleep(int microSeconds);
private:
struct b3ClockData* m_data;
};

109
examples/pybullet/CMakeLists.txt
View File

@@ -8,61 +8,60 @@ INCLUDE_DIRECTORIES(
SET(pybullet_SRCS
pybullet.c
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/SharedMemory/InProcessMemory.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h
../../examples/SharedMemory/PhysicsDirect.cpp
../../examples/SharedMemory/PhysicsDirect.h
../../examples/SharedMemory/PhysicsDirectC_API.cpp
../../examples/SharedMemory/PhysicsDirectC_API.h
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.h
../../examples/SharedMemory/PhysicsClientC_API.cpp
../../examples/SharedMemory/PhysicsClientC_API.h
../../examples/SharedMemory/Win32SharedMemory.cpp
../../examples/SharedMemory/Win32SharedMemory.h
../../examples/SharedMemory/PosixSharedMemory.cpp
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/SharedMemory/InProcessMemory.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h
../../examples/SharedMemory/PhysicsDirect.cpp
../../examples/SharedMemory/PhysicsDirect.h
../../examples/SharedMemory/PhysicsDirectC_API.cpp
../../examples/SharedMemory/PhysicsDirectC_API.h
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.h
../../examples/SharedMemory/PhysicsClientC_API.cpp
../../examples/SharedMemory/PhysicsClientC_API.h
../../examples/SharedMemory/Win32SharedMemory.cpp
../../examples/SharedMemory/Win32SharedMemory.h
../../examples/SharedMemory/PosixSharedMemory.cpp
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
)
IF(WIN32)

69
examples/pybullet/premake4.lua
View File

@@ -1,18 +1,16 @@
project ("pybullet")
language "C++"
kind "SharedLib"
targetsuffix ("")
targetprefix ("")
targetextension (".so")
includedirs {"../../src", "../../examples",
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
initOpenGL()
initGlew()
@@ -20,10 +18,8 @@ project ("pybullet")
".",
"../../src",
"../ThirdPartyLibs",
"/usr/include/python2.7",
}
if os.is("MacOSX") then
links{"Cocoa.framework","Python"}
end
@@ -40,8 +36,69 @@ project ("pybullet")
files {
"pybullet.c",
"../../examples/ExampleBrowser/ExampleEntries.cpp",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
"../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
"../../examples/OpenGLWindow/SimpleCamera.cpp",
"../../examples/OpenGLWindow/SimpleCamera.h",
"../../examples/TinyRenderer/geometry.cpp",
"../../examples/TinyRenderer/model.cpp",
"../../examples/TinyRenderer/tgaimage.cpp",
"../../examples/TinyRenderer/our_gl.cpp",
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
"../../examples/SharedMemory/PhysicsDirect.cpp",
"../../examples/SharedMemory/PhysicsDirect.h",
"../../examples/SharedMemory/PhysicsDirectC_API.cpp",
"../../examples/SharedMemory/PhysicsDirectC_API.h",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
"../../examples/SharedMemory/PhysicsClientC_API.h",
"../../examples/SharedMemory/Win32SharedMemory.cpp",
"../../examples/SharedMemory/Win32SharedMemory.h",
"../../examples/SharedMemory/PosixSharedMemory.cpp",
"../../examples/SharedMemory/PosixSharedMemory.h",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
"../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
}
includedirs {
_OPTIONS["python_include_dir"],
}
libdirs {
_OPTIONS["python_lib_dir"]
}
if os.is("Linux") then
initX11()
end

230
examples/pybullet/pybullet.c
View File

@@ -294,16 +294,16 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
//todo(erwincoumans): set max forces, kp, kd
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double targetPosition=0;
double targetVelocity=0;
double maxForce=100000;
double gains=0.1;
double kp=0.1;
double kd=1.0;
int valid = 0;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -311,10 +311,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
size= PySequence_Size(args);
if (size==4)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@@ -324,7 +324,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==5)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@@ -334,8 +335,17 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==6)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==8)
{
// only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
@@ -343,9 +353,20 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
valid = 1;
}
if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD)
{
PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD");
return NULL;
}
if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8)
{
PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity");
return NULL;
}
if (valid)
{
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -374,10 +395,9 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
case CONTROL_MODE_VELOCITY:
{
double kd = gains;
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetKd(commandHandle,info.m_uIndex,kd);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
@@ -389,10 +409,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
double kp = gains;
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
default:
@@ -405,12 +426,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "error in setJointControl.");
PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
return NULL;
}
static PyObject *
pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
{
@@ -1022,6 +1041,8 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal, fov) -
// set resolution and initialize camera based on camera position, target
// position, camera up, fulstrum near/far values and camera field of view.
// renderImage(w, h, targetPos, distance, yaw, pitch, upAxisIndex, nearVal, farVal, fov)
//
// Note if the (w,h) is too small, the objects may not appear based on
// where the camera has been set
@@ -1132,6 +1153,36 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
aspect = width/height;
b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal);
}
}
else if (size==11)
{
int upAxisIndex=1;
float camDistance,yaw,pitch,roll;
//sometimes more arguments are better :-)
if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &roll, &upAxisIndex, &nearVal, &farVal, &fov))
{
b3RequestCameraImageSetPixelResolution(command,width,height);
if (pybullet_internalSetVector(objTargetPos, targetPos))
{
//printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov);
b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,roll,upAxisIndex);
aspect = width/height;
b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal);
} else
{
PyErr_SetString(SpamError, "Error parsing camera target pos");
}
} else
{
PyErr_SetString(SpamError, "Error parsing arguments");
}
}
else
{
@@ -1143,6 +1194,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
//b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
@@ -1151,11 +1205,13 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
PyObject *pylistRGB;
PyObject* pylistDep;
PyObject* pylistSeg;
int i, j, p;
b3GetCameraImageData(sm, &imageData);
//TODO(hellojas): error handling if image size is 0
pyResultList = PyTuple_New(4);
pyResultList = PyTuple_New(5);
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
@@ -1167,15 +1223,23 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
pylistRGB = PyTuple_New(num);
pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
pylistSeg = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
for (i=0;i<imageData.m_pixelWidth;i++)
{
for (j=0;j<imageData.m_pixelHeight;j++)
{
// TODO(hellojas): validate depth values make sense
int depIndex = i+j*imageData.m_pixelWidth;
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
{
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
}
{
item2 = PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]);
PyTuple_SetItem(pylistSeg, depIndex, item2);
}
for (p=0; p<bytesPerPixel; p++)
{
int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
@@ -1188,6 +1252,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
PyTuple_SetItem(pyResultList, 2,pylistRGB);
PyTuple_SetItem(pyResultList, 3,pylistDep);
PyTuple_SetItem(pyResultList, 4,pylistSeg);
return pyResultList;
}
}
@@ -1467,6 +1532,123 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args)
return Py_None;
}
///Given an object id, joint positions, joint velocities and joint accelerations,
///compute the joint forces using Inverse Dynamics
static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args)
{
int size;
if (0 == sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
size = PySequence_Size(args);
if (size==4)
{
int bodyIndex;
PyObject* objPositionsQ;
PyObject* objVelocitiesQdot;
PyObject* objAccelerations;
if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, &objVelocitiesQdot, &objAccelerations))
{
int szObPos = PySequence_Size(objPositionsQ);
int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations);
int numJoints = b3GetNumJoints(sm, bodyIndex);
if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && (szObAcc == numJoints))
{
int szInBytes = sizeof(double)*numJoints;
int i;
PyObject* pylist = 0;
double* jointPositionsQ = (double*)malloc(szInBytes);
double* jointVelocitiesQdot = (double*)malloc(szInBytes);
double* jointAccelerations = (double*)malloc(szInBytes);
double* jointForcesOutput = (double*)malloc(szInBytes);
for (i = 0; i < numJoints; i++)
{
jointPositionsQ[i] = pybullet_internalGetFloatFromSequence(objPositionsQ, i);
jointVelocitiesQdot[i] = pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
jointAccelerations[i] = pybullet_internalGetFloatFromSequence(objAccelerations, i);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(sm,
bodyIndex, jointPositionsQ, jointVelocitiesQdot, jointAccelerations);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
{
int bodyUniqueId;
int dofCount;
b3GetStatusInverseDynamicsJointForces(statusHandle,
&bodyUniqueId,
&dofCount,
0);
if (dofCount)
{
b3GetStatusInverseDynamicsJointForces(statusHandle,
0,
0,
jointForcesOutput);
{
{
int i;
pylist = PyTuple_New(dofCount);
for (i = 0; i<dofCount; i++)
PyTuple_SetItem(pylist, i, PyFloat_FromDouble(jointForcesOutput[i]));
}
}
}
}
else
{
PyErr_SetString(SpamError, "Internal error in calculateInverseDynamics");
}
}
free(jointPositionsQ);
free(jointVelocitiesQdot);
free(jointAccelerations);
free(jointForcesOutput);
if (pylist)
return pylist;
}
else
{
PyErr_SetString(SpamError, "calculateInverseDynamics numJoints needs to be positive and [joint positions], [joint velocities], [joint accelerations] need to match the number of joints.");
return NULL;
}
}
else
{
PyErr_SetString(SpamError, "calculateInverseDynamics expects 4 arguments, body index, [joint positions], [joint velocities], [joint accelerations].");
return NULL;
}
}
else
{
PyErr_SetString(SpamError, "calculateInverseDynamics expects 4 arguments, body index, [joint positions], [joint velocities], [joint accelerations].");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyMethodDef SpamMethods[] = {
@@ -1533,6 +1715,8 @@ static PyMethodDef SpamMethods[] = {
{"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"},
{ "calculateInverseDynamics", pybullet_calculateInverseDynamics, METH_VARARGS,
"Given an object id, joint positions, joint velocities and joint accelerations, compute the joint forces using Inverse Dynamics" },
//todo(erwincoumans)
//saveSnapshot
//loadSnapshot

46
examples/pybullet/testrender.py Normal file
View File

@@ -0,0 +1,46 @@
import numpy as np
import matplotlib.pyplot as plt
import pybullet
pybullet.connect(pybullet.GUI)
pybullet.loadURDF("r2d2.urdf")
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
yaw = 40
pitch = 10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.01
farPlane = 1000
fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
#renderImage(w, h, view[16], projection[16])
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
for pitch in range (0,360,10) :
img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov)
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
#print 'width = %d height = %d' % (w,h)
# reshape creates np array
np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr*(1./255.)
#show
plt.imshow(np_img_arr,interpolation='none')
#plt.show()
plt.pause(0.01)
pybullet.resetSimulation()

2
src/BulletDynamics/CMakeLists.txt
View File

@@ -32,6 +32,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyJointLimitConstraint.cpp
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodyFixedConstraint.cpp
Featherstone/btMultiBodyJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
@@ -89,6 +90,7 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyJointLimitConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodyFixedConstraint.h
Featherstone/btMultiBodyJointMotor.h
)

2
src/BulletDynamics/Character/btCharacterControllerInterface.h
View File

@@ -37,7 +37,7 @@ public:
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump () = 0;
virtual void jump(const btVector3& dir = btVector3()) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;

457
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
return direction - parallelComponent(direction, normal);
}
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
{
m_upAxis = upAxis;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
setUp(up);
setStepHeight(stepHeight);
m_addedMargin = 0.02;
m_walkDirection.setValue(0,0,0);
m_walkDirection.setValue(0.0,0.0,0.0);
m_AngVel.setValue(0.0, 0.0, 0.0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_gravity = 9.8 * 3 ; // 3G acceleration.
m_gravity = 9.8 * 3.0 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
m_SetjumpSpeed = m_jumpSpeed;
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
m_currentStepOffset = 0;
m_currentStepOffset = 0.0;
m_maxPenetrationDepth = 0.2;
full_drop = false;
bounce_fix = false;
m_linearDamping = btScalar(0.0);
m_angularDamping = btScalar(0.0);
}
btKinematicCharacterController::~btKinematicCharacterController ()
@@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
btScalar maxPen = btScalar(0.0);
// btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_manifoldArray.resize(0);
@@ -198,11 +205,14 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
if (!needsCollision(obj0, obj1))
continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btScalar dist = pt.getDistance();
if (dist < 0.0)
if (dist < -m_maxPenetrationDepth)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
// TODO: cause problems on slopes, not sure if it is needed
//if (dist < maxPen)
//{
// maxPen = dist;
// m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
}
//}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
@@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
start.setOrigin(m_currentPosition);
m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
m_currentPosition = m_targetPosition;
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
}
else
{
world->convexSweepTest (m_convexShape, start, end, callback);
world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
btTransform& xform = m_ghostObject->getWorldTransform();
xform.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(xform);
// fix penetration if we hit a ceiling for example
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(world))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
m_currentPosition = m_targetPosition;
if (m_verticalOffset > 0)
{
m_verticalOffset = 0.0;
m_verticalVelocity = 0.0;
m_currentStepOffset = m_stepHeight;
}
} else {
m_currentStepOffset = m_stepHeight;
m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
{
bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
return collides;
}
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
@@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
@@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
//interferes with step movement
//updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
@@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest)
if (!(start == end))
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
else
{
collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we moved only a fraction
//btScalar hitDistance;
@@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
// if (!m_wasJumping)
// we moved whole way
m_currentPosition = m_targetPosition;
}
m_currentPosition = m_targetPosition;
// if (callback.m_closestHitFraction == 0.f)
// break;
@@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
btVector3 gravity_drop = m_up * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if (m_verticalVelocity > 0.0)
return;
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
@@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -471,30 +529,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
if (bounce_fix == true)
has_hit = callback.hasHit() || callback2.hasHit();
has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
has_hit = callback2.hasHit();
has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
downVelocity = stepHeight;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
if (callback.hasHit() || runonce == true)
if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we dropped a fraction of the height -> hit floor
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,10 +574,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
@@ -579,21 +640,63 @@ btScalar timeInterval
m_velocityTimeInterval += timeInterval;
}
void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
{
m_AngVel = velocity;
}
const btVector3& btKinematicCharacterController::getAngularVelocity() const
{
return m_AngVel;
}
void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
{
m_walkDirection = velocity;
// HACK: if we are moving in the direction of the up, treat it as a jump :(
if (m_walkDirection.length2() > 0)
{
btVector3 w = velocity.normalized();
btScalar c = w.dot(m_up);
if (c != 0)
{
//there is a component in walkdirection for vertical velocity
btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
m_walkDirection -= upComponent;
m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
if (c > 0.0f)
{
m_wasJumping = true;
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
}
}
}
else
m_verticalVelocity = 0.0f;
}
btVector3 btKinematicCharacterController::getLinearVelocity() const
{
return m_walkDirection + (m_verticalVelocity * m_up);
}
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
{
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
}
void btKinematicCharacterController::warp (const btVector3& origin)
@@ -607,62 +710,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration (collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}
#include <stdio.h>
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
if (m_AngVel.length2() > 0.0f)
{
m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
}
// integrate for angular velocity
if (m_AngVel.length2() > 0.0f)
{
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
btQuaternion orn = rot * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
}
// quick check...
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
//btVector3 lvel = m_walkDirection;
btScalar c = 0.0f;
if (m_walkDirection.length2() > 0)
{
// apply damping
m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
}
m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
btTransform xform;
xform = m_ghostObject->getWorldTransform ();
xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp (collisionWorld);
stepUp(collisionWorld);
//todo: Experimenting with behavior of controller when it hits a ceiling..
//bool hitUp = stepUp (collisionWorld);
//if (hitUp)
//{
// m_verticalVelocity -= m_gravity * dt;
// if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
// {
// m_verticalVelocity = m_jumpSpeed;
// }
// if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
// {
// m_verticalVelocity = -btFabs(m_fallSpeed);
// }
// m_verticalOffset = m_verticalVelocity * dt;
// xform = m_ghostObject->getWorldTransform();
//}
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
@@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
stepDown (collisionWorld, dt);
//todo: Experimenting with max jump height
//if (m_wasJumping)
//{
// btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
// if (ds > m_maxJumpHeight)
// {
// // substract the overshoot
// m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
// // max height was reached, so potential energy is at max
// // and kinematic energy is 0, thus velocity is 0.
// if (m_verticalVelocity > 0.0)
// m_verticalVelocity = 0.0;
// }
//}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const
return onGround();
}
void btKinematicCharacterController::jump ()
void btKinematicCharacterController::jump(const btVector3& v)
{
if (!canJump())
return;
m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
#if 0
currently no jumping.
btTransform xform;
@@ -727,14 +898,16 @@ void btKinematicCharacterController::jump ()
#endif
}
void btKinematicCharacterController::setGravity(btScalar gravity)
void btKinematicCharacterController::setGravity(const btVector3& gravity)
{
m_gravity = gravity;
if (gravity.length2() > 0) setUpVector(-gravity);
m_gravity = gravity.length();
}
btScalar btKinematicCharacterController::getGravity() const
btVector3 btKinematicCharacterController::getGravity() const
{
return m_gravity;
return -m_gravity * m_up;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
return m_maxSlopeRadians;
}
bool btKinematicCharacterController::onGround () const
void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
{
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
m_maxPenetrationDepth = d;
}
btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
{
return m_maxPenetrationDepth;
}
bool btKinematicCharacterController::onGround () const
{
return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
}
void btKinematicCharacterController::setStepHeight(btScalar h)
{
m_stepHeight = h;
}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
@@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}
void btKinematicCharacterController::setUp(const btVector3& up)
{
if (up.length2() > 0 && m_gravity > 0.0f)
{
setGravity(-m_gravity * up.normalized());
return;
}
setUpVector(up);
}
void btKinematicCharacterController::setUpVector(const btVector3& up)
{
if (m_up == up)
return;
btVector3 u = m_up;
if (up.length2() > 0)
m_up = up.normalized();
else
m_up = btVector3(0.0, 0.0, 0.0);
if (!m_ghostObject) return;
btQuaternion rot = getRotation(m_up, u);
//set orientation with new up
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion orn = rot.inverse() * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
}
btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
{
if (v0.length2() == 0.0f || v1.length2() == 0.0f)
{
btQuaternion q;
return q;
}
return shortestArcQuatNormalize2(v0, v1);
}

60
src/BulletDynamics/Character/btKinematicCharacterController.h
View File

@@ -43,10 +43,12 @@ protected:
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
btScalar m_maxPenetrationDepth;
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_SetjumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@@ -61,24 +63,34 @@ protected:
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
btVector3 m_AngVel;
btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
btQuaternion m_currentOrientation;
btQuaternion m_targetOrientation;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
int m_upAxis;
btVector3 m_up;
btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
@@ -94,11 +106,18 @@ protected:
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
void setUpVector(const btVector3& up);
btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
@@ -112,14 +131,9 @@ public:
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
void setUpAxis (int axis)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
}
void setUp(const btVector3& up);
const btVector3& getUp() { return m_up; }
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
@@ -136,27 +150,47 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
virtual void setAngularVelocity(const btVector3& velocity);
virtual const btVector3& getAngularVelocity() const;
virtual void setLinearVelocity(const btVector3& velocity);
virtual btVector3 getLinearVelocity() const;
void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getLinearDamping() const { return m_linearDamping; }
void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getAngularDamping() const { return m_angularDamping; }
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
void setStepHeight(btScalar h);
btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
void jump(const btVector3& v = btVector3());
void setGravity(btScalar gravity);
btScalar getGravity() const;
void applyImpulse(const btVector3& v) { jump(v); }
void setGravity(const btVector3& gravity);
btVector3 getGravity() const;
/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
void setMaxPenetrationDepth(btScalar d);
btScalar getMaxPenetrationDepth() const;
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{

16
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
View File

@@ -319,14 +319,6 @@ protected:
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -489,6 +481,14 @@ public:
//If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
virtual btScalar getParam(int num, int axis = -1) const;
static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
};

52
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -299,7 +299,7 @@ void btMultiBody::setupPlanar(int i,
m_links[i].m_eVector = parentComToThisComOffset;
//
static btVector3 vecNonParallelToRotAxis(1, 0, 0);
btVector3 vecNonParallelToRotAxis(1, 0, 0);
if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
vecNonParallelToRotAxis.setValue(0, 1, 0);
//
@@ -466,6 +466,16 @@ btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
}
}
btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
{
btMatrix3x3 result = local_frame;
btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
return result;
}
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
{
int num_links = getNumLinks();
@@ -714,15 +724,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
btScalar * Y = &scratch_r[0];
//
//aux variables
static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
static btSpatialTransformationMatrix fromWorld;
btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
btSpatialTransformationMatrix fromWorld;
fromWorld.m_trnVec.setZero();
/////////////////
@@ -919,8 +929,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::eSpherical:
case btMultibodyLink::ePlanar:
{
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
//unroll the loop?
for(int row = 0; row < 3; ++row)
@@ -1323,11 +1333,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
btScalar * Y = r_ptr;
////////////////
//aux variables
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
static btSpatialTransformationMatrix fromParent;
btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
btSpatialTransformationMatrix fromParent;
/////////////////
// First 'upward' loop.
@@ -1522,8 +1532,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
//
static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
pBaseQuat[0] = baseQuat.x();
pBaseQuat[1] = baseQuat.y();
@@ -1557,8 +1567,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
case btMultibodyLink::eSpherical:
{
static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
break;

4
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -273,6 +273,10 @@ public:
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
//
// transform a frame in local coordinate to a frame in world coordinate
//
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
//
// calculate kinetic energy and angular momentum

628
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -53,323 +53,359 @@ void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala
}
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& constraintNormalAng,
const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
if (bodyA)
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
if (solverConstraint.m_linkA<0)
{
rel_pos1 = posAworld - multiBodyA->getBasePos();
} else
{
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
//resize..
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
//copy/determine
if(jacOrgA)
{
for (int i=0;i<ndofA;i++)
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
}
else
{
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
//determine..
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0;
if (angConstraint) {
torqueAxis0 = constraintNormalAng;
}
else {
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
}
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormalLin;
}
else //if(rb0)
{
btVector3 torqueAxis0;
if (angConstraint) {
torqueAxis0 = constraintNormalAng;
}
else {
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
}
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = constraintNormalLin;
}
if (multiBodyB)
{
if (solverConstraint.m_linkB<0)
{
rel_pos2 = posBworld - multiBodyB->getBasePos();
} else
{
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
//resize..
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
//copy/determine..
if(jacOrgB)
{
for (int i=0;i<ndofB;i++)
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
}
else
{
//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
//determine..
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis1;
if (angConstraint) {
torqueAxis1 = constraintNormalAng;
}
else {
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
}
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormalLin;
}
else //if(rb1)
{
btVector3 torqueAxis1;
if (angConstraint) {
torqueAxis1 = constraintNormalAng;
}
else {
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
}
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -constraintNormalLin;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* deltaVelA = 0;
btScalar* deltaVelB = 0;
int ndofA = 0;
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l = deltaVelA[i];
denom0 += j*l;
}
}
else if(rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
if (angConstraint) {
denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
}
else {
denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
}
}
//
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l = deltaVelB[i];
denom1 += j*l;
}
}
else if(rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
if (angConstraint) {
denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
}
else {
denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
}
}
//
btScalar d = denom0+denom1;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
}
else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
//compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
else if(rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
}
else if(rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
if (bodyA)
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
if (solverConstraint.m_linkA<0)
{
rel_pos1 = posAworld - multiBodyA->getBasePos();
} else
{
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
//resize..
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
//copy/determine
if(jacOrgA)
{
for (int i=0;i<ndofA;i++)
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
}
else
{
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
//determine..
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
else //if(rb0)
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
if (multiBodyB)
{
if (solverConstraint.m_linkB<0)
{
rel_pos2 = posBworld - multiBodyB->getBasePos();
} else
{
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
//resize..
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
//copy/determine..
if(jacOrgB)
{
for (int i=0;i<ndofB;i++)
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
}
else
{
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
//resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
//determine..
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
else //if(rb1)
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* deltaVelA = 0;
btScalar* deltaVelB = 0;
int ndofA = 0;
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l = deltaVelA[i];
denom0 += j*l;
}
}
else if(rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
//
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l = deltaVelB[i];
denom1 += j*l;
}
}
else if(rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
//
btScalar d = denom0+denom1;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
}
else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
}
//compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
else if(rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
else if(rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
}
}
} else
*/
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
{
btScalar positionalError = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar positionalError = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
positionalError = -penetration * erp/infoGlobal.m_timeStep;
positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
return rel_vel;
}

10
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -69,12 +69,16 @@ protected:
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& constraintNormalAng,
const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint = false,
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);

211
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp Normal file
View File

@@ -0,0 +1,211 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyFixedConstraint.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
#define BTMBFIXEDCONSTRAINT_DIM 6
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB)
{
m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB)
{
m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodyFixedConstraint::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
{
}
int btMultiBodyFixedConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyFixedConstraint::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
int numDim = BTMBFIXEDCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
} else
{
if (m_bodyA) {
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
}
}
btVector3 pivotBworld = m_pivotInB;
btMatrix3x3 frameBworld = m_frameInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
} else
{
if (m_bodyB) {
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
}
}
btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
btVector3 constraintNormalLin(0,0,0);
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 3) {
constraintNormalLin[i] = -1;
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
}
else { //i>=3
constraintNormalAng = frameAworld.getColumn(i%3);
posError = angleDiff[i%3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true
);
}
}
}
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

94
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h Normal file
View File

@@ -0,0 +1,94 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
#define BT_MULTIBODY_FIXED_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyFixedConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
public:
btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
virtual ~btMultiBodyFixedConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H

Some files were not shown because too many files have changed in this diff Show More