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

fix a lot of issues, add gyroscopic force integrator, add contact polygon winding, add some quaternion constraints, add more tests

Browse Source
This commit is contained in:
Irlan
2017-03-24 18:49:41 -03:00
parent dd6ca355e9
commit 8defab9945
103 changed files with 3840 additions and 3355 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/bounce/bounce.h
View File

@ -30,7 +30,7 @@
#include <bounce/collision/gjk/gjk.h>
#include <bounce/collision/gjk/gjk_cache.h>
#include <bounce/collision/sat/sat.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
#include <bounce/collision/broad_phase.h>
#include <bounce/collision/shapes/sphere.h>

2
include/bounce/cloth/cloth.h
View File

@ -20,7 +20,7 @@
#define B3_CLOTH_H
#include <bounce/common/math/vec3.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
struct b3Mesh;
class b3Draw;

18
include/bounce/collision/broad_phase.h
View File

@ -98,9 +98,9 @@ private :
u32 m_moveBufferCapacity;
// The buffer holding the unique overlapping AABB pairs.
b3Pair* m_pairBuffer;
u32 m_pairBufferCapacity;
u32 m_pairBufferCount;
b3Pair* m_pairs;
u32 m_pairCapacity;
u32 m_pairCount;
};
inline const b3AABB3& b3BroadPhase::GetAABB(i32 proxyId) const
@ -144,7 +144,7 @@ template<class T>
inline void b3BroadPhase::FindNewPairs(T* callback)
{
// Reset the overlapping pairs buffer count for the current step.
m_pairBufferCount = 0;
m_pairCount = 0;
// Notifying this class with QueryCallback(), gets the (duplicated) overlapping pair buffer.
for (u32 i = 0; i < m_moveBufferCount; ++i)
@ -164,22 +164,22 @@ inline void b3BroadPhase::FindNewPairs(T* callback)
m_moveBufferCount = 0;
// Sort the (duplicated) overlapping pair buffer to prune duplicated pairs.
std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount);
std::sort(m_pairs, m_pairs + m_pairCount);
// Skip duplicated overlapping pairs.
u32 index = 0;
while (index < m_pairBufferCount)
while (index < m_pairCount)
{
const b3Pair* primaryPair = m_pairBuffer + index;
const b3Pair* primaryPair = m_pairs + index;
// Report an unique overlapping pair to the client.
callback->AddPair(m_tree.GetUserData(primaryPair->proxy1), m_tree.GetUserData(primaryPair->proxy2));
// Skip all duplicated pairs until an unique pair is found.
++index;
while (index < m_pairBufferCount)
while (index < m_pairCount)
{
const b3Pair* secondaryPair = m_pairBuffer + index;
const b3Pair* secondaryPair = m_pairs + index;
if (secondaryPair->proxy1 != primaryPair->proxy1 || secondaryPair->proxy2 != primaryPair->proxy2)
{
break;

34
include/bounce/collision/distance.h → include/bounce/collision/collision.h
View File

@ -16,42 +16,12 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_DISTANCE_H
#define B3_DISTANCE_H
#ifndef B3_COLLISION_H
#define B3_COLLISION_H
#include <bounce/common/geometry.h>
#include <bounce/collision/shapes/aabb3.h>
///////////////////////////////////////////////////////////////////////////////////////////////////
// Find the closest point for a point P to a normalized plane.
b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane);
// Find the closest point for a point P to a segment AB.
b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P,
const b3Vec3& A, const b3Vec3& B);
// Find the closest point for a point P to a triangle ABC.
b3Vec3 b3ClosestPointOnTriangle(const b3Vec3& P,
const b3Vec3& A, const b3Vec3& B, const b3Vec3& C);
// Find the closest points of two lines.
void b3ClosestPointsOnLines(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& E1,
const b3Vec3& P2, const b3Vec3& E2);
// Find the closest points of two normalized lines.
void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& N1,
const b3Vec3& P2, const b3Vec3& N2);
// Find the closest points of two segments.
void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& Q1,
const b3Vec3& P2, const b3Vec3& Q2);
///////////////////////////////////////////////////////////////////////////////////////////////////
// Input for a ray cast.
struct b3RayCastInput
{

24
include/bounce/collision/gjk/gjk.h
View File

@ -26,12 +26,12 @@ struct b3SimplexCache;
struct b3SimplexVertex
{
b3Vec3 pointA; // support vertex on proxy A
b3Vec3 pointB; // support vertex on proxy B
b3Vec3 point1; // support vertex on proxy 1
b3Vec3 point2; // support vertex on proxy 2
b3Vec3 point; // minkowski vertex
float32 weight; // barycentric coordinate for point
u32 indexA; // support A vertex index
u32 indexB; // support B vertex index
u32 index1; // support 1 vertex index
u32 index2; // support 2 vertex index
};
struct b3Simplex
@ -41,7 +41,7 @@ struct b3Simplex
b3Vec3 GetSearchDirection(const b3Vec3& Q) const;
b3Vec3 GetClosestPoint() const;
void GetClosestPoints(b3Vec3* pA, b3Vec3* pB) const;
void GetClosestPoints(b3Vec3* p1, b3Vec3* p2) const;
void Solve2(const b3Vec3& Q);
void Solve3(const b3Vec3& Q);
@ -49,9 +49,11 @@ struct b3Simplex
// Cache
void ReadCache(const b3SimplexCache* cache,
const b3Transform& xfA, const b3GJKProxy& proxyA,
const b3Transform& xfB, const b3GJKProxy& proxyB);
const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2);
void WriteCache(b3SimplexCache* cache) const;
float32 GetMetric() const;
};
@ -61,14 +63,14 @@ struct b3Simplex
// If the distance is zero then the proxies are overlapping.
struct b3GJKOutput
{
b3Vec3 pointA; // closest point on proxy A
b3Vec3 pointB; // closest point on proxy B
b3Vec3 point1; // closest point on proxy 1
b3Vec3 point2; // closest point on proxy 2
float32 distance; // euclidean distance between the closest points
u32 iterations; // number of GJK iterations
};
// Find the closest points and distance between two proxies.
b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
const b3Transform& xfB, const b3GJKProxy& proxyB);
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2);
#endif

16
include/bounce/collision/gjk/gjk_cache.h
View File

