mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-05 15:21:06 +00:00
add BspDemo.bsp data file
add sphere2.urdf move btSpatialAlgebra into LinearMath remove some warnings, introduce BT_ZERO, BT_ONE, BT_HALF as defines for 0.f/0., 1.f/1., 0.5f/0.5 respectively
This commit is contained in:
erwincoumans
parent
a1bf9c5556
commit
794c8ec064
@ -2,6 +2,6 @@
|
||||
rem @echo off
|
||||
|
||||
|
||||
premake4 --file=bin2cpp.lua --binfile="../btgui/FontFiles/OpenSans.ttf" --cppfile="../btgui/OpenGLWindow/OpenSans.cpp" --stringname="OpenSansData" bin2cpp
|
||||
premake4 --file=bin2cpp.lua --binfile="../btgui/OpenGLWindow/OpenSans.ttf" --cppfile="../btgui/OpenGLWindow/OpenSans.cpp" --stringname="OpenSansData" bin2cpp
|
||||
|
||||
pause
|
||||
|
||||
@ -9,7 +9,7 @@ function convertFile(filenameIn, filenameOut, stringname)
|
||||
local bytes = f:read(block)
|
||||
if not bytes then break end
|
||||
for b in string.gfind(bytes, ".") do
|
||||
fw:write(string.format("char(%u),", string.byte(b)))
|
||||
fw:write(string.format("%u,", string.byte(b)))
|
||||
end
|
||||
--io.write(string.rep(" ", block - string.len(bytes) + 1))
|
||||
--io.write(string.gsub(bytes, "%c", "."), "\n")
|
||||
|
||||
@ -43,26 +43,6 @@
|
||||
configuration{}
|
||||
end
|
||||
|
||||
function initGlut()
|
||||
configuration {}
|
||||
if os.is("Windows") then
|
||||
configuration {"Windows"}
|
||||
includedirs {
|
||||
projectRootDir .. "btgui/OpenGLWindow/Glut"
|
||||
}
|
||||
libdirs { projectRootDir .. "btgui/OpenGLWindow/Glut"}
|
||||
configuration {"Windows", "x32"}
|
||||
links {"glut32"}
|
||||
configuration {"Windows", "x64"}
|
||||
links {"glut64"}
|
||||
end
|
||||
|
||||
configuration {"MacOSX"}
|
||||
links { "Glut.framework" }
|
||||
configuration {"Linux"}
|
||||
links {"glut"}
|
||||
configuration{}
|
||||
end
|
||||
|
||||
function initGlew()
|
||||
configuration {}
|
||||
@ -70,9 +50,9 @@
|
||||
configuration {"Windows"}
|
||||
defines { "GLEW_STATIC"}
|
||||
includedirs {
|
||||
projectRootDir .. "btgui/OpenGLWindow/GlewWindows"
|
||||
projectRootDir .. "examples/ThirdPartyLibs/Glew"
|
||||
}
|
||||
files { projectRootDir .. "btgui/OpenGLWindow/GlewWindows/glew.c"}
|
||||
files { projectRootDir .. "examples/ThirdPartyLibs/Glew/glew.c"}
|
||||
end
|
||||
if os.is("Linux") then
|
||||
configuration{"Linux"}
|
||||
@ -83,9 +63,9 @@
|
||||
print("Using static glew and dynamic loading of glx functions")
|
||||
defines { "GLEW_STATIC","GLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1"}
|
||||
includedirs {
|
||||
projectRootDir .. "btgui/OpenGLWindow/GlewWindows"
|
||||
projectRootDir .. "examples/ThirdPartyLibs/Glew"
|
||||
}
|
||||
files { projectRootDir .. "btgui/OpenGLWindow/GlewWindows/glew.c"}
|
||||
files { projectRootDir .. "examples/ThirdPartyLibs/Glew/glew.c"}
|
||||
links {"dl"}
|
||||
end
|
||||
|
||||
@ -100,7 +80,7 @@
|
||||
else
|
||||
print("No X11/X.h found, using dynamic loading of X11")
|
||||
includedirs {
|
||||
projectRootDir .. "btgui/OpenGLWindow/optionalX11"
|
||||
projectRootDir .. "examples/ThirdPartyLibs/optionalX11"
|
||||
}
|
||||
defines {"DYNAMIC_LOAD_X11_FUNCTIONS"}
|
||||
links {"dl","pthread"}
|
||||
|
||||
@ -114,83 +114,35 @@
|
||||
|
||||
language "C++"
|
||||
|
||||
include "../examples/ExampleBrowser"
|
||||
include "../examples/OpenGLWindow"
|
||||
|
||||
include "../examples/ThirdPartyLibs/Gwen"
|
||||
|
||||
include "../examples/HelloWorld"
|
||||
include "../examples/BasicDemo"
|
||||
|
||||
|
||||
|
||||
if not _OPTIONS["without-gtest"] then
|
||||
include "../test/gtest-1.7.0"
|
||||
-- include "../test/hello_gtest"
|
||||
include "../test/collision"
|
||||
include "../test/TestBullet3OpenCL"
|
||||
end
|
||||
|
||||
if findOpenGL3() then
|
||||
include "../Demos3/AllBullet2Demos"
|
||||
include "../Demos3/GpuDemos"
|
||||
include"../Demos3/BasicDemoConsole"
|
||||
include"../Demos3/BasicDemoCustomOpenGL2"
|
||||
|
||||
-- include "../Demos3/CpuDemos"
|
||||
-- include "../Demos3/Wavefront"
|
||||
-- include "../btgui/MultiThreading"
|
||||
|
||||
include "../btgui/OpenGLWindow"
|
||||
include "../btgui/Bullet3AppSupport"
|
||||
|
||||
-- include "../Demos3/ImplicitCloth"
|
||||
include "../Demos3/SimpleOpenGL3"
|
||||
include "../btgui/urdf"
|
||||
|
||||
include "../btgui/lua-5.2.3"
|
||||
include "../test/lua"
|
||||
include "../btgui/Gwen"
|
||||
include "../btgui/GwenOpenGLTest"
|
||||
end
|
||||
|
||||
|
||||
-- include "../demo/gpudemo"
|
||||
if _OPTIONS["midi"] then
|
||||
include "../btgui/MidiTest"
|
||||
end
|
||||
|
||||
-- include "../opencl/vector_add_simplified"
|
||||
-- include "../opencl/vector_add"
|
||||
-- include "../test/clew"
|
||||
-- include "../Demos3/GpuGuiInitialize"
|
||||
|
||||
-- include "../test/OpenCL/BasicInitialize"
|
||||
include "../test/OpenCL/KernelLaunch"--
|
||||
-- include "../test/OpenCL/BroadphaseCollision"
|
||||
-- include "../test/OpenCL/NarrowphaseCollision"
|
||||
include "../test/OpenCL/ParallelPrimitives"
|
||||
include "../test/OpenCL/RadixSortBenchmark"
|
||||
|
||||
include "../src/BulletSoftBody"
|
||||
include "../src/BulletDynamics"
|
||||
include "../src/BulletCollision"
|
||||
include "../src/LinearMath"
|
||||
|
||||
include "../src/Bullet3Dynamics"
|
||||
include "../src/Bullet3Common"
|
||||
include "../src/Bullet3Geometry"
|
||||
include "../src/Bullet3Collision"
|
||||
include "../src/Bullet3Serialize/Bullet2FileLoader"
|
||||
include "../src/Bullet3OpenCL"
|
||||
|
||||
-- include "../demo/gpu_initialize"
|
||||
-- include "../opencl/lds_bank_conflict"
|
||||
-- include "../opencl/reduce"
|
||||
-- include "../btgui/OpenGLWindow"
|
||||
-- include "../demo/ObjLoader"
|
||||
-- include "../test/b3DynamicBvhBroadphase"
|
||||
|
||||
if _OPTIONS["enet"] then
|
||||
include "../btgui/enet"
|
||||
include "../test/enet/server"
|
||||
include "../test/enet/client"
|
||||
end
|
||||
|
||||
if _OPTIONS["bullet2demos"] then
|
||||
include "../Extras"
|
||||
if findOpenGL() then
|
||||
include "../Demos"
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
include "../src/BulletSoftBody"
|
||||
include "../src/BulletDynamics"
|
||||
include "../src/BulletCollision"
|
||||
include "../src/LinearMath"
|
||||
|
||||
include "../src/Bullet3Common"
|
||||
include "../src/Bullet3Geometry"
|
||||
include "../src/Bullet3Collision"
|
||||
include "../src/Bullet3Dynamics"
|
||||
include "../src/Bullet3OpenCL"
|
||||
include "../src/Bullet3Serialize/Bullet2FileLoader"
|
||||
|
||||
|
||||
|
||||
BIN
data/BspDemo.bsp
Normal file
BIN
data/BspDemo.bsp
Normal file
Binary file not shown.
47
data/sphere2.urdf
Normal file
47
data/sphere2.urdf
Normal file
@ -0,0 +1,47 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="urdf_robot">
|
||||
<link name="baseLink">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="10.0"/>
|
||||
<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>
|
||||
<sphere radius="0.5"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.5"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="childA">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="10.0"/>
|
||||
<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>
|
||||
<sphere radius="0.5"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.5"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_baseLink_childA" type="fixed">
|
||||
<parent link="baseLink"/>
|
||||
<child link="childA"/>
|
||||
<origin xyz="0 0 1.0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
@ -67,7 +67,7 @@ int main(int argc, char** argv)
|
||||
if (isDynamic)
|
||||
groundShape->calculateLocalInertia(mass,localInertia);
|
||||
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
//using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
|
||||
@ -24,7 +24,7 @@ subject to the following restrictions:
|
||||
|
||||
class btCollisionShape;
|
||||
class btCollisionObject;
|
||||
class btBulletSerializedArrays;
|
||||
struct btBulletSerializedArrays;
|
||||
|
||||
|
||||
struct ConstraintInput;
|
||||
|
||||
@ -772,14 +772,14 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
|
||||
//info->m_lowerLimit[srow] = -SIMD_INFINITY;
|
||||
//info->m_upperLimit[srow] = SIMD_INFINITY;
|
||||
|
||||
btScalar dt = 1.0 / info->fps;
|
||||
btScalar dt = BT_ONE / info->fps;
|
||||
btScalar kd = limot->m_springDamping;
|
||||
btScalar ks = limot->m_springStiffness;
|
||||
btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
|
||||
// btScalar erp = 0.1;
|
||||
btScalar cfm = 0.0;
|
||||
btScalar mA = 1.0 / m_rbA.getInvMass();
|
||||
btScalar mB = 1.0 / m_rbB.getInvMass();
|
||||
btScalar cfm = BT_ZERO;
|
||||
btScalar mA = BT_ONE / m_rbA.getInvMass();
|
||||
btScalar mB = BT_ONE / m_rbB.getInvMass();
|
||||
btScalar m = mA > mB ? mB : mA;
|
||||
btScalar angularfreq = sqrt(ks / m);
|
||||
|
||||
@ -787,7 +787,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
|
||||
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
|
||||
if( 0.25 < angularfreq * dt)
|
||||
{
|
||||
ks = 1.0 / dt / dt / 16.0 / m;
|
||||
ks = BT_ONE / dt / dt / btScalar(16.0) / m;
|
||||
}
|
||||
//avoid overdamping
|
||||
if(kd * dt > m)
|
||||
|
||||
@ -303,7 +303,7 @@ void btHingeConstraint::buildJacobian()
|
||||
|
||||
static inline btScalar btNormalizeAnglePositive(btScalar angle)
|
||||
{
|
||||
return btFmod(btFmod(angle, 2.0*SIMD_PI) + 2.0*SIMD_PI, 2.0*SIMD_PI);
|
||||
return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -36,311 +36,7 @@ enum btMultiBodyLinkFlags
|
||||
|
||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
|
||||
struct btSpatialForceVector
|
||||
{
|
||||
btVector3 m_topVec, m_bottomVec;
|
||||
//
|
||||
btSpatialForceVector() { setZero(); }
|
||||
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
|
||||
btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
setValue(ax, ay, az, lx, ly, lz);
|
||||
}
|
||||
//
|
||||
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
|
||||
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
|
||||
}
|
||||
//
|
||||
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
|
||||
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
|
||||
m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;
|
||||
}
|
||||
//
|
||||
const btVector3 & getLinear() const { return m_topVec; }
|
||||
const btVector3 & getAngular() const { return m_bottomVec; }
|
||||
//
|
||||
void setLinear(const btVector3 &linear) { m_topVec = linear; }
|
||||
void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
|
||||
//
|
||||
void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
|
||||
void addLinear(const btVector3 &linear) { m_topVec += linear; }
|
||||
//
|
||||
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
|
||||
//
|
||||
btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
|
||||
btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
|
||||
btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
|
||||
btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
|
||||
btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
|
||||
btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
|
||||
//btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
|
||||
};
|
||||
|
||||
struct btSpatialMotionVector
|
||||
{
|
||||
btVector3 m_topVec, m_bottomVec;
|
||||
//
|
||||
btSpatialMotionVector() { setZero(); }
|
||||
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
|
||||
//
|
||||
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
|
||||
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
|
||||
}
|
||||
//
|
||||
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
|
||||
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
|
||||
m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;
|
||||
}
|
||||
//
|
||||
const btVector3 & getAngular() const { return m_topVec; }
|
||||
const btVector3 & getLinear() const { return m_bottomVec; }
|
||||
//
|
||||
void setAngular(const btVector3 &angular) { m_topVec = angular; }
|
||||
void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
|
||||
//
|
||||
void addAngular(const btVector3 &angular) { m_topVec += angular; }
|
||||
void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
|
||||
//
|
||||
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
|
||||
//
|
||||
btScalar dot(const btSpatialForceVector &b) const
|
||||
{
|
||||
return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
|
||||
}
|
||||
//
|
||||
template<typename SpatialVectorType>
|
||||
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
|
||||
{
|
||||
out.m_topVec = m_topVec.cross(b.m_topVec);
|
||||
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
|
||||
}
|
||||
template<typename SpatialVectorType>
|
||||
SpatialVectorType cross(const SpatialVectorType &b) const
|
||||
{
|
||||
SpatialVectorType out;
|
||||
out.m_topVec = m_topVec.cross(b.m_topVec);
|
||||
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
|
||||
return out;
|
||||
}
|
||||
//
|
||||
btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
|
||||
btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
|
||||
btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
|
||||
btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
|
||||
btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
|
||||
btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
|
||||
btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
|
||||
};
|
||||
|
||||
struct btSymmetricSpatialDyad
|
||||
{
|
||||
btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
|
||||
//
|
||||
btSymmetricSpatialDyad() { setIdentity(); }
|
||||
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
|
||||
//
|
||||
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
|
||||
{
|
||||
m_topLeftMat = topLeftMat;
|
||||
m_topRightMat = topRightMat;
|
||||
m_bottomLeftMat = bottomLeftMat;
|
||||
}
|
||||
//
|
||||
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
|
||||
{
|
||||
m_topLeftMat += topLeftMat;
|
||||
m_topRightMat += topRightMat;
|
||||
m_bottomLeftMat += bottomLeftMat;
|
||||
}
|
||||
//
|
||||
void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); }
|
||||
//
|
||||
btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
|
||||
{
|
||||
m_topLeftMat -= mat.m_topLeftMat;
|
||||
m_topRightMat -= mat.m_topRightMat;
|
||||
m_bottomLeftMat -= mat.m_bottomLeftMat;
|
||||
return *this;
|
||||
}
|
||||
//
|
||||
btSpatialForceVector operator * (const btSpatialMotionVector &vec)
|
||||
{
|
||||
return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
|
||||
}
|
||||
};
|
||||
|
||||
struct btSpatialTransformationMatrix
|
||||
{
|
||||
btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
|
||||
btVector3 m_trnVec;
|
||||
//
|
||||
enum eOutputOperation
|
||||
{
|
||||
None = 0,
|
||||
Add = 1,
|
||||
Subtract = 2
|
||||
};
|
||||
//
|
||||
template<typename SpatialVectorType>
|
||||
void transform( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformRotationOnly( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformInverse( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformInverseRotationOnly( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void transformInverse( const btSymmetricSpatialDyad &inMat,
|
||||
btSymmetricSpatialDyad &outMat,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1],
|
||||
m_trnVec[2], 0, -m_trnVec[0],
|
||||
-m_trnVec[1], m_trnVec[0], 0);
|
||||
|
||||
|
||||
if(outOp == None)
|
||||
{
|
||||
outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
|
||||
outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
|
||||
outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
|
||||
outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
|
||||
outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
|
||||
outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
|
||||
outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
SpatialVectorType operator * (const SpatialVectorType &vec)
|
||||
{
|
||||
SpatialVectorType out;
|
||||
transform(vec, out);
|
||||
return out;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
|
||||
{
|
||||
//output op maybe?
|
||||
|
||||
out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
|
||||
out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
|
||||
out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
|
||||
//maybe simple a*spatTranspose(a) would be nicer?
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
|
||||
{
|
||||
btSymmetricSpatialDyad out;
|
||||
|
||||
out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
|
||||
out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
|
||||
out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
|
||||
|
||||
return out;
|
||||
//maybe simple a*spatTranspose(a) would be nicer?
|
||||
}
|
||||
#include "LinearMath/btSpatialAlgebra.h"
|
||||
|
||||
#endif
|
||||
//}
|
||||
|
||||
@ -94,7 +94,7 @@ struct btVectorX
|
||||
{
|
||||
T temp;
|
||||
temp = scale / absxi;
|
||||
ssq = ssq * (temp * temp) + 1.0;
|
||||
ssq = ssq * (temp * temp) + BT_ONE;
|
||||
scale = absxi;
|
||||
}
|
||||
else
|
||||
|
||||
@ -486,9 +486,17 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
#define SIMD_EPSILON DBL_EPSILON
|
||||
#define SIMD_INFINITY DBL_MAX
|
||||
#define BT_ONE 1.0
|
||||
#define BT_ZERO 0.0
|
||||
#define BT_TWO 2.0
|
||||
#define BT_HALF 0.5
|
||||
#else
|
||||
#define SIMD_EPSILON FLT_EPSILON
|
||||
#define SIMD_INFINITY FLT_MAX
|
||||
#define BT_ONE 1.0f
|
||||
#define BT_ZERO 0.0f
|
||||
#define BT_TWO 2.0f
|
||||
#define BT_HALF 0.5f
|
||||
#endif
|
||||
|
||||
SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
|
||||
|
||||
331
src/LinearMath/btSpatialAlgebra.h
Normal file
331
src/LinearMath/btSpatialAlgebra.h
Normal file
@ -0,0 +1,331 @@
|
||||
/*
|
||||
Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
///These spatial algebra classes are used for btMultiBody,
|
||||
///see BulletDynamics/Featherstone
|
||||
|
||||
#ifndef BT_SPATIAL_ALGEBRA_H
|
||||
#define BT_SPATIAL_ALGEBRA_H
|
||||
|
||||
|
||||
#include "btMatrix3x3.h"
|
||||
|
||||
struct btSpatialForceVector
|
||||
{
|
||||
btVector3 m_topVec, m_bottomVec;
|
||||
//
|
||||
btSpatialForceVector() { setZero(); }
|
||||
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
|
||||
btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
setValue(ax, ay, az, lx, ly, lz);
|
||||
}
|
||||
//
|
||||
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
|
||||
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
|
||||
}
|
||||
//
|
||||
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
|
||||
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
|
||||
m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;
|
||||
}
|
||||
//
|
||||
const btVector3 & getLinear() const { return m_topVec; }
|
||||
const btVector3 & getAngular() const { return m_bottomVec; }
|
||||
//
|
||||
void setLinear(const btVector3 &linear) { m_topVec = linear; }
|
||||
void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
|
||||
//
|
||||
void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
|
||||
void addLinear(const btVector3 &linear) { m_topVec += linear; }
|
||||
//
|
||||
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
|
||||
//
|
||||
btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
|
||||
btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
|
||||
btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
|
||||
btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
|
||||
btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
|
||||
btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
|
||||
//btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
|
||||
};
|
||||
|
||||
struct btSpatialMotionVector
|
||||
{
|
||||
btVector3 m_topVec, m_bottomVec;
|
||||
//
|
||||
btSpatialMotionVector() { setZero(); }
|
||||
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
|
||||
//
|
||||
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
|
||||
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
|
||||
}
|
||||
//
|
||||
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
|
||||
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
|
||||
{
|
||||
m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
|
||||
m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;
|
||||
}
|
||||
//
|
||||
const btVector3 & getAngular() const { return m_topVec; }
|
||||
const btVector3 & getLinear() const { return m_bottomVec; }
|
||||
//
|
||||
void setAngular(const btVector3 &angular) { m_topVec = angular; }
|
||||
void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
|
||||
//
|
||||
void addAngular(const btVector3 &angular) { m_topVec += angular; }
|
||||
void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
|
||||
//
|
||||
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
|
||||
//
|
||||
btScalar dot(const btSpatialForceVector &b) const
|
||||
{
|
||||
return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
|
||||
}
|
||||
//
|
||||
template<typename SpatialVectorType>
|
||||
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
|
||||
{
|
||||
out.m_topVec = m_topVec.cross(b.m_topVec);
|
||||
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
|
||||
}
|
||||
template<typename SpatialVectorType>
|
||||
SpatialVectorType cross(const SpatialVectorType &b) const
|
||||
{
|
||||
SpatialVectorType out;
|
||||
out.m_topVec = m_topVec.cross(b.m_topVec);
|
||||
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
|
||||
return out;
|
||||
}
|
||||
//
|
||||
btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
|
||||
btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
|
||||
btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
|
||||
btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
|
||||
btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
|
||||
btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
|
||||
btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
|
||||
};
|
||||
|
||||
struct btSymmetricSpatialDyad
|
||||
{
|
||||
btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
|
||||
//
|
||||
btSymmetricSpatialDyad() { setIdentity(); }
|
||||
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
|
||||
//
|
||||
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
|
||||
{
|
||||
m_topLeftMat = topLeftMat;
|
||||
m_topRightMat = topRightMat;
|
||||
m_bottomLeftMat = bottomLeftMat;
|
||||
}
|
||||
//
|
||||
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
|
||||
{
|
||||
m_topLeftMat += topLeftMat;
|
||||
m_topRightMat += topRightMat;
|
||||
m_bottomLeftMat += bottomLeftMat;
|
||||
}
|
||||
//
|
||||
void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); }
|
||||
//
|
||||
btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
|
||||
{
|
||||
m_topLeftMat -= mat.m_topLeftMat;
|
||||
m_topRightMat -= mat.m_topRightMat;
|
||||
m_bottomLeftMat -= mat.m_bottomLeftMat;
|
||||
return *this;
|
||||
}
|
||||
//
|
||||
btSpatialForceVector operator * (const btSpatialMotionVector &vec)
|
||||
{
|
||||
return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
|
||||
}
|
||||
};
|
||||
|
||||
struct btSpatialTransformationMatrix
|
||||
{
|
||||
btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
|
||||
btVector3 m_trnVec;
|
||||
//
|
||||
enum eOutputOperation
|
||||
{
|
||||
None = 0,
|
||||
Add = 1,
|
||||
Subtract = 2
|
||||
};
|
||||
//
|
||||
template<typename SpatialVectorType>
|
||||
void transform( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformRotationOnly( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformInverse( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void transformInverseRotationOnly( const SpatialVectorType &inVec,
|
||||
SpatialVectorType &outVec,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
if(outOp == None)
|
||||
{
|
||||
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
|
||||
outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void transformInverse( const btSymmetricSpatialDyad &inMat,
|
||||
btSymmetricSpatialDyad &outMat,
|
||||
eOutputOperation outOp = None)
|
||||
{
|
||||
const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1],
|
||||
m_trnVec[2], 0, -m_trnVec[0],
|
||||
-m_trnVec[1], m_trnVec[0], 0);
|
||||
|
||||
|
||||
if(outOp == None)
|
||||
{
|
||||
outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
|
||||
outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
|
||||
outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
|
||||
}
|
||||
else if(outOp == Add)
|
||||
{
|
||||
outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
|
||||
outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
|
||||
outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
|
||||
}
|
||||
else if(outOp == Subtract)
|
||||
{
|
||||
outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
|
||||
outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
|
||||
outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
SpatialVectorType operator * (const SpatialVectorType &vec)
|
||||
{
|
||||
SpatialVectorType out;
|
||||
transform(vec, out);
|
||||
return out;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
|
||||
{
|
||||
//output op maybe?
|
||||
|
||||
out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
|
||||
out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
|
||||
out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
|
||||
//maybe simple a*spatTranspose(a) would be nicer?
|
||||
}
|
||||
|
||||
template<typename SpatialVectorType>
|
||||
btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
|
||||
{
|
||||
btSymmetricSpatialDyad out;
|
||||
|
||||
out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
|
||||
out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
|
||||
out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
|
||||
|
||||
return out;
|
||||
//maybe simple a*spatTranspose(a) would be nicer?
|
||||
}
|
||||
|
||||
#endif //BT_SPATIAL_ALGEBRA_H
|
||||
|
||||
@ -12,7 +12,7 @@
|
||||
#include "Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h"
|
||||
|
||||
#ifdef B3_USE_ZLIB
|
||||
#include "../btgui/minizip/unzip.h"
|
||||
#include "minizip/unzip.h"
|
||||
#endif
|
||||
|
||||
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
|
||||
|
||||
@ -20,6 +20,7 @@ function createProject(vendor)
|
||||
{
|
||||
".","../gtest-1.7.0/include",
|
||||
"../../src",
|
||||
"../../examples/ThirdPartyLibs", --for unzip
|
||||
}
|
||||
|
||||
links {"gtest",
|
||||
@ -34,8 +35,8 @@ function createProject(vendor)
|
||||
--you can comment out the following few lines, then you need to unzip the untest_data.zip manually
|
||||
defines {"B3_USE_ZLIB"}
|
||||
files {
|
||||
"../../btgui/minizip/*.c",
|
||||
"../../btgui/zlib/*.c",
|
||||
"../../examples/ThirdPartyLibs/minizip/*.c",
|
||||
"../../examples/ThirdPartyLibs/zlib/*.c",
|
||||
}
|
||||
|
||||
files {
|
||||
|
||||
@ -15,6 +15,12 @@
|
||||
|
||||
}
|
||||
|
||||
|
||||
if os.is("Windows") then
|
||||
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
|
||||
defines {"_VARIADIC_MAX=10"}
|
||||
end
|
||||
|
||||
links {"LinearMath", "gtest"}
|
||||
|
||||
files {
|
||||
|
||||
@ -5,9 +5,11 @@
|
||||
files{"src/gtest-all.cc"}
|
||||
|
||||
--defines {"GTEST_HAS_PTHREAD=1"}
|
||||
|
||||
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
|
||||
defines {"_VARIADIC_MAX=10"}
|
||||
|
||||
if os.is("Windows") then
|
||||
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
|
||||
defines {"_VARIADIC_MAX=10"}
|
||||
end
|
||||
|
||||
--targetdir "../../lib"
|
||||
|
||||
|
||||
@ -12,6 +12,11 @@
|
||||
".","../gtest-1.7.0/include"
|
||||
}
|
||||
|
||||
if os.is("Windows") then
|
||||
--see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
|
||||
defines {"_VARIADIC_MAX=10"}
|
||||
end
|
||||
|
||||
-- linkLib "gtest"
|
||||
links "gtest"
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user