refactoring

This commit is contained in:
Irlan
2018-10-08 16:18:28 -03:00
parent b48b16843a
commit f98374143a
47 changed files with 895 additions and 1046 deletions

View File

@ -56,23 +56,20 @@
#include <bounce/dynamics/contacts/convex_contact.h>
#include <bounce/dynamics/contacts/mesh_contact.h>
#include <bounce/dynamics/rope/rope.h>
#include <bounce/garment/sewing_pattern.h>
#include <bounce/garment/garment.h>
#include <bounce/garment/garment_mesh.h>
#include <bounce/dynamics/cloth/cloth_mesh.h>
#include <bounce/dynamics/cloth/cloth.h>
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/spring_force.h>
#include <bounce/dynamics/body.h>
#include <bounce/dynamics/world.h>
#include <bounce/dynamics/world_listeners.h>
#include <bounce/controllers/cloth_dragger.h>
#include <bounce/controllers/body_dragger.h>
#include <bounce/rope/rope.h>
#include <bounce/cloth/cloth_mesh.h>
#include <bounce/cloth/cloth.h>
#include <bounce/cloth/particle.h>
#include <bounce/cloth/spring_force.h>
#include <bounce/cloth/garment/sewing_pattern.h>
#include <bounce/cloth/garment/garment.h>
#include <bounce/cloth/garment/garment_mesh.h>
#endif

View File

@ -21,6 +21,7 @@
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/common/memory/stack_allocator.h>
#include <bounce/common/memory/block_pool.h>
class b3World;
@ -83,7 +84,20 @@ struct b3ClothDef
class b3Cloth
{
public:
// Get the world the cloth belongs to.
b3Cloth(const b3ClothDef& def);
~b3Cloth();
// Set the acceleration of gravity.
void SetGravity(const b3Vec3& gravity);
// Get the acceleration of gravity.
b3Vec3 GetGravity() const;
// Attach a world to this cloth.
// The cloth will be able to respond to collisions with the static shapes in the attached world.
void SetWorld(b3World* world);
// Get the world attached to this cloth.
const b3World* GetWorld() const;
b3World* GetWorld();
@ -99,9 +113,6 @@ public:
// Destroy a given force.
void DestroyForce(b3Force* force);
// Perform a ray cast with the cloth.
void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2);
// Perform a ray cast with the cloth.
bool RayCastSingle(b3ClothRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
@ -129,20 +140,14 @@ public:
// Get the next cloth in the world cloth list.
b3Cloth* GetNext();
// Perform a time step.
void Step(float32 dt);
// Debug draw the cloth using the associated cloth mesh.
void Draw() const;
private:
friend class b3World;
friend class b3List2<b3Cloth>;
b3Cloth(const b3ClothDef& def, b3World* world);
~b3Cloth();
// Perform a time step.
// Called only inside b3World.
void Step(float32 dt, const b3Vec3& gravity);
// Compute mass of each particle.
void ComputeMass();
@ -161,6 +166,15 @@ private:
// Solve
void Solve(float32 dt, const b3Vec3& gravity);
// Stack allocator
b3StackAllocator m_stackAllocator;
// The world attached to this cloth
b3World* m_world;
// Gravity acceleration
b3Vec3 m_gravity;
// Proxy mesh
const b3ClothMesh* m_mesh;
@ -196,15 +210,23 @@ private:
// List of triangle contacts
b3List2<b3TriangleContact> m_triangleContactList;
// The parent world of this cloth.
b3World* m_world;
// Links to the world cloth list.
b3Cloth* m_prev;
b3Cloth* m_next;
};
inline void b3Cloth::SetGravity(const b3Vec3& gravity)
{
m_gravity = gravity;
}
inline b3Vec3 b3Cloth::GetGravity() const
{
return m_gravity;
}
inline void b3Cloth::SetWorld(b3World* world)
{
m_world = world;
}
inline const b3World* b3Cloth::GetWorld() const
{
return m_world;
@ -230,14 +252,4 @@ inline const b3List2<b3Force>& b3Cloth::GetForceList() const
return m_forceList;
}
inline const b3Cloth* b3Cloth::GetNext() const
{
return m_next;
}
inline b3Cloth* b3Cloth::GetNext()
{
return m_next;
}
#endif