@ -31,25 +31,25 @@ struct b3SimplexCache
float32 metric; // distance or area or volume
u32 iterations; // number of GJK iterations
u16 count; // number of support vertices
u8 indexA[4]; // support vertices on proxy A
u8 indexB[4]; // support vertices on proxy B
u8 index1[4]; // support vertices on proxy 1
u8 index2[4]; // support vertices on proxy 2
};
// Find the closest points and distance between two proxies.
// Assumes a simplex is given for increasing the performance of
// the algorithm when called more than once.
b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
const b3Transform& xfB, const b3GJKProxy& proxyB,
b3GJKOutput b3GJK(const b3Transform& xf1, const b3GJKProxy& proxy1,
const b3Transform& xf2, const b3GJKProxy& proxy2,
bool applyRadius, b3SimplexCache* cache);
// A feature pair contains the vertices of the features associated
// with the closest points.
struct b3GJKFeaturePair
{
u32 indexA[3]; // vertices on proxy A
u32 countA; // number of vertices on proxy A
u32 indexB[3]; // vertices on proxy B
u32 countB; // number of vertices on proxy B
u32 index1[3]; // vertices on proxy 1
u32 count1; // number of vertices on proxy 1
u32 index2[3]; // vertices on proxy 2
u32 count2; // number of vertices on proxy 2
};
// Identify the vertices of the features that the closest points between two

12
include/bounce/collision/sat/sat.h
View File

@ -33,15 +33,15 @@ struct b3FaceQuery
float32 b3Project(const b3Hull* hull, const b3Plane& plane);
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2);
///////////////////////////////////////////////////////////////////////////////////////////////////
struct b3EdgeQuery
{
u32 indexA;
u32 indexB;
u32 index1;
u32 index2;
float32 separation;
};
@ -49,7 +49,7 @@ bool b3IsMinkowskiFace(const b3Vec3& A, const b3Vec3& B, const b3Vec3& B_x_A, co
float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C1);
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1,
const b3Transform& xf2, const b3Hull* hull2);
#endif

12
include/bounce/collision/sat/sat_edge_and_hull.h
View File

@ -21,20 +21,20 @@
#include <bounce/collision/sat/sat.h>
struct b3Capsule;
struct b3Segment;
///////////////////////////////////////////////////////////////////////////////////////////////////
float32 b3ProjectEdge(const b3Capsule* hull, const b3Plane& plane);
float32 b3ProjectEdge(const b3Segment* hull, const b3Plane& plane);
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Capsule* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Segment* hull1,
const b3Transform& xf2, const b3Hull* hull2);
///////////////////////////////////////////////////////////////////////////////////////////////////
float32 b3ProjectEdge(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C2);
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Capsule* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Segment* hull1,
const b3Transform& xf2, const b3Hull* hull2);
#endif

8
include/bounce/collision/sat/sat_vertex_and_hull.h
View File

@ -21,13 +21,11 @@
#include <bounce/collision/sat/sat.h>
struct b3Sphere;
///////////////////////////////////////////////////////////////////////////////////////////////////
float32 b3ProjectVertex(const b3Sphere* hull, const b3Plane& plane);
float32 b3ProjectVertex(const b3Vec3& hull, const b3Plane& plane);
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Sphere* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3FaceQuery b3QueryFaceSeparation(const b3Transform& xf1, const b3Vec3& hull1,
const b3Transform& xf2, const b3Hull* hull2);
#endif

6
include/bounce/collision/shapes/box_hull.h
View File

@ -90,6 +90,8 @@ struct b3BoxHull : public b3Hull
faces = boxFaces;
planes = boxPlanes;
faceCount = 6;
Validate();
}
// Set this box from three extents and centered at the origin.
@ -102,8 +104,6 @@ struct b3BoxHull : public b3Hull
}
// Set this box to the unit box and transform it.
// Warning: The transform must not contain non-uniform
// scaling!
void SetTransform(const b3Transform& T)
{
boxVertices[0] = b3Vec3(1.0f, 1.0f, -1.0f);
@ -169,6 +169,8 @@ struct b3BoxHull : public b3Hull
faceCount = 6;
centroid = T * centroid;
Validate();
}
};

9
include/bounce/collision/shapes/capsule.h
View File

