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

improve friction quality, add shader-only support, improve debug drawing facilities, fix couple things

Browse Source
This commit is contained in:
Irlan
2017-02-07 14:31:52 -02:00
parent d59b67c3c3
commit a346a1472d
30 changed files with 1795 additions and 20880 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/bounce/cloth/cloth.cpp
View File

@@ -385,44 +385,16 @@ void b3Cloth::Draw(b3Draw* draw) const
b3Particle* p2 = m_ps + t->v2;
b3Particle* p3 = m_ps + t->v3;
b3Vec3 vs1[3];
vs1[0] = p1->p;
vs1[1] = p2->p;
vs1[2] = p3->p;
b3Vec3 v1 = p1->p;
b3Vec3 v2 = p2->p;
b3Vec3 v3 = p3->p;
draw->DrawPolygon(vs1, 3, color4);
draw->DrawSolidPolygon(vs1, 3, color3);
b3Vec3 vs2[3];
vs2[0] = p1->p;
vs2[1] = p3->p;
vs2[2] = p2->p;
draw->DrawPolygon(vs2, 3, color4);
draw->DrawSolidPolygon(vs2, 3, color3);
}
#if 0
for (u32 i = 0; i < m_c2Count; ++i)
{
b3C2* c = m_c2s + i;
b3Particle* p1 = m_ps + c->i1;
b3Particle* p2 = m_ps + c->i2;
b3Particle* p3 = m_ps + c->i3;
b3Particle* p4 = m_ps + c->i4;
b3Vec3 c1 = (p1->p + p2->p + p3->p) / 3.0f;
b3Vec3 n1 = b3Cross(p2->p - p1->p, p3->p - p1->p);
b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
n1.Normalize();
draw->DrawSegment(c1, c1 + n1, color1);
b3Vec3 n2 = -n1;
b3Vec3 c2 = (p1->p + p4->p + p2->p) / 3.0f;
b3Vec3 n2 = b3Cross(p2->p - p1->p, p4->p - p1->p);
n2.Normalize();
draw->DrawSegment(c2, c2 + n2, color1);
draw->DrawSolidTriangle(n1, v1, v2, v3, color3);
draw->DrawSolidTriangle(n2, v1, v3, v2, color3);
}
#endif
}

10
src/bounce/collision/trees/static_tree.cpp
View File

@@ -60,10 +60,16 @@ struct b3SortPredicate
}
};
void b3StaticTree::Build(u32* ids, const b3AABB3* set, u32 n)
void b3StaticTree::Build(const b3AABB3* set, u32 n)
{
B3_ASSERT(n > 0);
u32* ids = (u32*)b3Alloc(n * sizeof(u32));
for (u32 i = 0; i < n; ++i)
{
ids[i] = i;
}
// Leafs = n, Internals = n - 1, Total = 2n - 1, if we assume
// each leaf node contains exactly 1 object.
const u32 kMinObjectsPerLeaf = 1;
@@ -184,6 +190,8 @@ void b3StaticTree::Build(u32* ids, const b3AABB3* set, u32 n)
}
}
b3Free(ids);
B3_ASSERT(leafCount == leafCapacity);
B3_ASSERT(internalCount == internalCapacity);
B3_ASSERT(m_nodeCount == nodeCapacity);

88
src/bounce/dynamics/contacts/contact_solver.cpp
View File

