AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add rope

Browse Source
This commit is contained in:
Irlan
2017-05-28 21:05:32 -03:00
parent e0d2580fa1
commit c411bf341a
34 changed files with 1693 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
include/bounce/bounce.h
View File

@ -54,10 +54,18 @@
#include <bounce/dynamics/contacts/convex_contact.h>
#include <bounce/dynamics/contacts/mesh_contact.h>
#include <bounce/dynamics/rope/rope.h>
#include <bounce/dynamics/cloth/cloth.h>
#include <bounce/dynamics/body.h>
//#include <bounce/dynamics/tree/joints/tree_weld_joint.h>
//#include <bounce/dynamics/tree/joints/tree_prismatic_joint.h>
//#include <bounce/dynamics/tree/joints/tree_revolute_joint.h>
//#include <bounce/dynamics/tree/joints/tree_spherical_joint.h>
//#include <bounce/dynamics/tree/tree_body.h>
//#include <bounce/dynamics/tree/body_tree.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/world_listeners.h>
#include <bounce/cloth/cloth.h>
#endif

6
include/bounce/common/draw.h
View File

@ -101,6 +101,12 @@ public :
// Draw a solid sphere with center and radius.
virtual void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color) = 0;
// Draw a capsule with segment and radius.
virtual void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color) = 0;
// Draw a solid capsule with segment and radius.
virtual void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color) = 0;
// Draw a AABB.
virtual void DrawAABB(const b3AABB3& aabb, const b3Color& color) = 0;

2
include/bounce/common/geometry.h
View File