@ -21,21 +21,20 @@
#include <bounce/common/math/vec3.h>
struct b3Capsule
struct b3Segment
{
b3Vec3 vertices[2];
float32 radius;
const b3Vec3& GetVertex(u32 index) const;
u32 GetSupportVertex(const b3Vec3& direction) const;
};
inline const b3Vec3& b3Capsule::GetVertex(u32 index) const
inline const b3Vec3& b3Segment::GetVertex(u32 index) const
{
return vertices[index];
}
inline u32 b3Capsule::GetSupportVertex(const b3Vec3& d) const
inline u32 b3Segment::GetSupportVertex(const b3Vec3& d) const
{
if (b3Dot(d, vertices[0]) > b3Dot(d, vertices[1]))
{

2
include/bounce/collision/shapes/hull.h
View File

@ -51,7 +51,9 @@ struct b3Hull
const b3Plane& GetPlane(u32 index) const;
u32 GetSupportVertex(const b3Vec3& direction) const;
//u32 GetSupportEdge(const b3Vec3& direction) const;
u32 GetSupportFace(const b3Vec3& direction) const;
b3Plane GetEdgeSidePlane(u32 index) const;
u32 GetSize() const;

2
include/bounce/collision/trees/dynamic_tree.h
View File

@ -22,7 +22,7 @@
#include <bounce/common/draw.h>
#include <bounce/common/template/stack.h>
#include <bounce/collision/shapes/aabb3.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
#define NULL_NODE (-1)

2
include/bounce/collision/trees/static_tree.h
View File

@ -22,7 +22,7 @@
#include <bounce/common/draw.h>
#include <bounce/common/template/stack.h>
#include <bounce/collision/shapes/aabb3.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
#define NULL_NODE_S (0xFFFFFFFF)

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

@ -54,7 +54,8 @@ public :
e_contactPointsFlag = 0x0008,
e_contactNormalsFlag = 0x0010,
e_contactTangentsFlag = 0x0020,
e_aabbsFlag = 0x0040,
e_contactAreasFlag = 0x0040,
e_aabbsFlag = 0x0080,
};
b3Draw()

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

@ -75,13 +75,6 @@ inline float32 b3Distance(const b3Vec3& P, const b3Plane& plane)
return b3Dot(plane.normal, P) - plane.offset;
}
// Project a point onto a normal plane.
inline b3Vec3 b3Project(const b3Vec3& P, const b3Plane& plane)
{
float32 fraction = b3Distance(P, plane);
return P - fraction * plane.normal;
}
// Compute barycentric coordinates (u, v) for point Q to segment AB.
// The last output value is the divisor.
inline void b3Barycentric(float32 out[3],
@ -150,4 +143,33 @@ inline void b3Barycentric(float32 out[5],
out[4] = sign * divisor;
}
// Project a point onto a normal plane.
inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
{
float32 fraction = b3Distance(P, plane);
return P - fraction * plane.normal;
}
// Project a point onto a segment AB.
inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
{
float32 wAB[3];
b3Barycentric(wAB, A, B, P);
if (wAB[1] <= 0.0f)
{
return A;
}
if (wAB[0] <= 0.0f)
{
return B;
}
float32 s = 1.0f / wAB[2];
float32 wA = s * wAB[0];
float32 wB = s * wAB[1];
return wA * A + wB * B;
}
#endif

375
include/bounce/common/math/mat.h
View File

@ -20,69 +20,350 @@
#define B3_MAT_H
#include <bounce/common/math/math.h>
#include <bounce/common/math/mat22.h>
#include <bounce/common/math/mat33.h>
// A vector stored in column-major order.
template<u32 n>
struct b3Vec
struct b3Mat23
{
b3Vec() { }
b3Mat23() { }
const float32& operator[](u32 i) const
b3Mat23(const b3Vec2& _x, const b3Vec2& _y, const b3Vec2& _z)
{
return e[i];
}
float32& operator[](u32 i)
{
return e[i];
}
void operator+=(const b3Vec<n>& v)
{
for (u32 i = 0; i < n; ++i)
{
e[i] += v[i];
}
x = _x;
y = _y;
z = _z;
}
float32 e[n];
void SetZero()
{
x.SetZero();
y.SetZero();
z.SetZero();
}
b3Vec2 x, y, z;
};
template<u32 n>
inline b3Vec<n> operator-(const b3Vec<n>& v)
struct b3Mat24
{
b3Vec<n> result;
for (u32 i = 0; i < n; ++i)
b3Mat24() { }
b3Mat24(const b3Vec2& _x, const b3Vec2& _y, const b3Vec2& _z, const b3Vec2& _w)
{
result[i] = -v[i];
x = _x;
y = _y;
z = _z;
w = _w;
}
void SetZero()
{
x.SetZero();
y.SetZero();
z.SetZero();
w.SetZero();
}
b3Vec2 x, y, z, w;
};
struct b3Mat32
{
b3Mat32() { }
b3Mat32(const b3Vec3& _x, const b3Vec3& _y)
{
x = _x;
y = _y;
}
void SetZero()
{
x.SetZero();
y.SetZero();
}
b3Vec3 x, y;
};
struct b3Mat34
{
b3Mat34() { }
b3Mat34(const b3Vec3& _x, const b3Vec3& _y, const b3Vec3& _z, const b3Vec3& _w)
{
x = _x;
y = _y;
z = _z;
w = _w;
}
void SetZero()
{
x.SetZero();
y.SetZero();
z.SetZero();
w.SetZero();
}
b3Vec3 x, y, z, w;
};
struct b3Vec4
{
b3Vec4() { }
b3Vec4(float32 _x, float32 _y, float32 _z, float32 _w)
{
x = _x;
y = _y;
z = _z;
w = _w;
}
void SetZero()
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
w = 0.0f;
}
float32 x, y, z, w;
};
struct b3Mat43
{
b3Mat43() { }
b3Mat43(const b3Vec4& _x, const b3Vec4& _y, const b3Vec4& _z)
{
x = _x;
y = _y;
z = _z;
}
void SetZero()
{
x.SetZero();
y.SetZero();
z.SetZero();
}
b3Vec4 x, y, z;
};
struct b3Mat44
{
b3Mat44() { }
b3Mat44(const b3Vec4& _x, const b3Vec4& _y, const b3Vec4& _z, const b3Vec4& _w)
{
x = _x;
y = _y;
z = _z;
w = _w;
}
void SetZero()
{
x.SetZero();
y.SetZero();
z.SetZero();
w.SetZero();
}
b3Vec4 x, y, z, w;
};
inline b3Vec4 operator+(const b3Vec4& a, const b3Vec4& b)
{
return b3Vec4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
}
inline b3Vec4 operator-(const b3Vec4& a, const b3Vec4& b)
{
return b3Vec4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
}
// 1x1 * 1x4 = 1x1
inline b3Vec4 operator*(float32 s, const b3Vec4& v)
{
return b3Vec4(s * v.x, s * v.y, s * v.z, s * v.w);
}
// a * 4x4 = 4x4
inline b3Mat44 operator*(float32 s, const b3Mat44& A)
{
return b3Mat44(s * A.x, s * A.y, s * A.z, s * A.w);
}
// 4x4 * 4x1 = 4x1
inline b3Vec4 operator*(const b3Mat44& A, const b3Vec4& v)
{
return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w;
}
// 4x4 * 4x4 = 4x4
inline b3Mat44 operator*(const b3Mat44& A, const b3Mat44& B)
{
return b3Mat44(A * B.x, A * B.y, A * B.z, A * B.w);
}
// a * 3x4 = 3x4
inline b3Mat34 operator*(float32 s, const b3Mat34& A)
{
return b3Mat34(s * A.x, s * A.y, s * A.z, s * A.w);
}
// 4x3 * 3x1 = 4x1
inline b3Vec4 operator*(const b3Mat43& A, const b3Vec3& v)
{
return v.x * A.x + v.y * A.y + v.z * A.z;
}
// 3x4 * 4x1 = 3x1
inline b3Vec3 operator*(const b3Mat34& A, const b3Vec4& v)
{
return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w;
}
// 1x4 * 4x1 = 1x1
inline float32 operator*(const b3Vec4& A, const b3Vec4& B)
{
return A.x * B.x + A.y * B.y + A.z * B.z + A.w * B.w;
}
// 3x1 * 1x1 = 3x1
inline b3Vec3 operator*(const b3Vec3& v, float32 s)
{
return s * v;
}
// 2x1 * 1x1 = 2x1
inline b3Vec2 operator*(const b3Vec2& v, float32 s)
{
return s * v;
}
// 1x3 * 3x1 = 1x1
inline float32 operator*(const b3Vec3& A, const b3Vec3& B)
{
return A.x * B.x + A.y * B.y + A.z * B.z;
}
// 1x3 * 3x3 = 1x3
inline b3Vec3 operator*(const b3Vec3& A, const b3Mat33& B)
{
return b3Vec3(A * B.x, A * B.y, A * B.z);
}
// 1x4 * 4x4 = 1x4
inline b3Vec4 operator*(const b3Vec4& A, const b3Mat44& B)
{
return b3Vec4(A * B.x, A * B.y, A * B.z, A * B.w);
}
// 1x4 * 4x3 = 1x3
inline b3Vec3 operator*(const b3Vec4& A, const b3Mat43& B)
{
return b3Vec3(A * B.x, A * B.y, A * B.z);
}
// 3x2 * 2x1 = 3x1
inline b3Vec3 operator*(const b3Mat32& A, const b3Vec2& B)
{
return B.x * A.x + B.y * A.y;
}
// 2x3 * 2x1 = 2x1
inline b3Vec2 operator*(const b3Mat23& A, const b3Vec3& B)
{
return B.x * A.x + B.y * A.y + B.z * A.z;
}
// 2x3 * 2x2 = 2x2
inline b3Mat22 operator*(const b3Mat23& A, const b3Mat32& B)
{
return b3Mat22(A * B.x, A * B.y);
}
// 2x3 * 3x3 = 2x3
inline b3Mat23 operator*(const b3Mat23& A, const b3Mat33& B)
{
return b3Mat23(A * B.x, A * B.y, A * B.z);
}
// 3x4 * 4x3 = 3x3
inline b3Mat33 operator*(const b3Mat34& A, const b3Mat43& B)
{
return b3Mat33(A * B.x, A * B.y, A * B.z);
}
// 3x4 * 4x4 = 3x3
inline b3Mat34 operator*(const b3Mat34& A, const b3Mat44& B)
{
return b3Mat34(A * B.x, A * B.y, A * B.z, A * B.w);
}
// 2x4 * 4x1 = 2x1
inline b3Vec2 operator*(const b3Mat24& A, const b3Vec4& B)
{
return B.x * A.x + B.y * A.y + B.z * A.z + B.w * A.w;
}
// 2x4 * 4x3 = 4x3
inline b3Mat23 operator*(const b3Mat24& A, const b3Mat43& B)
{
return b3Mat23(A * B.x, A * B.y, A * B.z);
}
// 2x4 * 2x4 = 2x4
inline b3Mat24 operator*(const b3Mat24& A, const b3Mat44& B)
{
return b3Mat24(A * B.x, A * B.y, A * B.z, A * B.w);
}
// 4x4 * 4x3 = 4x3
inline b3Mat43 operator*(const b3Mat44& A, const b3Mat43& B)
{
return b3Mat43(A * B.x, A * B.y, A * B.z);
}
inline b3Mat23 b3Transpose(const b3Mat32& A)
{
b3Mat23 result;
result.x = b3Vec2(A.x.x, A.y.x);
result.y = b3Vec2(A.x.y, A.y.y);
result.z = b3Vec2(A.x.z, A.y.z);
return result;
}
// A matrix stored in column-major order.
template<u32 n, u32 m>
struct b3Mat
inline b3Mat32 b3Transpose(const b3Mat23& A)
{
b3Mat() { }
b3Mat32 result;
result.x = b3Vec3(A.x.x, A.y.x, A.z.x);
result.y = b3Vec3(A.x.y, A.y.y, A.z.y);
return result;
}
const float32& operator()(u32 i, u32 j) const
{
return e[i + n * j];
}
inline b3Mat34 b3Transpose(const b3Mat43& A)
{
b3Mat34 result;
result.x = b3Vec3(A.x.x, A.y.x, A.z.x);
result.y = b3Vec3(A.x.y, A.y.y, A.z.y);
result.z = b3Vec3(A.x.z, A.y.z, A.z.z);
result.w = b3Vec3(A.x.w, A.y.w, A.z.w);
return result;
}
float32& operator()(u32 i, u32 j)
{
return e[i + n * j];
}
float32 e[n * m];
};
// Solve Ax = b.
// It doesn't compute the inverse.
// Therefore, is more efficient.
// Returns false if the matrix is singular.
// Warning: Make sure to pass a copy of the original matrix to the function. A will be invalidated.
bool b3Solve(float32* b, float32* A, u32 n);
inline b3Mat43 b3Transpose(const b3Mat34& A)
{
b3Mat43 result;
result.x = b3Vec4(A.x.x, A.y.x, A.z.x, A.w.x);
result.y = b3Vec4(A.x.y, A.y.y, A.z.y, A.w.y);
result.z = b3Vec4(A.x.z, A.y.z, A.z.z, A.w.z);
return result;
}
#endif

