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

fix capsule to hull contact generation, add weld joint with an alternative constraint model

Browse Source
This commit is contained in:
Irlan 2017-03-06 13:34:24 -03:00
parent 03ebca5728
commit c581dee66e
12 changed files with 492 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/testbed/framework/test_entries.cpp
View File

@ -30,6 +30,7 @@
#include <testbed/tests/multiple_shapes.h>
#include <testbed/tests/quadric_shapes.h>
#include <testbed/tests/spring.h>
#include <testbed/tests/weld_test.h>
#include <testbed/tests/newton_cradle.h>
#include <testbed/tests/hinge_motor.h>
#include <testbed/tests/hinge_chain.h>
@ -65,6 +66,7 @@ TestEntry g_tests[] =
{ "Multiple Shapes", &MultipleShapes::Create },
{ "Quadric Shapes", &QuadricShapes::Create },
{ "Springs", &Spring::Create },
{ "Weld Test", &WeldTest::Create },
{ "Newton's Cradle", &NewtonCradle::Create },
{ "Hinge Motor", &HingeMotor::Create },
{ "Hinge Chain", &HingeChain::Create },

94
examples/testbed/tests/weld_test.h Normal file
View File

@ -0,0 +1,94 @@
/*
* 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 WELD_TEST_H
#define WELD_TEST_H
class WeldTest : public Test
{
public:
WeldTest()
{
{
b3BodyDef bd;
b3Body* ground = m_world.CreateBody(bd);
b3HullShape shape;
shape.m_hull = &m_groundHull;
b3ShapeDef sd;
sd.shape = &shape;
ground->CreateShape(sd);
}
b3Body* hinge, *door;
{
b3BodyDef bd;
bd.type = b3BodyType::e_dynamicBody;
bd.position.Set(-2.0f, 5.05f, 0.0f);
hinge = m_world.CreateBody(bd);
b3CapsuleShape shape;
shape.m_centers[0].Set(0.0f, -3.5f, 0.0f);
shape.m_centers[1].Set(0.0f, 3.5f, 0.0f);
shape.m_radius = 0.5f;
b3ShapeDef sd;
sd.shape = &shape;
sd.density = 1.0f;
hinge->CreateShape(sd);
}
{
b3BodyDef bd;
bd.type = b3BodyType::e_dynamicBody;
bd.orientation.Set(b3Vec3(1.0f, 0.0f, 0.0f), 0.25f * B3_PI);
bd.position.Set(1.0f, 5.05f, 0.0f);
door = m_world.CreateBody(bd);
b3HullShape hull;
hull.m_hull = &m_doorHull;
b3ShapeDef sdef;
sdef.shape = &hull;
sdef.density = 2.0f;
door->CreateShape(sdef);
}
{
b3Vec3 hingeAnchor(-2.0f, 5.0f, 0.0f);
b3WeldJointDef jd;
jd.Initialize(hinge, door, hingeAnchor);
b3WeldJoint* wj = (b3WeldJoint*)m_world.CreateJoint(jd);
}
}
static Test* Create()
{
return new WeldTest();
}
};
#endif

1
include/bounce/bounce.h
View File

@ -41,6 +41,7 @@
#include <bounce/dynamics/joints/mouse_joint.h>
#include <bounce/dynamics/joints/spring_joint.h>
#include <bounce/dynamics/joints/weld_joint.h>
#include <bounce/dynamics/joints/sphere_joint.h>
#include <bounce/dynamics/joints/revolute_joint.h>
#include <bounce/dynamics/joints/cone_joint.h>

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

@ -243,6 +243,7 @@ private:
friend class b3JointSolver;
friend class b3MouseJoint;
friend class b3SpringJoint;
friend class b3WeldJoint;
friend class b3RevoluteJoint;
friend class b3SphereJoint;
friend class b3ConeJoint;

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

@ -33,6 +33,7 @@ enum b3JointType
e_unknownJoint,
e_mouseJoint,
e_springJoint,
e_weldJoint,
e_revoluteJoint,
e_sphereJoint,
e_coneJoint,

99
include/bounce/dynamics/joints/weld_joint.h Normal file
View File

@ -0,0 +1,99 @@
/*
* 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_WELD_JOINT_H
#define B3_WELD_JOINT_H
#include <bounce/dynamics/joints/joint.h>
struct b3WeldJointDef : public b3JointDef
{
b3WeldJointDef()
{
type = e_weldJoint;
localAnchorA.SetZero();
localAnchorB.SetZero();
relativeRotation.SetIdentity();
}
// Initialize this definition from bodies and world anchor point.
void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& anchor);
// The joint anchor relative body A's origin.
b3Vec3 localAnchorA;
// The joint anchor relative body B's origin.
b3Vec3 localAnchorB;
// The initial relative rotation from body A to body B.
b3Quat relativeRotation;
};
// A weld joint removes the relative rotation between two bodies.
// You need to specify the relative rotation and the local anchor points.
// @todo Soft this constraint.
class b3WeldJoint : public b3Joint
{
public:
// Get the anchor point on body A in world coordinates.
b3Vec3 GetAnchorA() const;
// Get the anchor point on body B in world coordinates.
b3Vec3 GetAnchorB() const;
// Draw this joint.
void Draw(b3Draw* draw) const;
private:
friend class b3Joint;
friend class b3JointManager;
friend class b3JointSolver;
b3WeldJoint(const b3WeldJointDef* def);
virtual void InitializeConstraints(const b3SolverData* data);
virtual void WarmStart(const b3SolverData* data);
virtual void SolveVelocityConstraints(const b3SolverData* data);
virtual bool SolvePositionConstraints(const b3SolverData* data);
// Solver shared
b3Vec3 m_localAnchorA;
b3Vec3 m_localAnchorB;
b3Quat m_dq0;
// Solver temp
u32 m_indexA;
u32 m_indexB;
float32 m_mA;
float32 m_mB;
b3Mat33 m_iA;
b3Mat33 m_iB;
b3Vec3 m_localCenterA;
b3Vec3 m_localCenterB;
// Point-to-point
b3Vec3 m_rA;
b3Vec3 m_rB;
b3Vec3 m_impulse;
b3Mat33 m_mass;
b3Vec3 m_axisImpulse;
//b3Vec3 m_velocityBias;
};
#endif

4
premake5.lua
View File

@ -11,8 +11,8 @@ bounce_inc_dir = "include/"
bounce_src_dir = "src/"
examples_inc_dir = "examples/"
examples_src_dir = "examples/"
tests_inc_dir = "tests/"
tests_src_dir = "tests/"
tests_inc_dir = "test/"
tests_src_dir = "test/"
obj_dir = "/obj/"
bin_dir = "/bin/"

81
src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp
View File

@ -50,7 +50,7 @@ void b3BuildEdgeContact(b3Manifold& manifold,
}
b3Vec3 PA, PB;
b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, E1, P2, E2);
b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, N1, P2, N2);
b3FeaturePair pair = b3MakePair(0, 1, index2, index2 + 1);
@ -82,52 +82,55 @@ void b3BuildFaceContact(b3Manifold& manifold,
b3ClipVertex clipEdge1[2];
u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, xf2, index2, hull2);
// Project clipped edge 1 onto face plane 2.
float32 r1 = hull1->radius;
float32 r2 = B3_HULL_RADIUS;
float32 totalRadius = r1 + r2;
b3Plane localPlane2 = hull2->GetPlane(index2);
b3Plane plane2 = b3Mul(xf2, localPlane2);
const b3Face* face2 = hull2->GetFace(index2);
const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge);
b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin);
b3Vec3 normal = -plane2.normal;
// Project.
if (clipCount == 2)
b3Vec3 center;
center.SetZero();
u32 pointCount = 0;
for (u32 i = 0; i < clipCount; ++i)
{
float32 r1 = hull1->radius;
float32 r2 = B3_HULL_RADIUS;
float32 totalRadius = r1 + r2;
b3Plane localPlane2 = hull2->GetPlane(index2);
b3Plane plane2 = b3Mul(xf2, localPlane2);
const b3Face* face2 = hull2->GetFace(index2);
const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge);
b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin);
b3Vec3 cp1 = b3ClosestPointOnPlane(clipEdge1[0].position, plane2);
b3Vec3 cp2 = b3ClosestPointOnPlane(clipEdge1[1].position, plane2);
float32 s1 = b3Distance(clipEdge1[0].position, plane2);
float32 s2 = b3Distance(clipEdge1[1].position, plane2);
if (s1 <= totalRadius && s2 <= totalRadius)
float32 s = b3Distance(clipEdge1[i].position, plane2);
if (s <= totalRadius)
{
b3Vec3 normal = -plane2.normal;
b3Vec3 p1 = 0.5f * (clipEdge1[0].position + r1 * normal + cp1 - r2 * normal);
b3Vec3 p2 = 0.5f * (clipEdge1[1].position + r1 * normal + cp2 - r2 * normal);
b3Vec3 cp = b3ClosestPointOnPlane(clipEdge1[i].position, plane2);
b3Vec3 p = 0.5f * (clipEdge1[i].position + r1 * normal + cp - r2 * normal);
manifold.pointCount = 2;
b3ManifoldPoint* mp = manifold.points + pointCount;
mp->triangleKey = B3_NULL_TRIANGLE;
mp->key = b3MakeKey(clipEdge1[i].pair);
mp->localNormal = b3MulT(xf1.rotation, normal);
mp->localPoint = b3MulT(xf1, clipEdge1[i].position);
mp->localPoint2 = b3MulT(xf2, cp);
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(clipEdge1[0].pair);
manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint = b3MulT(xf1, clipEdge1[0].position);
manifold.points[0].localPoint2 = b3MulT(xf2, cp1);
++pointCount;
manifold.points[1].triangleKey = B3_NULL_TRIANGLE;
manifold.points[1].key = b3MakeKey(clipEdge1[1].pair);
manifold.points[1].localNormal = b3MulT(xf1.rotation, normal);
manifold.points[1].localPoint = b3MulT(xf1, clipEdge1[1].position);
manifold.points[1].localPoint2 = b3MulT(xf2, cp2);
manifold.center = 0.5f * (p1 + p2);
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
center += p;
}
}
if (pointCount > 0)
{
center /= pointCount;
manifold.center = center;
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
manifold.pointCount = pointCount;
}
}
void b3CollideCapsuleAndHull(b3Manifold& manifold,

30
src/bounce/dynamics/draw_world.cpp
View File

@ -16,29 +16,7 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
#include <bounce/common/draw.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/body.h>
#include <bounce/dynamics/contacts/convex_contact.h>
#include <bounce/dynamics/contacts/mesh_contact.h>
#include <bounce/dynamics/shapes/shape.h>
#include <bounce/dynamics/shapes/sphere_shape.h>
#include <bounce/dynamics/shapes/capsule_shape.h>
#include <bounce/dynamics/shapes/hull_shape.h>
#include <bounce/dynamics/shapes/mesh_shape.h>
#include <bounce/dynamics/joints/mouse_joint.h>
#include <bounce/dynamics/joints/spring_joint.h>
#include <bounce/dynamics/joints/revolute_joint.h>
#include <bounce/dynamics/joints/sphere_joint.h>
#include <bounce/dynamics/joints/cone_joint.h>
#include <bounce/collision/shapes/sphere.h>
#include <bounce/collision/shapes/capsule.h>
#include <bounce/collision/shapes/hull.h>
#include <bounce/collision/shapes/mesh.h>
#include <bounce/bounce.h>
const b3Color b3Color_black(0.0f, 0.0f, 0.0f);
const b3Color b3Color_white(1.0f, 1.0f, 1.0f);
@ -251,6 +229,12 @@ void b3World::DrawJoint(const b3Joint* joint) const
o->Draw(b3_debugDraw);
break;
}
case e_weldJoint:
{
b3WeldJoint* o = (b3WeldJoint*)joint;
o->Draw(b3_debugDraw);
break;
}
case e_revoluteJoint:
{
b3RevoluteJoint* o = (b3RevoluteJoint*)joint;

15
src/bounce/dynamics/joints/joint.cpp
View File

@ -19,6 +19,7 @@
#include <bounce/dynamics/joints/joint.h>
#include <bounce/dynamics/joints/mouse_joint.h>
#include <bounce/dynamics/joints/spring_joint.h>
#include <bounce/dynamics/joints/weld_joint.h>
#include <bounce/dynamics/joints/revolute_joint.h>
#include <bounce/dynamics/joints/sphere_joint.h>
#include <bounce/dynamics/joints/cone_joint.h>
@ -40,7 +41,12 @@ b3Joint* b3Joint::Create(const b3JointDef* def)
joint = new (block) b3SpringJoint((b3SpringJointDef*)def);
break;
}
case e_revoluteJoint:
case e_weldJoint:
{
void* block = b3Alloc(sizeof(b3WeldJoint));
joint = new (block) b3WeldJoint((b3WeldJointDef*)def);
break;
}case e_revoluteJoint:
{
void* block = b3Alloc(sizeof(b3RevoluteJoint));
joint = new (block) b3RevoluteJoint((b3RevoluteJointDef*)def);
@ -88,6 +94,13 @@ void b3Joint::Destroy(b3Joint* joint)
b3Free(joint);
break;
}
case e_weldJoint:
{
b3WeldJoint* o = (b3WeldJoint*)joint;
o->~b3WeldJoint();
b3Free(joint);
break;
}
case b3JointType::e_revoluteJoint:
{
b3RevoluteJoint* o = (b3RevoluteJoint*)joint;

229
src/bounce/dynamics/joints/weld_joint.cpp Normal file
View File

@ -0,0 +1,229 @@
/*
* 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.
*/
#include <bounce/dynamics/joints/weld_joint.h>
#include <bounce/dynamics/body.h>
#include <bounce/common/draw.h>
#include <bounce/common/math/mat.h>
void b3WeldJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchor)
{
bodyA = bA;
bodyB = bB;
localAnchorA = bodyA->GetLocalPoint(anchor);
localAnchorB = bodyB->GetLocalPoint(anchor);
b3Quat qA = bodyA->GetOrientation();
b3Quat qB = bodyB->GetOrientation();
relativeRotation = b3Conjugate(qA) * qB;
}
b3WeldJoint::b3WeldJoint(const b3WeldJointDef* def)
{
m_type = e_weldJoint;
m_localAnchorA = def->localAnchorA;
m_localAnchorB = def->localAnchorB;
m_dq0 = def->relativeRotation;
m_impulse.SetZero();
m_axisImpulse.SetZero();
}
void b3WeldJoint::InitializeConstraints(const b3SolverData* data)
{
b3Body* m_bodyA = GetBodyA();
b3Body* m_bodyB = GetBodyB();
m_indexA = m_bodyA->m_islandID;
m_indexB = m_bodyB->m_islandID;
m_mA = m_bodyA->m_invMass;
m_mB = m_bodyB->m_invMass;
m_iA = m_bodyA->m_worldInvI;
m_iB = m_bodyB->m_worldInvI;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
b3Quat qA = data->positions[m_indexA].q;
b3Quat qB = data->positions[m_indexB].q;
{
// Compute effective mass for the block solver
m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
b3Mat33 RA = b3Skew(m_rA);
b3Mat33 RAT = b3Transpose(RA);
b3Mat33 RB = b3Skew(m_rB);
b3Mat33 RBT = b3Transpose(RB);
b3Mat33 M = b3Diagonal(m_mA + m_mB);
m_mass = M + RA * m_iA * RAT + RB * m_iB * RBT;
}
}
void b3WeldJoint::WarmStart(const b3SolverData* data)
{
b3Vec3 vA = data->velocities[m_indexA].v;
b3Vec3 wA = data->velocities[m_indexA].w;
b3Vec3 vB = data->velocities[m_indexB].v;
b3Vec3 wB = data->velocities[m_indexB].w;
vA -= m_mA * m_impulse;
wA -= m_iA * (b3Cross(m_rA, m_impulse) + m_axisImpulse);
vB += m_mB * m_impulse;
wB += m_iB * (b3Cross(m_rB, m_impulse) + m_axisImpulse);
data->velocities[m_indexA].v = vA;
data->velocities[m_indexA].w = wA;
data->velocities[m_indexB].v = vB;
data->velocities[m_indexB].w = wB;
}
void b3WeldJoint::SolveVelocityConstraints(const b3SolverData* data)
{
b3Vec3 vA = data->velocities[m_indexA].v;
b3Vec3 wA = data->velocities[m_indexA].w;
b3Vec3 vB = data->velocities[m_indexB].v;
b3Vec3 wB = data->velocities[m_indexB].w;
b3Quat qA = data->positions[m_indexA].q;
b3Quat qB = data->positions[m_indexB].q;
{
b3Vec3 Cdot = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA);
b3Vec3 impulse = m_mass.Solve(-Cdot);
m_impulse += impulse;
vA -= m_mA * impulse;
wA -= m_iA * b3Cross(m_rA, impulse);
vB += m_mB * impulse;
wB += m_iB * b3Cross(m_rB, impulse);
}
{
b3Vec3 Cdot = wB - wA;
b3Vec3 impulse = -Cdot;
m_axisImpulse += impulse;
wA -= m_iA * impulse;
wB += m_iB * impulse;
}
data->velocities[m_indexA].v = vA;
data->velocities[m_indexA].w = wA;
data->velocities[m_indexB].v = vB;
data->velocities[m_indexB].w = wB;
}
bool b3WeldJoint::SolvePositionConstraints(const b3SolverData* data)
{
b3Vec3 xA = data->positions[m_indexA].x;
b3Quat qA = data->positions[m_indexA].q;
b3Vec3 xB = data->positions[m_indexB].x;
b3Quat qB = data->positions[m_indexB].q;
float32 linearError = 0.0f;
{
// Compute effective mass
b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
b3Vec3 rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
b3Mat33 M = b3Diagonal(m_mA + m_mB);
b3Mat33 RA = b3Skew(rA);
b3Mat33 RAT = b3Transpose(RA);
b3Mat33 RB = b3Skew(rB);
b3Mat33 RBT = b3Transpose(RB);
b3Mat33 mass = M + RA * m_iA * RAT + RB * m_iB * RBT;
b3Vec3 C = xB + rB - xA - rA;
b3Vec3 impulse = mass.Solve(-C);
xA -= m_mA * impulse;
qA -= b3Derivative(qA, b3Mul(m_iA, b3Cross(rA, impulse)));
qA.Normalize();
xB += m_mB * impulse;
qB += b3Derivative(qB, b3Mul(m_iB, b3Cross(rB, impulse)));
qB.Normalize();
linearError += b3Length(C);
}
float32 angularError = 0.0f;
{
// qC = inv(dq0) * dq = q_identity
// qB - qA - q_rel0
// Small angle approximation
// C = 2 * sin(theta / 2) = theta
b3Quat dq = b3Conjugate(m_dq0) * b3Conjugate(qA) * qB;
if (dq.w < 0.0f)
{
dq.x = -dq.x;
dq.y = -dq.y;
dq.z = -dq.z;
}
b3Vec3 axis(dq.x, dq.y, dq.z);
axis = b3Mul(qA, axis);
b3Vec3 C = 2.0f * axis;
b3Vec3 P = -C;
qA -= b3Derivative(qA, m_iA * P);
qA.Normalize();
qB += b3Derivative(qB, m_iB * P);
qB.Normalize();
angularError += 2.0f * b3Length(C);
}
data->positions[m_indexA].x = xA;
data->positions[m_indexA].q = qA;
data->positions[m_indexB].x = xB;
data->positions[m_indexB].q = qB;
return linearError <= B3_LINEAR_SLOP && angularError <= B3_ANGULAR_SLOP;
}
b3Vec3 b3WeldJoint::GetAnchorA() const
{
return GetBodyA()->GetWorldPoint(m_localAnchorA);
}
b3Vec3 b3WeldJoint::GetAnchorB() const
{
return GetBodyB()->GetWorldPoint(m_localAnchorB);
}
void b3WeldJoint::Draw(b3Draw* draw) const
{
b3Vec3 a = GetAnchorA();
b3Vec3 b = GetAnchorB();
draw->DrawPoint(a, 4.0f, b3Color_red);
draw->DrawPoint(b, 4.0f, b3Color_green);
draw->DrawSegment(a, b, b3Color_yellow);
}

0
tests/ignore.txt → test/ignore.txt
View File