@@ -22,6 +22,9 @@
#include <bounce/dynamics/body.h>
#include <bounce/common/memory/stack_allocator.h>
// Turn on or off central friction. This is an important optimization for old hardwares.
#define B3_CENTRAL_FRICTION 0
// This solver implements PGS for solving velocity constraints and
// NGS for solving position constraints.
@@ -138,6 +141,7 @@ void b3ContactSolver::InitializeConstraints()
pcp->localPointB = cp->localPoint2;
vcp->normalImpulse = cp->normalImpulse;
vcp->tangentImpulse = cp->tangentImpulse;
}
}
}
@@ -225,8 +229,43 @@ void b3ContactSolver::InitializeConstraints()
vcp->velocityBias = -vc->restitution * vn;
}
}
#if B3_CENTRAL_FRICTION == 0
// Add friction constraints.
{
vcp->tangent1 = vcm->tangent1;
vcp->tangent2 = vcm->tangent2;
b3Vec3 t1 = vcp->tangent1;
b3Vec3 t2 = vcp->tangent2;
// Compute effective mass.
// Identities used:
// I = I^T because I is symmetric. Its inverse is also symmetric.
// dot(a * u, b * v) = a * b * dot(u, v)
// dot(t1, t2) = 0
b3Vec3 rt1A = b3Cross(rA, t1);
b3Vec3 rt1B = b3Cross(rB, t1);
b3Vec3 rt2A = b3Cross(rA, t2);
b3Vec3 rt2B = b3Cross(rB, t2);
float32 kTan11 = mA + mB + b3Dot(iA * rt1A, rt1A) + b3Dot(iB * rt2B, rt2B);
float32 kTan12 = b3Dot(iA * rt1A, rt2A) + b3Dot(iB * rt1B, rt2B);
float32 kTan21 = kTan12;
float32 kTan22 = mA + mB + b3Dot(iA * rt2A, rt2A) + b3Dot(iB * rt2B, rt2B);
b3Mat22 K;
K.x.x = kTan11;
K.x.y = kTan12;
K.y.x = kTan21;
K.y.y = kTan22;
vcp->tangentMass = K;
}
#endif
}
#if B3_CENTRAL_FRICTION == 1
if (pointCount > 0)
{
b3Vec3 rA = vcm->center - xA;
@@ -235,7 +274,7 @@ void b3ContactSolver::InitializeConstraints()
vcm->rA = rA;
vcm->rB = rB;
// Add friction constraint.
// Add friction constraints.
{
b3Vec3 t1 = vcm->tangent1;
b3Vec3 t2 = vcm->tangent2;
@@ -270,6 +309,7 @@ void b3ContactSolver::InitializeConstraints()
vcm->motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
}
}
#endif
}
}
}
@@ -304,7 +344,10 @@ void b3ContactSolver::WarmStart()
{
b3VelocityConstraintPoint* vcp = vcm->points + k;
b3Vec3 P = vcp->normalImpulse * vcp->normal;
b3Vec3 P1 = vcp->normalImpulse * vcp->normal;
b3Vec3 P2 = vcp->tangentImpulse.x * vcp->tangent1;
b3Vec3 P3 = vcp->tangentImpulse.y * vcp->tangent2;
b3Vec3 P = P1 + P2 + P3;
vA -= mA * P;
wA -= iA * b3Cross(vcp->rA, P);
@@ -313,18 +356,20 @@ void b3ContactSolver::WarmStart()
wB += iB * b3Cross(vcp->rB, P);
}
#if B3_CENTRAL_FRICTION == 1
if (pointCount > 0)
{
b3Vec3 P1 = vcm->tangentImpulse.x * vcm->tangent1;
b3Vec3 P2 = vcm->tangentImpulse.y * vcm->tangent2;
b3Vec3 P3 = vcm->motorImpulse * vcm->normal;
vA -= mA * (P1 + P2);
wA -= iA * (b3Cross(vcm->rA, P1 + P2) + P3);
vB += mB * (P1 + P2);
wB += iB * (b3Cross(vcm->rB, P1 + P2) + P3);
}
#endif
}
m_velocities[indexA].v = vA;
@@ -387,8 +432,43 @@ void b3ContactSolver::SolveVelocityConstraints()
normalImpulse += vcp->normalImpulse;
}
#if B3_CENTRAL_FRICTION == 0
// Solve tangent constraints.
{
b3Vec3 dv = vB + b3Cross(wB, vcp->rB) - vA - b3Cross(wA, vcp->rA);
b3Vec2 Cdot;
Cdot.x = b3Dot(dv, vcp->tangent1);
Cdot.y = b3Dot(dv, vcp->tangent2);
b3Vec2 impulse = vcp->tangentMass.Solve(-Cdot);
b3Vec2 oldImpulse = vcp->tangentImpulse;
vcp->tangentImpulse += impulse;
float32 maxImpulse = vc->friction * vcp->normalImpulse;
if (b3Dot(vcp->tangentImpulse, vcp->tangentImpulse) > maxImpulse * maxImpulse)
{
vcp->tangentImpulse.Normalize();
vcp->tangentImpulse *= maxImpulse;
}
impulse = vcp->tangentImpulse - oldImpulse;
b3Vec3 P1 = impulse.x * vcp->tangent1;
b3Vec3 P2 = impulse.y * vcp->tangent2;
b3Vec3 P = P1 + P2;
vA -= mA * P;
wA -= iA * b3Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b3Cross(vcp->rB, P);
}
#endif
}
#if B3_CENTRAL_FRICTION == 1
if (pointCount > 0)
{
// Solve tangent constraints.
@@ -438,6 +518,7 @@ void b3ContactSolver::SolveVelocityConstraints()
wB += iB * P;
}
}
#endif
}
m_velocities[indexA].v = vA;
@@ -471,6 +552,7 @@ void b3ContactSolver::StoreImpulses()
b3ManifoldPoint* cp = m->points + k;
b3VelocityConstraintPoint* vcp = vcm->points + k;
cp->normalImpulse = vcp->normalImpulse;
cp->tangentImpulse = vcp->tangentImpulse;
}
}
}