15
include/bounce/common/math/mat22.h
View File

@ -30,6 +30,13 @@ struct b3Mat22
// Set this matrix from two vectors.
b3Mat22(const b3Vec2& _x, const b3Vec2& _y) : x(_x), y(_y) { }
// Set this matrix to the zero matrix.
void SetZero()
{
x.SetZero();
y.SetZero();
}
// Solve Ax = b.
// It doesn't compute the inverse.
// Therefore, is more efficient.
@ -45,6 +52,12 @@ inline b3Vec2 operator*(const b3Mat22& A, const b3Vec2& v)
return v.x * A.x + v.y * A.y;
}
// Add two matrices.
inline b3Mat22 operator+(const b3Mat22& A, const b3Mat22& B)
{
return b3Mat22(A.x + B.x, A.y + B.y);
}
// Multiply a matrix times a vector.
inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v)
{
@ -54,6 +67,6 @@ inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v)
// Invert a matrix.
// If the matrix determinant is zero this returns
// the zero matrix.
inline b3Mat22 b3Inverse(const b3Mat22& A);
b3Mat22 b3Inverse(const b3Mat22& A);
#endif

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

@ -37,7 +37,19 @@ struct b3Quat
{
Set(axis, angle);
}
// Write an indexed value to this quaternion.
float32& operator[](u32 i)
{
return (&x)[i];
}
// Read an indexed value from this quaternion.
float32 operator[](u32 i) const
{
return (&x)[i];
}
// Add a quaternion to this quaternion.
void operator+=(const b3Quat& q)
{
@ -203,6 +215,77 @@ inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
return v + qs * t + b3Cross(qv, t);
}
// Inverse rotate a vector by an orientation quaternion.
inline b3Vec3 b3MulT(const b3Quat& q, const b3Vec3& v)
{
return b3Mul(b3Conjugate(q), v);
}
// Convert a 3-by-3 rotation matrix to an orientation quaternion.
inline b3Quat b3ConvertRotToQuat(const b3Mat33& m)
{
// Check the diagonal.
float32 trace = m[0][0] + m[1][1] + m[2][2];
if (trace > 0.0f)
{
b3Quat result;
float32 s = b3Sqrt(trace + 1.0f);
result.w = 0.5f * s;
float32 t = 0.5f / s;
result.x = t * (m[1][2] - m[2][1]);
result.y = t * (m[2][0] - m[0][2]);
result.z = t * (m[0][1] - m[1][0]);
return result;
}
// Diagonal is negative.
const i32 next[3] = { 1, 2, 0 };
i32 i = 0;
if (m[1][1] > m[0][0])
{
i = 1;
}
if (m[2][2] > m[i][i])
{
i = 2;
}
i32 j = next[i];
i32 k = next[j];
float32 s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
float32 q[4];
q[i] = s * 0.5f;
float32 t;
if (s != 0.0f)
{
t = 0.5f / s;
}
else
{
t = s;
}
q[3] = t * (m[j][k] - m[k][j]);
q[j] = t * (m[i][j] + m[j][i]);
q[k] = t * (m[i][k] + m[k][i]);
b3Quat result;
result.x = q[0];
result.y = q[1];
result.z = q[2];
result.w = q[3];
return result;
}
// Convert an orientation quaternion to a 3-by-3 rotation matrix.
inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
{
@ -218,43 +301,4 @@ inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
b3Vec3( xz + wy, yz - wx, 1.0f - (xx + yy)));
}
// Perform a linear interpolation between two quaternions.
inline b3Quat b3Lerp(const b3Quat& a, const b3Quat& b, float32 fraction)
{
B3_ASSERT(fraction >= 0.0f);
B3_ASSERT(fraction <= 1.0f);
float32 w1 = 1.0f - fraction;
float32 w2 = fraction;
return w1 * a + w2 * b;
}
// Perform a spherical interpolation between two quaternions.
inline b3Quat b3Slerp(const b3Quat& a, const b3Quat& b, float32 fraction)
{
B3_ASSERT(fraction >= 0.0f);
B3_ASSERT(fraction <= 1.0f);
float32 w1 = 1.0f - fraction;
float32 w2 = fraction;
float32 cosine = b3Dot(a, b);
b3Quat b2 = b;
if (cosine <= FLT_EPSILON * FLT_EPSILON)
{
b2 = -b;
cosine = -cosine;
}
if (cosine > 1.0f - FLT_EPSILON)
{
return w1 * a + w2 * b2;
}
float32 angle = acos(cosine);
float32 sine = sin(angle);
b3Quat q1 = sin(w1 * angle) * a;
b3Quat q2 = sin(w2 * angle) * b2;
float32 invSin = 1.0f / sine;
return invSin * (q1 + q2);
}
#endif

