fix capsule to hull contact generation, add weld joint with an alternative constraint model
This commit is contained in:
@ -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,
|
||||
|
@ -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;
|
||||
|
@ -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
229
src/bounce/dynamics/joints/weld_joint.cpp
Normal 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);
|
||||
}
|
Reference in New Issue
Block a user