@ -82,7 +82,7 @@ inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
return P - fraction * plane.normal;
}
// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v)
// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v)
// with respect to a segment AB.
// The last output value is the divisor.
inline void b3BarycentricCoordinates(float32 out[3],

58
include/bounce/common/math/mat33.h
View File

@ -56,6 +56,14 @@ struct b3Mat33
z += B.z;
}
// Subtract this matrix from a matrix.
void operator-=(const b3Mat33& B)
{
x -= B.x;
y -= B.y;
z -= B.z;
}
// Set this matrix to the zero matrix.
void SetZero()
{
@ -81,6 +89,10 @@ struct b3Mat33
b3Vec3 x, y, z;
};
// Usefull constants.
extern b3Mat33 b3Mat33_zero;
extern b3Mat33 b3Mat33_identity;
// Add two matrices.
inline b3Mat33 operator+(const b3Mat33& A, const b3Mat33& B)
{
@ -185,6 +197,11 @@ inline b3Mat33 b3Diagonal(float32 x, float32 y, float32 z)
// returns the zero matrix.
b3Mat33 b3Inverse(const b3Mat33& A);
// Invert a symmetric matrix.
// If the matrix is singular this
// returns the zero matrix.
b3Mat33 b3SymInverse(const b3Mat33& A);
// Return a skew (anti-symmetric) matrix for a vector.
inline b3Mat33 b3Skew(const b3Vec3& v)
{
@ -228,4 +245,43 @@ inline b3Mat33 b3Basis(const b3Vec3& a)
return A;
}
#endif
// Rotation about the x-axis.
inline b3Mat33 b3Mat33RotationX(float32 angle)
{
float32 c = cos(angle);
float32 s = sin(angle);
b3Mat33 R;
R.x.Set(1.0f, 0.0f, 0.0f);
R.y.Set(0.0f, c, s);
R.z.Set(0.0f, -s, c);
return R;
}
// Rotation about the y-axis.
inline b3Mat33 b3Mat33RotationY(float32 angle)
{
float32 c = cos(angle);
float32 s = sin(angle);
b3Mat33 R;
R.x.Set(c, 0.0f, -s);
R.y.Set(0.0f, 1.0f, 0.0f);
R.z.Set(s, 0.0f, c);
return R;
}
// Rotation about the z-axis.
inline b3Mat33 b3Mat33RotationZ(float32 angle)
{
float32 c = cos(angle);
float32 s = sin(angle);
b3Mat33 R;
R.x.Set(c, s, 0.0f);
R.y.Set(-s, c, 0.0f);
R.z.Set(0.0f, 0.0f, 1.0f);
return R;
}
#endif

92
include/bounce/common/math/quat.h
View File

@ -147,7 +147,7 @@ inline b3Quat operator+(const b3Quat& a, const b3Quat& b)
return b3Quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
}
// Sobtract two quaternions.
// Subtract two quaternions.
inline b3Quat operator-(const b3Quat& a, const b3Quat& b)
{
return b3Quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
@ -165,8 +165,8 @@ inline b3Quat operator-(const b3Quat& q)
return b3Quat(-q.x, -q.y, -q.z, -q.w);
}
// Compute a quaternion-quaternion product.
inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
// Multiply two quaternions.
inline b3Quat b3Mul(const b3Quat& a, const b3Quat& b)
{
return b3Quat(
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
@ -175,7 +175,32 @@ inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);
}
// Compute the length of a quaternion.
// Multiply two quaternions.
inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
{
return b3Mul(a, b);
}
// Perform the dot poduct of two quaternions.
inline float32 b3Dot(const b3Quat& a, const b3Quat& b)
{
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
// Return the conjugate of a quaternion.
// If the quaternion is unit this returns its inverse.
inline b3Quat b3Conjugate(const b3Quat& q)
{
return b3Quat(-q.x, -q.y, -q.z, q.w);
}
// Multiply the conjugate of a quaternion times another quaternion.
inline b3Quat b3MulT(const b3Quat& a, const b3Quat& b)
{
return b3Mul(b3Conjugate(a), b);
}
// Return the length of a quaternion.
inline float32 b3Length(const b3Quat& q)
{
return b3Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
@ -193,18 +218,6 @@ inline b3Quat b3Normalize(const b3Quat& q)
return b3Quat(0.0f, 0.0f, 0.0f, 1.0f);
}
// Perform the dot poduct of two quaternions.
inline float b3Dot(const b3Quat& a, const b3Quat& b)
{
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
// Conjugate of a quaternion (inverse if the quaternion is unit).
inline b3Quat b3Conjugate(const b3Quat& q)
{
return b3Quat(-q.x, -q.y, -q.z, q.w);
}
// Rotate a vector.
inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
{
@ -221,8 +234,8 @@ inline b3Vec3 b3MulT(const b3Quat& q, const b3Vec3& v)
return b3Mul(b3Conjugate(q), v);
}
// Convert a 3-by-3 rotation matrix to an rotation quaternion.
inline b3Quat b3ConvertMatToQuat(const b3Mat33& m)
// Convert a 3-by3 rotation matrix to a rotation quaternion.
inline b3Quat b3Mat33Quat(const b3Mat33& m)
{
// Check the diagonal.
float32 trace = m[0][0] + m[1][1] + m[2][2];
@ -286,8 +299,8 @@ inline b3Quat b3ConvertMatToQuat(const b3Mat33& m)
return result;
}
// Convert an rotation quaternion to a 3-by-3 rotation matrix.
inline b3Mat33 b3ConvertQuatToMat(const b3Quat& q)
// Convert a rotation quaternion to a 3-by-3 rotation matrix.
inline b3Mat33 b3QuatMat33(const b3Quat& q)
{
float32 x = q.x, y = q.y, z = q.z, w = q.w;
float32 x2 = x + x, y2 = y + y, z2 = z + z;
@ -301,4 +314,43 @@ inline b3Mat33 b3ConvertQuatToMat(const b3Quat& q)
b3Vec3( xz + wy, yz - wx, 1.0f - (xx + yy)));
}
// Rotation about the x-axis.
inline b3Quat b3QuatRotationX(float32 angle)
{
float32 x = 0.5f * angle;
b3Quat q;
q.x = sin(x);
q.y = 0.0f;
q.z = 0.0f;
q.w = cos(x);
return q;
}
// Rotation about the y-axis.
inline b3Quat b3QuatRotationY(float32 angle)
{
float32 x = 0.5f * angle;
b3Quat q;
q.x = 0.0f;
q.y = sin(x);
q.z = 0.0f;
q.w = cos(x);
return q;
}
// Rotation about the z-axis.
inline b3Quat b3QuatRotationZ(float32 angle)
{
float32 x = 0.5f * angle;
b3Quat q;
q.x = 0.0f;
q.y = 0.0f;
q.z = sin(x);
q.w = cos(x);
return q;
}
#endif

241
include/bounce/common/math/transform.h
View File

@ -23,49 +23,181 @@
#include <bounce/common/math/quat.h>
// A transform represents a rigid frame.
// It has a translation representing a position
// and a rotation representing an orientation.
// It has a translation representing a position
// and a rotation matrix representing an orientation
// relative to some reference frame.
struct b3Transform
{
// Default ctor does nothing for performance.
b3Transform() { }
// Set this transform from a translation vector and an orientation
// quaternion.
b3Transform(const b3Vec3& p, const b3Quat& q)
// Set this transform from a rotation quaternion and a translation vector.
b3Transform(const b3Quat& _rotation, const b3Vec3& _translation)
{
position = p;
rotation = b3ConvertQuatToMat(q);
rotation = b3QuatMat33(_rotation);
position = _translation;
}
// Set this transform to the identity.
// Set this transform to the identity transform.
void SetIdentity()
{
position.SetZero();
rotation.SetIdentity();
position.SetZero();
}
b3Vec3 position; // in fact a translation
b3Mat33 rotation;
b3Vec3 position; // in fact a translation
};
// Multiply a transform times a vector.
inline b3Vec3 b3Mul(const b3Transform& T, const b3Vec3& v)
{
return b3Mul(T.rotation, v) + T.position;
}
// Multiply a transform times another transform.
inline b3Transform b3Mul(const b3Transform& A, const b3Transform& B)
{
// [A y][B x] = [AB Ax+y]
// [0 1][0 1] [0 1 ]
b3Transform C;
C.rotation = b3Mul(A.rotation, B.rotation);
C.position = b3Mul(A.rotation, B.position) + A.position;
return C;
}
// Multiply the transpose of one transform (inverse
// transform) times another transform (composed transform).
inline b3Transform b3MulT(const b3Transform& A, const b3Transform& B)
{
//[A^-1 -A^-1*y][B x] = [A^-1*B A^-1(x-y)]
//[0 1 ][0 1] [0 1 ]
b3Transform C;
C.rotation = b3MulT(A.rotation, B.rotation);
C.position = b3MulT(A.rotation, B.position - A.position);
return C;
}
// Multiply the transpose of a transform times a vector.
// If the transform represents a frame then this transforms
// the vector from one frame to another (inverse transform).
inline b3Vec3 b3MulT(const b3Transform& A, const b3Vec3& v)
{
//[A^-1 -A^-1*y][x] = A^-1*x - A^-1*y = A^-1 * (x - y)
//[0 1 ][1]
return b3MulT(A.rotation, v - A.position);
}
// Inverse transform.
inline b3Transform b3Inverse(const b3Transform& T)
{
b3Transform B;
B.rotation = b3Transpose(T.rotation);
B.position = b3MulT(T.rotation, -T.position);
return B;
}
// Multiply a transform times a vector. If the transform
// represents a frame this returns the vector in terms
// of the frame.
inline b3Vec3 operator*(const b3Transform& T, const b3Vec3& v)
{
return b3Mul(T, v);
}
// Multiply a transform times another transform (composed transform).
inline b3Transform operator*(const b3Transform& A, const b3Transform& B)
{
return b3Mul(A, B);
}
// A quaternion-based transform.
struct b3TransformQT
{
// Default ctor does nothing for performance.
b3TransformQT() { }
// Set this transform from a rotation matrix and a translation vector.
b3TransformQT(const b3Mat33& _rotation, const b3Vec3& _translation)
{
rotation = b3Mat33Quat(_rotation);
translation = _translation;
}
// Set this transform to the identity transform.
void SetIdentity()
{
rotation.SetIdentity();
translation.SetZero();
}
b3Quat rotation;
b3Vec3 translation;
};
// Convert a quaternion based transform to a matrix based transform.
inline b3Transform b3ConvertToTransform(const b3TransformQT& T)
{
return b3Transform(T.rotation, T.translation);
}
// Multiply a transform times another transform.
inline b3TransformQT b3Mul(const b3TransformQT& A, const b3TransformQT& B)
{
b3TransformQT C;
C.rotation = b3Mul(A.rotation, B.rotation);
C.translation = b3Mul(A.rotation, B.translation) + A.translation;
return C;
}
// Multiply the transpose of one transform (inverse
// transform) times another transform (composed transform).
inline b3TransformQT b3MulT(const b3TransformQT& A, const b3TransformQT& B)
{
b3TransformQT C;
C.rotation = b3MulT(A.rotation, B.rotation);
C.translation = b3MulT(A.rotation, B.translation - A.translation);
return C;
}
inline b3TransformQT operator*(const b3TransformQT& A, const b3TransformQT& B)
{
return b3Mul(A, B);
}
// Inverse transform a vector.
inline b3Vec3 b3MulT(const b3TransformQT& A, const b3Vec3& v)
{
return b3MulT(A.rotation, v - A.translation);
}
// Inverse transform.
inline b3TransformQT b3Inverse(const b3TransformQT& T)
{
b3TransformQT B;
B.rotation = b3Conjugate(T.rotation);
B.translation = b3MulT(T.rotation, -T.translation);
return B;
}
// Motion proxy for TOI computation.
struct b3Sweep
{
b3Vec3 localCenter; // local center
b3Vec3 worldCenter0; // last world center
b3Quat orientation0; // last orientation
float32 t0; // last fraction between [0, 1]
b3Vec3 worldCenter; // world center
b3Quat orientation; // world orientation
// Get this sweep transform at a given time between [0, 1]
b3Transform GetTransform(float32 t) const;
// Advance to a new initial state.
void Advance(float32 t);
b3Vec3 localCenter; // local center
b3Quat orientation0; // last orientation
b3Vec3 worldCenter0; // last world center
float32 t0; // last fraction between [0, 1]
b3Quat orientation; // world orientation
b3Vec3 worldCenter; // world center
};
inline b3Transform b3Sweep::GetTransform(float32 t) const
@ -75,7 +207,7 @@ inline b3Transform b3Sweep::GetTransform(float32 t) const
q.Normalize();
b3Transform xf;
xf.rotation = b3ConvertQuatToMat(q);
xf.rotation = b3QuatMat33(q);
xf.position = c - b3Mul(q, localCenter);
return xf;
}
@ -90,71 +222,4 @@ inline void b3Sweep::Advance(float32 t)
t0 = t;
}
// Multiply a transform times a vector. If the transform
// represents a frame this returns the vector in terms
// of the frame.
inline b3Vec3 operator*(const b3Transform& T, const b3Vec3& v)
{
return b3Mul(T.rotation, v) + T.position;
}
// Multiply a transform times another transform (composed transform).
// [A y][B x] = [AB Ax+y]
// [0 1][0 1] [0 1 ]
inline b3Transform operator*(const b3Transform& A, const b3Transform& B)
{
b3Transform C;
C.rotation = b3Mul(A.rotation, B.rotation);
C.position = b3Mul(A.rotation, B.position) + A.position;
return C;
}
// Multiply a transform times a vector.
inline b3Vec3 b3Mul(const b3Transform& T, const b3Vec3& v)
{
return b3Mul(T.rotation, v) + T.position;
}
// Multiply a transform times another transform.
// [A y][B x] = [AB Ax+y]
// [0 1][0 1] [0 1 ]
inline b3Transform b3Mul(const b3Transform& A, const b3Transform& B)
{
b3Transform C;
C.rotation = b3Mul(A.rotation, B.rotation);
C.position = b3Mul(A.rotation, B.position) + A.position;
return C;
}
// Multiply the transpose of one transform (inverse
// transform) times another transform (composed transform).
//[A^-1 -A^-1*y][B x] = [A^-1*B A^-1(x-y)]
//[0 1 ][0 1] [0 1 ]
inline b3Transform b3MulT(const b3Transform& A, const b3Transform& B)
{
b3Transform C;
C.rotation = b3MulT(A.rotation, B.rotation);
C.position = b3MulT(A.rotation, B.position - A.position);
return C;
}
// Multiply the transpose of a transform times a vector.
// If the transform represents a frame then this transforms
// the vector from one frame to another (inverse transform).
//[A^-1 -A^-1*y][x] = A^-1*x - A^-1*y = A^-1 * (x - y)
//[0 1 ][1]
inline b3Vec3 b3MulT(const b3Transform& A, const b3Vec3& v)
{
return b3MulT(A.rotation, v - A.position);
}
// Inverse transform.
inline b3Transform b3Inverse(const b3Transform& T)
{
b3Transform B;
B.rotation = b3Transpose(T.rotation);
B.position = b3MulT(T.rotation, -T.position);
return B;
}
#endif
#endif