24
include/bounce/common/settings.h
View File

@ -43,10 +43,6 @@ typedef float float32;
// Collision
// Maximum number of vertices, edges, and faces a
// polyhedron can have. Don't increase this value.
#define B3_MAX_HULL_FEATURES (256)
// How much an AABB in the broad-phase should be extended by
// to disallow unecessary proxy updates.
// A larger value increases performance when there are
@ -60,15 +56,11 @@ typedef float float32;
#define B3_AABB_MULTIPLIER (2.0f)
// Collision and constraint tolerance.
#define B3_LINEAR_SLOP (0.01f)
// Collision and constraint tolerance.
#define B3_LINEAR_SLOP (0.005f)
#define B3_ANGULAR_SLOP (2.0f / 180.0f * B3_PI)
// The radius of the hull shape skin.
#define B3_HULL_RADIUS (2.0f * B3_LINEAR_SLOP)
// Twice the radius of the hull shape skin.
#define B3_HULL_RADIUS (0.0f * B3_LINEAR_SLOP)
#define B3_HULL_RADIUS_SUM (2.0f * B3_HULL_RADIUS)
// Dynamics
@ -95,26 +87,22 @@ typedef float float32;
#define B3_MAX_ROTATION (0.5f * B3_PI)
#define B3_MAX_ROTATION_SQUARED (B3_MAX_ROTATION * B3_MAX_ROTATION)
// The maximum linear position correction used when solving constraints. This helps to
// The maximum position correction used when solving constraints. This helps to
// prevent overshoot.
#define B3_MAX_LINEAR_CORRECTION (0.2f)
// The maximum angular position correction used when solving constraints. This helps to
// prevent overshoot.
#define B3_MAX_ANGULAR_CORRECTION (8.0f / 180.0f * B3_PI)
// This controls how faster overlaps should be resolved per step.
// This is less than and would be close to 1, so that the all overlap is resolved per step.
// However values very close to 1 may lead to overshoot.
#define B3_BAUMGARTE (0.2f)
#define B3_BAUMGARTE (0.1f)
// If the relative velocity of a contact point is below
// the threshold then restitution is not applied.
#define B3_VELOCITY_THRESHOLD (1.0f)
// Sleep
#define B3_TIME_TO_SLEEP (0.2f )
#define B3_TIME_TO_SLEEP (0.2f)
#define B3_SLEEP_LINEAR_TOL (0.05f)
#define B3_SLEEP_ANGULAR_TOL (2.0f / 180.0f * B3_PI)
@ -128,6 +116,8 @@ typedef float float32;
#define B3_MiB(n) (1024 * B3_KiB(n))
#define B3_GiB(n) (1024 * B3_MiB(n))
#define B3_FORCE_INLINE __forceinline
#define B3_PROFILE(name) b3ProfileScope scope(name)
// You should implement this function to use your own memory allocator.

4
include/bounce/common/template/array.h
View File

@ -50,12 +50,12 @@ public:
return m_elements + i;
}
const T* Elements() const
const T* Begin() const
{
return m_elements;
}
T* Elements()
T* Begin()
{
return m_elements;
}