3
src/bounce/dynamics/contacts/manifold.cpp
View File

@@ -27,6 +27,7 @@ void b3Manifold::GuessImpulses()
{
b3ManifoldPoint* p = points + i;
p->normalImpulse = 0.0f;
p->tangentImpulse.SetZero();
p->persisting = 0;
}
}
@@ -45,6 +46,7 @@ void b3Manifold::FindImpulses(const b3Manifold& oldManifold)
if (p2->triangleKey == p1->triangleKey && p2->key == p1->key)
{
p2->normalImpulse = p1->normalImpulse;
p2->tangentImpulse = p1->tangentImpulse;
p2->persisting = 1;
break;
}
@@ -81,6 +83,7 @@ void b3WorldManifold::Initialize(const b3Manifold* manifold,
{
const b3ManifoldPoint* mp = manifold->points + i;
b3WorldManifoldPoint* wmp = points + i;
wmp->Initialize(mp, xfA, radiusA, xfB, radiusB);
}
}

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

@@ -122,7 +122,7 @@ void b3World::DebugDraw() const
if (flags & b3Draw::e_contactPointsFlag)
{
m_debugDraw->DrawPoint(p, yellow);
m_debugDraw->DrawPoint(p, 4.0f, yellow);
}
if (flags & b3Draw::e_contactNormalsFlag)
@@ -143,18 +143,28 @@ void b3World::DebugDraw() const
const b3WorldManifoldPoint* wmp = wm.points + j;
b3Vec3 n = wmp->normal;
b3Vec3 t1 = wm.tangent1;
b3Vec3 t2 = wm.tangent2;
b3Vec3 p = wmp->point;
float32 Pn = mp->normalImpulse;
float32 Pt1 = mp->tangentImpulse.x;
float32 Pt2 = mp->tangentImpulse.y;
if (flags & b3Draw::e_contactPointsFlag)
{
m_debugDraw->DrawPoint(p, mp->persisting ? green : red);
m_debugDraw->DrawPoint(p, 4.0f, mp->persisting ? green : red);
}
if (flags & b3Draw::e_contactNormalsFlag)
{
m_debugDraw->DrawSegment(p, p + n, white);
}
if (flags & b3Draw::e_contactTangentsFlag)
{
m_debugDraw->DrawSegment(p, p + t1, yellow);
m_debugDraw->DrawSegment(p, p + t2, yellow);
}
}
}
}
@@ -168,44 +178,34 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const
case e_sphereShape:
{
const b3SphereShape* sphere = (b3SphereShape*)shape;
b3Vec3 c = xf * sphere->m_center;
m_debugDraw->DrawPoint(c, wireColor);
b3Vec3 p = xf * sphere->m_center;
m_debugDraw->DrawPoint(p, 4.0f, wireColor);
break;
}
case e_capsuleShape:
{
const b3CapsuleShape* capsule = (b3CapsuleShape*)shape;
b3Vec3 c1 = xf * capsule->m_centers[0];
b3Vec3 c2 = xf * capsule->m_centers[1];
m_debugDraw->DrawPoint(c1, wireColor);
m_debugDraw->DrawPoint(c2, wireColor);
m_debugDraw->DrawSegment(c1, c2, wireColor);
b3Vec3 p1 = xf * capsule->m_centers[0];
b3Vec3 p2 = xf * capsule->m_centers[1];
m_debugDraw->DrawPoint(p1, 4.0f, wireColor);
m_debugDraw->DrawPoint(p2, 4.0f, wireColor);
m_debugDraw->DrawSegment(p1, p2, wireColor);
break;
}
case e_hullShape:
{
const b3HullShape* hs = (b3HullShape*)shape;
const b3Hull* hull = hs->m_hull;
for (u32 i = 0; i < hull->faceCount; ++i)
for (u32 i = 0; i < hull->edgeCount; i += 2)
{
b3Vec3 polygon[B3_MAX_HULL_FEATURES];
u32 vCount = 0;
const b3HalfEdge* edge = hull->GetEdge(i);
const b3HalfEdge* twin = hull->GetEdge(i + 1);
b3Vec3 p1 = xf * hull->vertices[edge->origin];
b3Vec3 p2 = xf * hull->vertices[twin->origin];
// Build convex polygon for loop
const b3Face* face = hull->GetFace(i);
const b3HalfEdge* begin = hull->GetEdge(face->edge);
const b3HalfEdge* edge = begin;
do
{
polygon[vCount++] = xf * hull->GetVertex(edge->origin);
edge = hull->GetEdge(edge->next);
} while (edge != begin);
m_debugDraw->DrawPolygon(polygon, vCount, wireColor);
m_debugDraw->DrawSegment(p1, p2, wireColor);
}
break;
}
case e_meshShape:
@@ -214,14 +214,13 @@ void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const
const b3Mesh* mesh = ms->m_mesh;
for (u32 i = 0; i < mesh->triangleCount; ++i)
{
const b3Triangle* triangle = mesh->triangles + i;
const b3Triangle* t = mesh->triangles + i;
b3Vec3 vs[3];
vs[0] = xf * mesh->vertices[triangle->v1];
vs[1] = xf * mesh->vertices[triangle->v2];
vs[2] = xf * mesh->vertices[triangle->v3];
b3Vec3 p1 = xf * mesh->vertices[t->v1];
b3Vec3 p2 = xf * mesh->vertices[t->v2];
b3Vec3 p3 = xf * mesh->vertices[t->v3];
m_debugDraw->DrawPolygon(vs, 3, wireColor);
m_debugDraw->DrawTriangle(p1, p2, p3, wireColor);
}
break;
}