View File

@ -20,7 +20,7 @@
#define B3_DIAG_MAT_33_H
#include <bounce/common/math/mat33.h>
#include <bounce/dynamics/cloth/dense_vec3.h>
#include <bounce/cloth/dense_vec3.h>
// Diagonal matrix storing only the diagonal elements of the
// original matrix.

View File

@ -29,9 +29,7 @@ class b3Particle;
// Force types
enum b3ForceType
{
e_frictionForce,
e_springForce,
e_bendForce,
};
struct b3ForceDef

View File

@ -19,7 +19,7 @@
#ifndef B3_GARMENT_H
#define B3_GARMENT_H
#include <bounce/garment/sewing_pattern.h>
#include <bounce/cloth/garment/sewing_pattern.h>
struct b3SewingLine
{

View File

@ -20,9 +20,9 @@
#define B3_PARTICLE_H
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/dynamics/cloth/force.h>
#include <bounce/common/math/vec2.h>
#include <bounce/common/template/list.h>
#include <bounce/cloth/force.h>
class b3Shape;
class b3Cloth;

View File

@ -20,8 +20,8 @@
#define B3_SPARSE_SYM_MAT_33_H
#include <bounce/common/math/mat33.h>
#include <bounce/dynamics/cloth/diag_mat33.h>
#include <bounce/dynamics/cloth/dense_vec3.h>
#include <bounce/cloth/diag_mat33.h>
#include <bounce/cloth/dense_vec3.h>
// An element in a sparse symmetric matrix.
struct b3RowValue

View File

@ -19,7 +19,7 @@
#ifndef B3_SPRING_FORCE_H
#define B3_SPRING_FORCE_H
#include <bounce/dynamics/cloth/force.h>
#include <bounce/cloth/force.h>
struct b3SpringForceDef : public b3ForceDef
{

View File

@ -1,129 +0,0 @@
/*
* 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_BODY_DRAGGER_H
#define B3_BODY_DRAGGER_H
#include <bounce/common/geometry.h>
#include <bounce/dynamics/body.h>
// A body dragger.
class b3BodyDragger
{
public:
b3BodyDragger(b3Ray3* ray, b3World* world)
{
m_ray = ray;
m_world = world;
m_shape = nullptr;
m_mouseJoint = nullptr;
}
~b3BodyDragger()
{
}
bool IsDragging() const
{
return m_shape != nullptr;
}
bool StartDragging()
{
B3_ASSERT(IsDragging() == false);
b3RayCastSingleShapeOutput out;
if (m_world->RayCastSingleShape(&out, m_ray->A(), m_ray->B()) == false)
{
return false;
}
m_x = out.fraction;
m_shape = out.shape;
b3BodyDef bd;
b3Body* groundBody = m_world->CreateBody(bd);
b3Body* body = m_shape->GetBody();
body->SetAwake(true);
b3MouseJointDef jd;
jd.bodyA = groundBody;
jd.bodyB = body;
jd.target = out.point;
jd.maxForce = 2000.0f * body->GetMass();
m_mouseJoint = (b3MouseJoint*)m_world->CreateJoint(jd);
m_p = body->GetLocalPoint(out.point);
return true;
}
void Drag()
{
B3_ASSERT(IsDragging() == true);
m_mouseJoint->SetTarget(GetPointB());
}
void StopDragging()
{
B3_ASSERT(IsDragging() == true);
b3Body* groundBody = m_mouseJoint->GetBodyA();
m_world->DestroyJoint(m_mouseJoint);
m_mouseJoint = nullptr;
m_world->DestroyBody(groundBody);
m_shape = nullptr;
}
b3Ray3* GetRay() const
{
return m_ray;
}
b3Body* GetBody() const
{
B3_ASSERT(IsDragging() == true);
return m_shape->GetBody();
}
b3Vec3 GetPointA() const
{
B3_ASSERT(IsDragging() == true);
return m_shape->GetBody()->GetWorldPoint(m_p);
}
b3Vec3 GetPointB() const
{
B3_ASSERT(IsDragging() == true);
return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
}
private:
b3Ray3 * m_ray;
float32 m_x;
b3World* m_world;
b3Shape* m_shape;
b3Vec3 m_p;
b3MouseJoint* m_mouseJoint;
};
#endif

View File

@ -1,224 +0,0 @@
/*
* 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_CLOTH_DRAGGER_H
#define B3_CLOTH_DRAGGER_H
#include <bounce/collision/collision.h>
#include <bounce/dynamics/cloth/cloth.h>
#include <bounce/dynamics/cloth/cloth_mesh.h>
#include <bounce/dynamics/cloth/particle.h>
#include <bounce/dynamics/cloth/spring_force.h>
#include <bounce/dynamics/world.h>
// A cloth triangle dragger.
class b3ClothDragger
{
public:
b3ClothDragger(b3Ray3* ray, b3World* world)
{
m_spring = false;
m_ray = ray;
m_world = world;
m_cloth = nullptr;
}
~b3ClothDragger()
{
}
bool IsDragging() const
{
return m_cloth != nullptr;
}
bool StartDragging()
{
B3_ASSERT(IsDragging() == false);
b3RayCastSingleClothOutput rayOut;
if (m_world->RayCastSingleCloth(&rayOut, m_ray->A(), m_ray->B()) == false)
{
return false;
}
m_cloth = rayOut.cloth;
m_mesh = m_cloth->GetMesh();
m_triangle = m_mesh->triangles + rayOut.triangle;
m_x = rayOut.fraction;
b3Particle* p1 = m_cloth->GetVertexParticle(m_triangle->v1);
b3Particle* p2 = m_cloth->GetVertexParticle(m_triangle->v2);
b3Particle* p3 = m_cloth->GetVertexParticle(m_triangle->v3);
b3Vec3 v1 = p1->GetPosition();
b3Vec3 v2 = p2->GetPosition();
b3Vec3 v3 = p3->GetPosition();
b3Vec3 B = GetPointB();
float32 wABC[4];
b3BarycentricCoordinates(wABC, v1, v2, v3, B);
if (wABC[3] > B3_EPSILON)
{
m_u = wABC[0] / wABC[3];
m_v = wABC[1] / wABC[3];
}
else
{
m_u = m_v = 0.0f;
}
if (m_spring)
{
b3ParticleDef pd;
pd.type = e_staticParticle;
pd.position = B;
m_particle = m_cloth->CreateParticle(pd);
{
b3SpringForceDef sfd;
sfd.p1 = m_particle;
sfd.p2 = p1;
sfd.restLength = 0.0f;
sfd.structural = 10000.0f;
m_s1 = (b3SpringForce*)m_cloth->CreateForce(sfd);
}
{
b3SpringForceDef sfd;
sfd.p1 = m_particle;
sfd.p2 = p2;
sfd.restLength = 0.0f;
sfd.structural = 10000.0f;
m_s2 = (b3SpringForce*)m_cloth->CreateForce(sfd);
}
{
b3SpringForceDef sfd;
sfd.p1 = m_particle;
sfd.p2 = p3;
sfd.restLength = 0.0f;
sfd.structural = 10000.0f;
m_s3 = (b3SpringForce*)m_cloth->CreateForce(sfd);
}
}
else
{
m_t1 = p1->GetType();
p1->SetType(e_staticParticle);
m_t2 = p2->GetType();
p2->SetType(e_staticParticle);
m_t3 = p3->GetType();
p3->SetType(e_staticParticle);
}
return true;
}
void Drag()
{
B3_ASSERT(IsDragging() == true);
b3Vec3 A = GetPointA();
b3Vec3 B = GetPointB();
b3Vec3 dx = B - A;
if (m_spring)
{
m_particle->SetPosition(B);
}
else
{
b3Particle* p1 = m_cloth->GetVertexParticle(m_triangle->v1);
p1->ApplyTranslation(dx);
b3Particle* p2 = m_cloth->GetVertexParticle(m_triangle->v2);
p2->ApplyTranslation(dx);
b3Particle* p3 = m_cloth->GetVertexParticle(m_triangle->v3);
p3->ApplyTranslation(dx);
}
}
void StopDragging()
{
B3_ASSERT(IsDragging() == true);
if (m_spring)
{
m_cloth->DestroyForce(m_s1);
m_cloth->DestroyForce(m_s2);
m_cloth->DestroyForce(m_s3);
m_cloth->DestroyParticle(m_particle);
}
else
{
m_cloth->GetVertexParticle(m_triangle->v1)->SetType(m_t1);
m_cloth->GetVertexParticle(m_triangle->v2)->SetType(m_t2);
m_cloth->GetVertexParticle(m_triangle->v3)->SetType(m_t3);
}
m_cloth = nullptr;
}
b3Vec3 GetPointA() const
{
B3_ASSERT(IsDragging() == true);
b3Vec3 A = m_cloth->GetVertexParticle(m_triangle->v1)->GetPosition();
b3Vec3 B = m_cloth->GetVertexParticle(m_triangle->v2)->GetPosition();
b3Vec3 C = m_cloth->GetVertexParticle(m_triangle->v3)->GetPosition();
return m_u * A + m_v * B + (1.0f - m_u - m_v) * C;
}
b3Vec3 GetPointB() const
{
B3_ASSERT(IsDragging() == true);
return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
}
private:
b3Ray3* m_ray;
float32 m_x;
b3World* m_world;
b3Cloth* m_cloth;
const b3ClothMesh* m_mesh;
b3ClothMeshTriangle* m_triangle;
float32 m_u, m_v;
bool m_spring;
b3Particle* m_particle;
b3SpringForce* m_s1;
b3SpringForce* m_s2;
b3SpringForce* m_s3;
b3ParticleType m_t1, m_t2, m_t3;
};
#endif

View File

@ -1,158 +0,0 @@
/*
* 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_BEND_FORCE_H
#define B3_BEND_FORCE_H
#include <bounce/dynamics/cloth/force.h>
struct b3BendForceDef : public b3ForceDef
{
b3BendForceDef()
{
type = e_bendForce;
p1 = nullptr;
p2 = nullptr;
p3 = nullptr;
p4 = nullptr;
restDistance = 0.0f;
restAngle = 0.0f;
structural = 0.0f;
damping = 0.0f;
}
//
void Initialize(b3Particle* particle1, b3Particle* particle2, b3Particle* particle3, b3Particle* particle4,
float32 structuralStiffness, float32 dampingStiffness);
// Particle 1
b3Particle* p1;
// Particle 2
b3Particle* p2;
// Particle 3
b3Particle* p3;
// Particle 4
b3Particle* p4;
// Rest distance
float32 restDistance;
// Rest angle
float32 restAngle;
// Structural stiffness
float32 structural;
// Damping stiffness
float32 damping;
};
//
class b3BendForce : public b3Force
{
public:
b3Particle* GetParticle1();
b3Particle* GetParticle2();
b3Particle* GetParticle3();
b3Particle* GetParticle4();
float32 GetRestDistance() const;
float32 GetRestAngle() const;
float32 GetStructuralStiffness() const;
float32 GetDampingStiffness() const;
private:
friend class b3Force;
friend class b3Cloth;
b3BendForce(const b3BendForceDef* def);
~b3BendForce();
void Apply(const b3ClothSolverData* data);
// Solver shared
// Particle 1
b3Particle* m_p1;
// Particle 2
b3Particle* m_p2;
// Particle 3
b3Particle* m_p3;
// Particle 4
b3Particle* m_p4;
// Rest distance
float32 m_L0;
// Rest angle
float32 m_angle0;
// Structural stiffness
float32 m_ks;
// Structural stiffness
float32 m_kd;
};
inline b3Particle* b3BendForce::GetParticle1()
{
return m_p1;
}
inline b3Particle* b3BendForce::GetParticle2()
{
return m_p2;
}
inline b3Particle* b3BendForce::GetParticle3()
{
return m_p3;
}
inline b3Particle* b3BendForce::GetParticle4()
{
return m_p4;
}
inline float32 b3BendForce::GetRestAngle() const
{
return m_angle0;
}
inline float32 b3BendForce::GetStructuralStiffness() const
{
return m_ks;
}
inline float32 b3BendForce::GetDampingStiffness() const
{
return m_kd;
}
#endif

View File

@ -26,17 +26,15 @@
#include <bounce/dynamics/joint_manager.h>
#include <bounce/dynamics/contact_manager.h>
struct b3ClothDef;
struct b3BodyDef;
class b3Cloth;
class b3Body;
class b3QueryListener;
class b3RayCastListener;
class b3ContactListener;
class b3ContactFilter;
struct b3RayCastSingleShapeOutput
struct b3RayCastSingleOutput
{
b3Shape* shape; // shape
b3Vec3 point; // intersection point on surface
@ -44,15 +42,6 @@ struct b3RayCastSingleShapeOutput
float32 fraction; // time of intersection on segment
};
struct b3RayCastSingleClothOutput
{
b3Cloth* cloth; // cloth
u32 triangle; // triangle
b3Vec3 point; // intersection point on surface
b3Vec3 normal; // surface normal of intersection
float32 fraction; // time of intersection on segment
};
// Use a physics world to create/destroy rigid bodies, execute ray cast and volume queries.
class b3World
{
@ -79,12 +68,6 @@ public:
// The acceleration has units of m/s^2.
void SetGravity(const b3Vec3& gravity);
// Create a new deformable cloth.
b3Cloth* CreateCloth(const b3ClothDef& def);
// Destroy an existing deformable cloth.
void DestroyCloth(b3Cloth* cloth);
// Create a new rigid body.
b3Body* CreateBody(const b3BodyDef& def);
@ -108,29 +91,14 @@ public:
// The ray cast output is the intercepted shape, the intersection
// point in world space, the face normal on the shape associated with the point,
// and the intersection fraction.
void RayCastShape(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
// Perform a ray cast with the world.
// If the ray doesn't intersect with a shape in the world then return false.
// The ray cast output is the intercepted shape, the intersection
// point in world space, the face normal on the shape associated with the point,
// and the intersection fraction.
bool RayCastSingleShape(b3RayCastSingleShapeOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
// Perform a ray cast with the world.
// The given ray cast listener will be notified when a ray intersects a shape
// in the world.
// The ray cast output is the intercepted cloth, the intersection
// point in world space, the face normal on the cloth associated with the point,
// and the intersection fraction.
void RayCastCloth(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
// Perform a ray cast with the world.
// If the ray doesn't intersect with a cloth in the world then return false.
// The ray cast output is the intercepted cloth, the intersection
// point in world space, the face normal on the cloth associated with the point,
// and the intersection fraction.
bool RayCastSingleCloth(b3RayCastSingleClothOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
bool RayCastSingle(b3RayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
// Perform a AABB query with the world.
// The query listener will be notified when two shape AABBs are overlapping.
@ -161,8 +129,7 @@ private :
e_shapeAddedFlag = 0x0001,
e_clearForcesFlag = 0x0002,
};
friend class b3Cloth;
friend class b3Body;
friend class b3Shape;
friend class b3Contact;
@ -172,20 +139,16 @@ private :
void Solve(float32 dt, u32 velocityIterations, u32 positionIterations);
void StepCloth(float32 dt);
bool m_sleeping;
bool m_warmStarting;
u32 m_flags;
b3Vec3 m_gravity;
b3StackAllocator m_stackAllocator;
b3BlockPool m_clothBlocks;
// Pool of bodies
b3BlockPool m_bodyBlocks;
// List of clothes
b3List2<b3Cloth> m_clothList;
// List of bodies
b3List2<b3Body> m_bodyList;

View File

@ -22,7 +22,6 @@
#include <bounce/common/math/math.h>
class b3Shape;
class b3Cloth;
class b3Contact;
class b3QueryListener
@ -47,12 +46,6 @@ public:
// the intersection point on the shape, the surface normal associated with the point, and the
// intersection fraction for the ray.
virtual float32 ReportShape(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction) = 0;
// Report that a cloth was hit by the ray to this contact listener.
// The reported information are the shape hit by the ray,
// the intersection point on the cloth, the surface normal associated with the point, the
// intersection fraction for the ray, and the triangle.
virtual float32 ReportCloth(b3Cloth* cloth, const b3Vec3& point, const b3Vec3& normal, float32 fraction, u32 triangle) = 0;
};
class b3ContactListener