28
include/bounce/dynamics/contacts/collide/clip.h
View File

@ -27,19 +27,19 @@
// A combination of features used to uniquely identify a vertex on a feature.
struct b3FeaturePair
{
u8 inEdgeA; // incoming edge on hull A
u8 inEdgeB; // incoming edge on hull B
u8 outEdgeA; // outgoing edge on hull A
u8 outEdgeB; // outgoing edge on hull B
u8 inEdge1; // incoming edge on hull 1
u8 inEdge2; // incoming edge on hull 2
u8 outEdge1; // outgoing edge on hull 1
u8 outEdge2; // outgoing edge on hull 2
};
inline b3FeaturePair b3MakePair(u32 inEdgeA, u32 inEdgeB, u32 outEdgeA, u32 outEdgeB)
inline b3FeaturePair b3MakePair(u32 inEdge1, u32 inEdge2, u32 outEdge1, u32 outEdge2)
{
b3FeaturePair out;
out.inEdgeA = u8(inEdgeA);
out.inEdgeB = u8(inEdgeB);
out.outEdgeA = u8(outEdgeA);
out.outEdgeB = u8(outEdgeB);
out.inEdge1 = u8(inEdge1);
out.inEdge2 = u8(inEdge2);
out.outEdge1 = u8(outEdge1);
out.outEdge2 = u8(outEdge2);
return out;
}
@ -75,11 +75,11 @@ struct b3ClipPlane
};
struct b3Hull;
struct b3Capsule;
struct b3Segment;
// Build a clip edge for an edge.
void b3BuildEdge(b3ClipVertex vOut[2],
const b3Capsule* hull);
const b3Segment* hull);
// Build a clip polygon given an index to the polygon face.
void b3BuildPolygon(b3ClipPolygon& pOut,
@ -99,15 +99,15 @@ void b3ClipPolygonToPlane(b3ClipPolygon& pOut,
// Clip a segment by a hull face (side planes).
// Return the number of output points.
u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
const b3ClipVertex vIn[2], const b3Capsule* hull);
const b3ClipVertex vIn[2], const b3Segment* hull);
// Clip a segment by a hull face (side planes).
// Return the number of output points.
u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
const b3ClipVertex vIn[2], const b3Transform& xf, u32 index, const b3Hull* hull);
const b3ClipVertex vIn[2], const b3Transform& xf, float32 r, u32 index, const b3Hull* hull);
// Clip a polygon by a hull face (side planes).
void b3ClipPolygonToFace(b3ClipPolygon& pOut,
const b3ClipPolygon& pIn, const b3Transform& xf, u32 index, const b3Hull* hull);
const b3ClipPolygon& pIn, const b3Transform& xf, float32 r, u32 index, const b3Hull* hull);
#endif

62
include/bounce/dynamics/contacts/collide/collide.h
View File

@ -44,29 +44,29 @@ struct b3SATFeaturePair
{
enum Type
{
e_edgeA, // an edge on hull A and an edge on hull B
e_faceA, // a face on hull A and a vertex/edge/face on hull B
e_faceB, // a face on hull B and a vertex/edge/face on hull A
e_edge1, // an edge on hull 1 and an edge on hull 2
e_face1, // a face on hull 1 and a vertex/edge/face on hull 2
e_face2, // a face on hull 2 and a vertex/edge/face on hull 1
};
b3SATCacheType state; // sat result
Type type; // feature pair type
u32 indexA; // feature index on hull A
u32 indexB; // feature index on hull B
u32 index1; // feature index on hull 1
u32 index2; // feature index on hull 2
};
struct b3FeatureCache
{
// Read the current state of the cache.
// Return e_unkown if neither a separation or penetration was detected.
b3SATCacheType ReadState(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3SATCacheType ReadState(const b3Transform& xf1, float32 r1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2);
b3SATCacheType ReadEdge(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3SATCacheType ReadEdge(const b3Transform& xf1, float32 r1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2);
b3SATCacheType ReadFace(const b3Transform& xfA, const b3Hull* hullA,
const b3Transform& xfB, const b3Hull* hullB);
b3SATCacheType ReadFace(const b3Transform& xf1, float32 r1, const b3Hull* hull1,
const b3Transform& xf2, float32 r2, const b3Hull* hull2);
// We could increase the cache size (e.g. a feature pair of the last two frames).
b3SATFeaturePair m_featurePair;
@ -95,45 +95,45 @@ public:
};
// Test if two generic shapes are overlapping.
bool b3TestOverlap(const b3Transform& xfA, u32 indexA, const b3Shape* shapeA,
const b3Transform& xfB, u32 indexB, const b3Shape* shapeB,
bool b3TestOverlap(const b3Transform& xf1, u32 index1, const b3Shape* shape1,
const b3Transform& xf2, u32 index2, const b3Shape* shape2,
b3ConvexCache* cache);
// Compute a manifold for two generic shapes except when one of them is a mesh.
void b3CollideShapeAndShape(b3Manifold& manifold,
const b3Transform& xfA, const b3Shape* shapeA,
const b3Transform& xfB, const b3Shape* shapeB,
const b3Transform& xf1, const b3Shape* shape1,
const b3Transform& xf2, const b3Shape* shape2,
b3ConvexCache* cache);
// Compute a manifold for two spheres.
void b3CollideSphereAndSphere(b3Manifold& manifold,
const b3Transform& xfA, const b3SphereShape* shapeA,
const b3Transform& xfB, const b3SphereShape* shapeB);
const b3Transform& xf1, const b3SphereShape* shape1,
const b3Transform& xf2, const b3SphereShape* shape2);
// Compute a manifold for a sphere and a hull.
void b3CollideSphereAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3SphereShape* shapeA,
const b3Transform& xfB, const b3HullShape* shapeB);
void b3CollideSphereAndHull(b3Manifold& manifold,
const b3Transform& xf1, const b3SphereShape* shape1,
const b3Transform& xf2, const b3HullShape* shape2);
// Compute a manifold for a sphere and a capsule.
void b3CollideSphereAndCapsule(b3Manifold& manifold,
const b3Transform& xfA, const b3SphereShape* shapeA,
const b3Transform& xfB, const b3CapsuleShape* shapeB);
const b3Transform& xf1, const b3SphereShape* shape1,
const b3Transform& xf2, const b3CapsuleShape* shape2);
// Compute a manifold for two capsules.
void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
const b3Transform& xfA, const b3CapsuleShape* shapeA,
const b3Transform& xfB, const b3CapsuleShape* shapeB);
const b3Transform& xf1, const b3CapsuleShape* shape1,
const b3Transform& xf2, const b3CapsuleShape* shape2);
// Compute a manifold for a capsule and a hull.
void b3CollideCapsuleAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3CapsuleShape* shapeA,
const b3Transform& xfB, const b3HullShape* shapeB);
void b3CollideCapsuleAndHull(b3Manifold& manifold,
const b3Transform& xf1, const b3CapsuleShape* shape1,
const b3Transform& xf2, const b3HullShape* shape2);
// Compute a manifold for two hulls.
void b3CollideHullAndHull(b3Manifold& manifold,
const b3Transform& xfA, const b3HullShape* shapeA,
const b3Transform& xfB, const b3HullShape* shapeB,
void b3CollideHullAndHull(b3Manifold& manifold,
const b3Transform& xf1, const b3HullShape* shape1,
const b3Transform& xf2, const b3HullShape* shape2,
b3ConvexCache* cache);
#endif
#endif

2
include/bounce/dynamics/contacts/contact.h
View File

@ -118,7 +118,7 @@ protected:
// Test if the shapes in this contact are overlapping.
virtual bool TestOverlap() = 0;
// Build new contact points.
// Initialize contact constraits.
virtual void Collide() = 0;
b3ContactType m_type;

2
include/bounce/dynamics/contacts/contact_cluster.h
View File

@ -64,6 +64,6 @@ u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32
// Reduce a set of contact points to a quad (approximate convex polygon).
// All points must lie in a common plane and an initial point must be given.
void b3ReducePolygon(b3ClusterPolygon& pOut, const b3ClusterPolygon& pIn,
u32 startIndex);
u32 startIndex, const b3Vec3& normal);
#endif