4
src/bounce/dynamics/joints/mouse_joint.cpp
View File

@@ -116,7 +116,7 @@ void b3MouseJoint::Draw(b3Draw* draw) const
b3Vec3 a = GetAnchorA();
b3Vec3 b = GetAnchorB();
draw->DrawPoint(a, green);
draw->DrawPoint(b, red);
draw->DrawPoint(a, 4.0f, green);
draw->DrawPoint(b, 4.0f, red);
draw->DrawSegment(a, b, yellow);
}

4
src/bounce/dynamics/joints/sphere_joint.cpp
View File

@@ -162,7 +162,7 @@ void b3SphereJoint::Draw(b3Draw* draw) const
b3Vec3 a = GetAnchorA();
b3Vec3 b = GetAnchorB();
draw->DrawPoint(a, b3Color(1.0f, 0.0f, 0.0f));
draw->DrawPoint(b, b3Color(0.0f, 1.0f, 0.0f));
draw->DrawPoint(a, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
draw->DrawPoint(b, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
draw->DrawSegment(a, b, b3Color(1.0f, 1.0f, 0.0f));
}

4
src/bounce/dynamics/joints/spring_joint.cpp
View File

@@ -261,7 +261,7 @@ void b3SpringJoint::Draw(b3Draw* draw) const
b3Vec3 a = GetBodyA()->GetWorldPoint(m_localAnchorA);
b3Vec3 b = GetBodyB()->GetWorldPoint(m_localAnchorB);
draw->DrawPoint(a, green);
draw->DrawPoint(b, green);
draw->DrawPoint(a, 4.0f, red);
draw->DrawPoint(b, 4.0f, green);
draw->DrawSegment(a, b, blue);
}