11
include/bounce/dynamics/body.h
View File

@ -106,12 +106,6 @@ struct b3BodyDef
class b3Body
{
public:
// A world manages the body construction.
b3Body(const b3BodyDef& def, b3World* world);
// A world manages the body destruction.
~b3Body() { }
// Get the type of the body.
b3BodyType GetType() const;
@ -307,6 +301,9 @@ private:
e_fixedRotationZ = 0x0010,
};
b3Body(const b3BodyDef& def, b3World* world);
~b3Body() { }
// Destroy all shapes associated with the body.
void DestroyShapes();
@ -429,7 +426,7 @@ inline void b3Body::SetTransform(const b3Vec3& position, const b3Vec3& axis, flo
b3Quat q = b3Quat(axis, angle);
m_xf.position = position;
m_xf.rotation = b3ConvertQuatToMat(q);
m_xf.rotation = b3QuatMat33(q);
m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
m_sweep.orientation = q;

0
include/bounce/cloth/cloth.h → include/bounce/dynamics/cloth/cloth.h
View File

109
include/bounce/dynamics/rope/rope.h Normal file
View File

@ -0,0 +1,109 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* 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 B3_ROPE_H
#define B3_ROPE_H
#include <bounce/common/math/transform.h>
class b3Draw;
struct b3RopeBody;
//
struct b3RopeDef
{
b3RopeDef()
{
vertices = NULL;
masses = NULL;
count = 0;
gravity.SetZero();
linearDamping = 0.6f;
angularDamping = 0.6f;
}
//
b3Vec3* vertices;
//
float32* masses;
//
u32 count;
//
b3Vec3 gravity;
//
float32 linearDamping;
//
float32 angularDamping;
};
//
class b3Rope
{
public:
//
b3Rope();
//
~b3Rope();
//
void Initialize(const b3RopeDef& def);
//
void SetOrigin(const b3Vec3& position)
{
m_p = position;
}
//
void SetGravity(const b3Vec3& gravity)
{
m_gravity = gravity;
}
//
void Step(float32 dt);
//
void Draw(b3Draw* draw) const;
private:
//
float32 m_kd1, m_kd2;
//
b3Vec3 m_gravity;
// Base
b3Vec3 m_v;
b3Vec3 m_w;
b3Vec3 m_p;
b3Quat m_q;
//
u32 m_count;
b3RopeBody* m_links;
};
#endif

6
include/bounce/dynamics/shapes/shape.h
View File

@ -59,12 +59,12 @@ struct b3ShapeDef
struct b3MassData
{
// The center of mass of the shape relative to the shape's origin.
b3Vec3 center;
// The mass of the shape in kilograms.
float32 mass;
// The shape center of mass relative to the shape's origin.
b3Vec3 center;
// The rotational inertia of the shape about the shape's center of mass.
b3Mat33 I;
};

340
include/bounce/dynamics/spatial.h Normal file
View File

@ -0,0 +1,340 @@
/*
* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
*
* 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 B3_SPATIAL_H
#define B3_SPATIAL_H
#include <bounce/common/math/mat33.h>
// A 6-by-1 motion vector.
struct b3MotionVec
{
b3MotionVec() { }
b3MotionVec(const b3Vec3& _w, const b3Vec3& _v)
{
w = _w;
v = _v;
}
void SetZero()
{
w.SetZero();
v.SetZero();
}
void operator+=(const b3MotionVec& b)
{
w += b.w;
v += b.v;
}
void operator-=(const b3MotionVec& b)
{
w -= b.w;
v -= b.v;
}
b3Vec3 w, v;
};
// a + b
inline b3MotionVec operator+(const b3MotionVec& a, const b3MotionVec& b)
{
return b3MotionVec(a.w + b.w, a.v + b.v);
}
// a - b
inline b3MotionVec operator-(const b3MotionVec& a, const b3MotionVec& b)
{
return b3MotionVec(a.w - b.w, a.v - b.v);
}
// -a
inline b3MotionVec operator-(const b3MotionVec& a)
{
return b3MotionVec(-a.w, -a.v);
}
// a * s
inline b3MotionVec operator*(const b3MotionVec& a, float32 s)
{
return b3MotionVec(s * a.w, s * a.v);
}
// s * a
inline b3MotionVec operator*(float32 s, const b3MotionVec& a)
{
return b3MotionVec(s * a.w, s * a.v);
}
// a / s
inline b3MotionVec operator/(const b3MotionVec& a, float32 s)
{
return b3MotionVec(a.w / s, a.v / s);
}
// a x b
// [wx 0][w2] = [wx * w2 + 0 * v2] = [wx * w2]
// [vx wx][v2] [vx * w2 + wx * v2] [vx * w2 + wx * v2]
inline b3MotionVec b3Cross(const b3MotionVec& a, const b3MotionVec& b)
{
b3MotionVec result;
result.w = b3Cross(a.w, b.w);
result.v = b3Cross(a.v, b.w) + b3Cross(a.w, b.v);
return result;
}
// A 6-by-1 force vector.
struct b3ForceVec
{
b3ForceVec() { }
b3ForceVec(const b3Vec3& _n, const b3Vec3& _f)
{
n = _n;
f = _f;
}
void SetZero()
{
n.SetZero();
f.SetZero();
}
void operator-=(const b3ForceVec& v)
{
n -= v.n;
f -= v.f;
}
void operator+=(const b3ForceVec& v)
{
n += v.n;
f += v.f;
}
b3Vec3 n, f;
};
// a + b
inline b3ForceVec operator+(const b3ForceVec& a, const b3ForceVec& b)
{
return b3ForceVec(a.n + b.n, a.f + b.f);
}
// a - b
inline b3ForceVec operator-(const b3ForceVec& a, const b3ForceVec& b)
{
return b3ForceVec(a.n - b.n, a.f - b.f);
}
// -a
inline b3ForceVec operator-(const b3ForceVec& a)
{
return b3ForceVec(-a.n, -a.f);
}
// a * s
inline b3ForceVec operator*(const b3ForceVec& a, float32 s)
{
return b3ForceVec(s * a.n, s * a.f);
}
// s * a
inline b3ForceVec operator*(float32 s, const b3ForceVec& a)
{
return b3ForceVec(s * a.n, s * a.f);
}
// a / s
inline b3ForceVec operator/(const b3ForceVec& a, float32 s)
{
return b3ForceVec(a.n / s, a.f / s);
}
// a^T = [a.b^T, a.a^T]
// a^T * b = a.b * b.a + a.a * b.b
inline float32 b3Dot(const b3MotionVec& a, const b3ForceVec& b)
{
return b3Dot(a.v, b.n) + b3Dot(a.w, b.f);
}
// A 6-by-6 spatial inertia matrix stored as a block matrix.
// A, B, C, D are the 3-by-3 matrices. D is not stored
// because it's defined as D = A^T.
struct b3SpInertia
{
b3SpInertia() { }
void SetZero()
{
A.SetZero();
B.SetZero();
C.SetZero();
}
// Set this matrix from mass and rotational inertia
// about the local center of mass (zero vector).
void SetLocalInertia(float32 m, const b3Mat33& I)
{
A.SetZero();
B = b3Diagonal(m);
C = I;
}
void operator-=(const b3SpInertia& M)
{
A -= M.A;
B -= M.B;
C -= M.C;
}
void operator+=(const b3SpInertia& M)
{
A += M.A;
B += M.B;
C += M.C;
}
// Solve Ax = b.
b3MotionVec Solve(const b3ForceVec& b) const;
b3Mat33 A, B, C;
};
inline b3MotionVec b3SpInertia::Solve(const b3ForceVec& b) const
{
// Numerical Recipes, p. 77
// Block matrix inversion:
// https://en.wikipedia.org/wiki/Block_matrix#Block_matrix_inversion
b3Mat33 invA_A, invA_B, invA_C, invA_D;
b3Mat33 D = b3Transpose(A);
b3Mat33 NinvB = -b3Inverse(B);
invA_B = b3Inverse(D * NinvB * A + C);
invA_A = invA_B * D * NinvB;
invA_D = b3Transpose(invA_A);
b3Mat33 T = A * invA_A;
T[0][0] -= 1.0f;
T[1][1] -= 1.0f;
T[2][2] -= 1.0f;
invA_C = NinvB * T;
b3MotionVec x;
x.w = invA_A * b.n + invA_B * b.f;
x.v = invA_C * b.n + invA_D * b.f;
return x;
}
// M * v
inline b3ForceVec operator*(const b3SpInertia& M, const b3MotionVec& v)
{
b3ForceVec result;
result.n = M.A * v.w + M.B * v.v;
result.f = M.C * v.w + b3MulT(M.A, v.v);
return result;
}
// a * b^T
inline b3SpInertia b3Outer(const b3ForceVec& a, const b3ForceVec& b)
{
b3SpInertia result;
result.A = b3Outer(a.n, b.f);
result.B = b3Outer(a.n, b.n);
result.C = b3Outer(a.f, b.f);
return result;
}
// A spatial transformation matrix. This is a
// 6-by-6 matrix, but we represent it efficiently
// with a rotation matrix and a translation vector.
struct b3SpTransform
{
b3SpTransform() { }
b3SpTransform(const b3Mat33& _E, const b3Vec3& _r)
{
E = _E;
r = _r;
}
void SetIdentity()
{
E.SetIdentity();
r.SetZero();
}
b3Mat33 E;
b3Vec3 r;
};
// X * v
inline b3MotionVec b3Mul(const b3SpTransform& X, const b3MotionVec& v)
{
b3MotionVec result;
result.w = X.E * v.w;
result.v = -b3Cross(X.r, X.E * v.w) + X.E * v.v;
return result;
}
// X^-1 * v
inline b3MotionVec b3MulT(const b3SpTransform& X, const b3MotionVec& v)
{
b3MotionVec result;
result.w = b3MulT(X.E, v.w);
result.v = b3MulT(X.E, v.v + b3Cross(X.r, v.w));
return result;
}
// X * v
inline b3ForceVec b3Mul(const b3SpTransform& X, const b3ForceVec& v)
{
b3ForceVec result;
result.n = X.E * v.n;
result.f = -b3Cross(X.r, X.E * v.n) + X.E * v.f;
return result;
}
// X^-1 * v
inline b3ForceVec b3MulT(const b3SpTransform& X, const b3ForceVec& v)
{
b3ForceVec result;
result.n = b3MulT(X.E, v.n);
result.f = b3MulT(X.E, v.f + b3Cross(X.r, v.n));
return result;
}
// X^-1 * I
inline b3SpInertia b3MulT(const b3SpTransform& X, const b3SpInertia& I)
{
b3Mat33 E = X.E;
b3Mat33 ET = b3Transpose(X.E);
b3Mat33 rx = b3Skew(X.r);
b3SpInertia result;
result.A = ET * (I.A - I.B * rx) * E;
result.B = ET * I.B * E;
result.C = ET * (rx * (I.A - I.B * rx) + I.C - b3Transpose(I.A) * rx) * E;
return result;
}
#endif

2
include/bounce/dynamics/world.h
View File

@ -76,7 +76,7 @@ public:
// Remove a joint from the world and deallocate it from the memory.
void DestroyJoint(b3Joint* joint);
// Simulate a physics step.
// The function parameters are the ammount of time to simulate,
// and the number of constraint solver iterations.