18
include/bounce/dynamics/contacts/contact_solver.h
View File

@ -62,29 +62,25 @@ struct b3VelocityConstraintPoint
{
b3Vec3 rA;
b3Vec3 rB;
b3Vec3 normal;
float32 normalMass;
float32 normalImpulse;
b3Vec3 tangent1;
b3Vec3 tangent2;
b3Mat22 tangentMass;
b3Vec2 tangentImpulse;
float32 velocityBias;
};
struct b3VelocityConstraintManifold
{
b3Vec3 center;
b3Vec3 rA;
b3Vec3 rB;
b3Vec3 normal;
b3Vec3 tangent1;
b3Vec3 tangent2;
b3Vec3 rA;
b3Vec3 rB;
//float32 leverArm;
b3Mat22 tangentMass;
b3Vec2 tangentImpulse;
b3Vec2 tangentImpulse;
float32 motorImpulse;
float32 motorMass;

51
include/bounce/dynamics/contacts/manifold.h
View File

@ -27,67 +27,54 @@
// A contact manifold point.
struct b3ManifoldPoint
{
b3Vec3 localNormal1; // local normal on the first shape
b3Vec3 localPoint1; // local point on the first shape without its radius
b3Vec3 localPoint2; // local point on the other shape without its radius
u32 triangleKey; // triangle identifier
u32 key; // point identifier
b3Vec3 localNormal; // local normal on the first shape
b3Vec3 localPoint; // local point on the first shape
b3Vec3 localPoint2; // local point on the other shape
float32 normalImpulse; // normal impulse
b3Vec2 tangentImpulse; // tangent impulses
u8 persisting; // indicates that the point is persisting
u32 persisting; // is this point persistent?
};
// A manifold is a group of contact points with similar contact normal.
// A contact manifold is a group of contact points with similar contact normal.
struct b3Manifold
{
// Choose arbitrary impulses for warm starting.
void GuessImpulses();
// Clear the manifold.
// Initialize impulses arbitrarily for warm starting.
void Initialize();
// Initialize impulses for warm starting.
void FindImpulses(const b3Manifold& old);
// Initialize impulses for warm starting from the old manifold.
void Initialize(const b3Manifold& old);
b3ManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; // manifold points
u32 pointCount; // number of manifold points
b3Vec3 center;
b3Vec3 normal;
b3Vec3 tangent1;
b3Vec3 tangent2;
b3Vec2 tangentImpulse;
float32 motorImpulse;
};
// A world manifold point.
struct b3WorldManifoldPoint
{
// Initialize this manifold from a local manifold point and two transforms.
// The radii should come from the shapes that generated the manifold.
void Initialize(const b3ManifoldPoint* point,
const b3Transform& xfA, float32 radiusA,
const b3Transform& xfB, float32 radiusB);
void Initialize(const b3ManifoldPoint* p, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB);
b3Vec3 point;
b3Vec3 normal;
b3Vec2 tangents[2];
float32 separation;
};
// A contact manifold is a group of contact points with similar normal.
struct b3WorldManifold
{
// Initialize this world manifold from a local manifold and two transforms.
// The radii should come from the shapes that generated the manifold.
void Initialize(const b3Manifold* manifold,
const b3Transform& xfA, float32 radiusA,
const b3Transform& xfB, float32 radiusB);
void Initialize(const b3Manifold* m, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB);
u32 pointCount;
b3WorldManifoldPoint points[B3_MAX_MANIFOLD_POINTS];
b3Vec3 center;
b3Vec3 normal;
b3Vec3 tangent1;
b3Vec3 tangent2;
b3WorldManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; // contact points
u32 pointCount; // number of contact points
};
#endif
#endif

6
include/bounce/dynamics/joints/cone_joint.h
View File

@ -37,7 +37,7 @@ struct b3ConeJointDef : public b3JointDef
// The joint frame relative to body A's frame.
b3Transform localFrameA;
// The joint frame relative to body B's frame.
b3Transform localFrameB;
@ -64,7 +64,7 @@ public:
// Get the joint frame relative to body A's frame.
const b3Transform& GetLocalFrameA() const;
// Get the joint frame relative to body B's frame.
const b3Transform& GetLocalFrameB() const;
@ -109,7 +109,7 @@ private:
float32 m_mA;
float32 m_mB;
b3Mat33 m_iA;
b3Mat33 m_iB;
b3Mat33 m_iB;
b3Vec3 m_localCenterA;
b3Vec3 m_localCenterB;

1
include/bounce/dynamics/joints/joint.h
View File

@ -20,6 +20,7 @@
#define B3_JOINT_H
#include <bounce/common/math/transform.h>
#include <bounce/common/math/mat.h>
#include <bounce/common/template/list.h>
#include <bounce/dynamics/time_step.h>

75
include/bounce/dynamics/joints/revolute_joint.h
View File

@ -20,15 +20,17 @@
#define B3_REVOLUTE_JOINT_H
#include <bounce/dynamics/joints/joint.h>
#include <bounce/common/math/mat.h>
struct b3RevoluteJointDef : public b3JointDef
{
b3RevoluteJointDef()
{
type = e_revoluteJoint;
localFrameA.SetIdentity();
localFrameB.SetIdentity();
localAnchorA.SetZero();
localRotationA.SetIdentity();
localAnchorB.SetZero();
localRotationB.SetIdentity();
referenceRotation.SetIdentity();
enableLimit = false;
lowerAngle = 0.0f;
upperAngle = 0.0f;
@ -40,19 +42,28 @@ struct b3RevoluteJointDef : public b3JointDef
// Initialize this definition from hinge axis, anchor point, and the lower and upper angle limits in radians.
void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& axis, const b3Vec3& anchor, float32 lowerAngle, float32 upperAngle);
// The joint frame relative body A's frame.
b3Transform localFrameA;
// The joint frame relative body B's frame.
b3Transform localFrameB;
// The joint anchor relative body A's origin.
b3Vec3 localAnchorA;
// The joint orientation relative body A's orientation.
b3Quat localRotationA;
// The joint anchor relative body B's origin.
b3Vec3 localAnchorB;
// The joint orientation relative body B's orientation.
b3Quat localRotationB;
// The initial relative rotation from body A to body B.
b3Quat referenceRotation;
// Enable the joint limit.
bool enableLimit;
// The lower angle limit in radians.
// The hinge lower angle limit in radians.
float32 lowerAngle;
// The upper angle limit in radians.
// The hinge upper angle limit in radians.
float32 upperAngle;
// Enable the joint motor.
@ -67,7 +78,7 @@ struct b3RevoluteJointDef : public b3JointDef
// A revolute joint constrains two bodies to share a point and an axis while
// they are free to rotate about the point and the axis.
// The relative rotation about the shared axis is the joint angle.
// The relative rotation about the shared axis is the joint rotation.
// You can limit the relative rotation with a lower and upper angle limit.
// You can use a motor to drive the relative rotation about the shared axis.
// A maximum motor torque is provided so that infinite forces are not generated.
@ -82,10 +93,10 @@ public:
b3Transform GetFrameB() const;
// Get the joint frame relative body A's frame.
const b3Transform& GetLocalFrameA() const;
b3Transform GetLocalFrameA() const;
// Get the joint frame relative body B's frame.
const b3Transform& GetLocalFrameB() const;
b3Transform GetLocalFrameB() const;
// Is the joint limit enabled?
bool IsLimitEnabled() const;
@ -135,8 +146,13 @@ private:
virtual bool SolvePositionConstraints(const b3SolverData* data);
// Solver shared
b3Transform m_localFrameA;
b3Transform m_localFrameB;
b3Quat m_referenceRotation;
b3Vec3 m_localAnchorA;
b3Quat m_localRotationA;
b3Vec3 m_localAnchorB;
b3Quat m_localRotationB;
bool m_enableMotor;
float32 m_motorSpeed;
@ -155,24 +171,31 @@ private:
b3Mat33 m_iB;
b3Vec3 m_localCenterA;
b3Vec3 m_localCenterB;
// Motor
// The limit axis is the same as the motor axis
// Hinge motor
b3Vec3 m_motor_J1; // 1x3 (row)
b3Vec3 m_motor_J2; // 1x3 (row)
float32 m_motorMass;
float32 m_motorImpulse;
// Limit
b3Vec3 m_limitAxis; // axis of rotation for limit contraint
float32 m_limitImpulse;
// Hinge limit
// The limit axis and constraint space mass are the same as the motor's
b3LimitState m_limitState; // constraint state
float32 m_limitImpulse;
// Point-to-point + axes-to-axes
// Spherical
b3Vec3 m_rA;
b3Vec3 m_rB;
b3Vec3 m_nA;
b3Vec3 m_nB;
b3Mat<5, 5> m_mass; // block solver
b3Vec<5> m_impulse; // block solver
b3Mat33 m_mass;
b3Vec3 m_impulse;
// Hinge
b3Mat23 m_J1;
b3Mat23 m_J2;
b3Mat32 m_J1T;
b3Mat32 m_J2T;
b3Mat22 m_K;
b3Vec2 m_axisImpulse;
};
#endif

14
include/bounce/dynamics/joints/weld_joint.h
View File

@ -28,7 +28,7 @@ struct b3WeldJointDef : public b3JointDef
type = e_weldJoint;
localAnchorA.SetZero();
localAnchorB.SetZero();
relativeRotation.SetIdentity();
referenceRotation.SetIdentity();
}
// Initialize this definition from bodies and world anchor point.
@ -41,7 +41,7 @@ struct b3WeldJointDef : public b3JointDef
b3Vec3 localAnchorB;
// The initial relative rotation from body A to body B.
b3Quat relativeRotation;
b3Quat referenceRotation;
};
// A weld joint removes the relative movement between two bodies.
@ -73,8 +73,7 @@ private:
// Solver shared
b3Vec3 m_localAnchorA;
b3Vec3 m_localAnchorB;
b3Quat m_dq0;
b3Quat m_referenceRotation;
// Solver temp
u32 m_indexA;
@ -92,8 +91,13 @@ private:
b3Vec3 m_impulse;
b3Mat33 m_mass;
// Weld constraint
b3Mat33 m_J1;
b3Mat33 m_J2;
b3Mat33 m_J1T;
b3Mat33 m_J2T;
b3Vec3 m_axisImpulse;
//b3Vec3 m_velocityBias;
b3Mat33 m_K;
};
#endif

1
include/bounce/dynamics/shapes/capsule_shape.h
View File

@ -21,7 +21,6 @@
#include <bounce/dynamics/shapes/shape.h>
// A capsule defined along the up-axis.
class b3CapsuleShape : public b3Shape
{
public:

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

@ -21,7 +21,7 @@
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/collision/distance.h>
#include <bounce/collision/collision.h>
struct b3ContactEdge;