33
src/bounce/dynamics/shapes/capsule_shape.cpp
View File

@@ -76,8 +76,8 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
float32 volume = (2.0f / 3.0f) * B3_PI * r3;
float32 mass = density * volume;
// Ic = Io + m * d^2
// Io = Ic - m * d^2
// I = Ic + m * d^2
// Ic = I - m * d^2
// Hemisphere inertia about the origin
float32 Io = (2.0f / 5.0f) * mass * r2;
@@ -126,6 +126,8 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
massData->center = Ic_Capsule.center;
massData->mass = Ic_Capsule.mass;
massData->I = Ic;
}
void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const
@@ -205,6 +207,33 @@ bool b3CapsuleShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& inpu
// Check for short segment.
if (dd < B3_EPSILON * B3_EPSILON)
{
float32 a = nn;
b3Vec3 m = A - P;
float32 b = b3Dot(m, n);
float32 c = b3Dot(m, m) - m_radius * m_radius;
float32 disc = b * b - a * c;
// Check for negative discriminant.
if (disc < 0.0f)
{
return false;
}
// Find the minimum time of impact of the line with the sphere.
float32 t = -b - b3Sqrt(disc);
// Is the intersection point on the segment?
if (t > 0.0f && t <= input.maxFraction * a)
{
// Finish solution.
t /= a;
output->fraction = t;
output->normal = b3Normalize(m + t * n);
return true;
}
return false;
}

1
src/bounce/dynamics/shapes/hull_shape.cpp
View File

@@ -18,7 +18,6 @@
#include <bounce/dynamics/shapes/hull_shape.h>
#include <bounce/collision/shapes/hull.h>
#include <bounce/common/template/array.h>
b3HullShape::b3HullShape()
{

24
src/bounce/quickhull/qh_hull.cpp
View File

@@ -747,26 +747,28 @@ void qhHull::Draw(b3Draw* draw) const
b3Vec3 c = face->center;
b3Vec3 n = face->plane.normal;
b3StackArray<b3Vec3, 32> vs;
const qhHalfEdge* begin = face->edge;
const qhHalfEdge* edge = begin;
do
{
vs.PushBack(edge->tail->position);
edge = edge->next;
} while (edge != begin);
draw->DrawSolidPolygon(n, vs.Elements(), vs.Count(), b3Color(1.0f, 1.0f, 1.0f, 0.5f));
qhVertex* v = face->conflictList.head;
while (v)
{
draw->DrawPoint(v->position, b3Color(1.0f, 1.0f, 0.0f));
draw->DrawPoint(v->position, 4.0f, b3Color(1.0f, 1.0f, 0.0f));
draw->DrawSegment(c, v->position, b3Color(1.0f, 1.0f, 0.0f));
v = v->next;
}
draw->DrawSegment(c, c + n, b3Color(1.0f, 1.0f, 1.0f));
b3StackArray<b3Vec3, 32> polygon;
qhHalfEdge* edge = face->edge;
do
{
polygon.PushBack(edge->tail->position);
edge = edge->next;
} while (edge != face->edge);
draw->DrawSolidPolygon(polygon.Elements(), polygon.Count(), b3Color(0.0f, 0.0f, 1.0f, 1.0f));
face = face->next;
}
}