New feature: soft bodies!

include/bounce/bounce.h

@@ -72,4 +72,8 @@
 #include <bounce/cloth/garment/garment.h>
 #include <bounce/cloth/garment/garment_mesh.h>
 
+#include <bounce/softbody/softbody_mesh.h>
+#include <bounce/softbody/softbody.h>
+#include <bounce/softbody/softbody_node.h>
+
 #endif

include/bounce/common/geometry.h

@@ -144,6 +144,32 @@ inline void b3BarycentricCoordinates(float32 out[4],
 	out[3] = out[0] + out[1] + out[2];
 }
 
+// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w, x) 
+// with respect to a tetrahedron ABCD.
+// The last output value is the (positive) divisor.
+inline void b3BarycentricCoordinates(float32 out[5],
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D,
+	const b3Vec3& Q)
+{
+	b3Vec3 AB = B - A;
+	b3Vec3 AC = C - A;
+	b3Vec3 AD = D - A;
+
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	b3Vec3 QC = C - Q;
+	b3Vec3 QD = D - Q;
+
+	float32 divisor = b3Det(AB, AC, AD);
+	float32 sign = b3Sign(divisor);
+
+	out[0] = sign * b3Det(QB, QC, QD);
+	out[1] = sign * b3Det(QA, QD, QC);
+	out[2] = sign * b3Det(QA, QB, QD);
+	out[3] = sign * b3Det(QA, QC, QB);
+	out[4] = sign * divisor;
+}
+
 // Project a point onto a segment AB.
 inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
 {

include/bounce/dynamics/body.h

@@ -293,9 +293,7 @@ private:
 	friend class b3MeshContact;
 	friend class b3ContactManager;
 	friend class b3ContactSolver;
-	friend class b3ClothSolver;
-	friend class b3ClothContactSolver;
-
+	
 	friend class b3Joint;
 	friend class b3JointManager;
 	friend class b3JointSolver;
@@ -308,6 +306,13 @@ private:
 
 	friend class b3List2<b3Body>;
 
+	friend class b3ClothSolver;
+	friend class b3ClothContactSolver;
+
+	friend class b3SoftBody;
+	friend class b3SoftBodySolver;
+	friend class b3SoftBodyContactSolver;
+
 	enum b3BodyFlags 
 	{
 		e_awakeFlag = 0x0001,

include/bounce/softbody/softbody.h

@@ -0,0 +1,193 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_H
+#define B3_SOFT_BODY_H
+
+#include <bounce/common/math/transform.h>
+#include <bounce/common/memory/stack_allocator.h>
+
+class b3World;
+
+struct b3SoftBodyMesh;
+
+class b3SoftBodyNode;
+
+struct b3RayCastInput;
+struct b3RayCastOutput;
+
+struct b3SoftBodyRayCastSingleOutput
+{
+	u32 tetrahedron;
+	u32 v1, v2, v3;
+	float32 fraction;
+	b3Vec3 normal;
+};
+
+// Soft body tetrahedron element
+struct b3SoftBodyElement
+{
+	float32 invP[16];
+	b3Mat33 K[16];
+	b3Quat q;
+};
+
+// Soft body tetrahedron triangle
+struct b3SoftBodyTriangle
+{
+	u32 v1, v2, v3;
+	u32 tetrahedron;
+};
+
+// Soft body definition
+// This requires defining a soft body mesh which is typically bound to a render mesh
+struct b3SoftBodyDef
+{
+	b3SoftBodyDef()
+	{
+		mesh = nullptr;
+		density = 0.1f;
+		E = 100.0f;
+		nu = 0.3f;
+	}
+
+	// Soft body mesh
+	const b3SoftBodyMesh* mesh;
+
+	// Density in kg/m^3
+	float32 density;
+
+	// Material Young's modulus in [0, inf]
+	// Units are 1e3N/m^2
+	float32 E;
+
+	// Material Poisson ratio in [0, 0.5]
+	// This is a dimensionless value
+	float32 nu;
+};
+
+// A soft body represents a deformable volume as a collection of nodes.
+class b3SoftBody
+{
+public:
+	b3SoftBody(const b3SoftBodyDef& def);
+	~b3SoftBody();
+
+	// Perform a ray cast with the soft body.
+	bool RayCastSingle(b3SoftBodyRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
+
+	// 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 bodies in the attached world.
+	void SetWorld(b3World* world);
+
+	// Get the world attached to this cloth.
+	const b3World* GetWorld() const;
+	b3World* GetWorld();
+
+	// Return the soft body mesh proxy.
+	const b3SoftBodyMesh* GetMesh() const;
+
+	// Return the node associated with the given vertex.
+	b3SoftBodyNode* GetVertexNode(u32 i);
+
+	// Return the kinetic (or dynamic) energy in this system.
+	float32 GetEnergy() const;
+
+	// Perform a time step. 
+	void Step(float32 dt, u32 velocityIterations, u32 positionIterations);
+
+	// Debug draw the body using the associated mesh.
+	void Draw() const;
+private:
+	// Compute mass of each node.
+	void ComputeMass();
+
+	// Update contacts.
+	void UpdateContacts();
+
+	// Solve
+	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
+
+	// Stack allocator
+	b3StackAllocator m_stackAllocator;
+
+	// Gravity acceleration
+	b3Vec3 m_gravity;
+
+	// Proxy mesh
+	const b3SoftBodyMesh* m_mesh;
+
+	// Soft body density
+	float32 m_density;
+
+	// Material Young's modulus
+	float32 m_E;
+
+	// Material poisson ratio
+	float32 m_nu;
+
+	// Soft body nodes
+	b3SoftBodyNode* m_nodes;
+
+	// Soft body elements
+	b3SoftBodyElement* m_elements;
+
+	// Soft body triangles
+	b3SoftBodyTriangle* m_triangles;
+
+	// Attached world
+	b3World* m_world;
+};
+
+inline void b3SoftBody::SetGravity(const b3Vec3& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline b3Vec3 b3SoftBody::GetGravity() const
+{
+	return m_gravity;
+}
+
+inline void b3SoftBody::SetWorld(b3World* world)
+{
+	m_world = world;
+}
+
+inline const b3World* b3SoftBody::GetWorld() const
+{
+	return m_world;
+}
+
+inline b3World* b3SoftBody::GetWorld()
+{
+	return m_world;
+}
+
+inline const b3SoftBodyMesh* b3SoftBody::GetMesh() const
+{
+	return m_mesh;
+}
+
+#endif

include/bounce/softbody/softbody_contact_solver.h

@@ -0,0 +1,129 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_CONTACT_SOLVER_H
+#define B3_SOFT_BODY_CONTACT_SOLVER_H
+
+#include <bounce/common/math/mat22.h>
+#include <bounce/common/math/mat33.h>
+
+class b3StackAllocator;
+
+class b3SoftBodyNode;
+class b3Body;
+
+class b3NodeBodyContact;
+
+struct b3DenseVec3;
+
+struct b3SoftBodySolverBodyContactVelocityConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+
+	b3Body* bodyB;
+	float32 invMassB;
+	b3Mat33 invIB;
+
+	float32 friction;
+
+	b3Vec3 point;
+	b3Vec3 rA;
+	b3Vec3 rB;
+
+	b3Vec3 normal;
+	float32 normalMass;
+	float32 normalImpulse;
+	float32 velocityBias;
+
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Mat22 tangentMass;
+	b3Vec2 tangentImpulse;
+};
+
+struct b3SoftBodySolverBodyContactPositionConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+	float32 radiusA;
+	b3Vec3 localCenterA;
+
+	b3Body* bodyB;
+	float32 invMassB;
+	b3Mat33 invIB;
+	float32 radiusB;
+	b3Vec3 localCenterB;
+
+	b3Vec3 rA;
+	b3Vec3 rB;
+
+	b3Vec3 normalA;
+	b3Vec3 localPointA;
+	b3Vec3 localPointB;
+};
+
+struct b3SoftBodyContactSolverDef
+{
+	b3StackAllocator* allocator;
+
+	b3DenseVec3* positions;
+	b3DenseVec3* velocities;
+
+	u32 bodyContactCapacity;
+};
+
+inline float32 b3MixFriction(float32 u1, float32 u2)
+{
+	return b3Sqrt(u1 * u2);
+}
+
+class b3SoftBodyContactSolver
+{
+public:
+	b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def);
+	~b3SoftBodyContactSolver();
+
+	void Add(b3NodeBodyContact* c);
+
+	void InitializeBodyContactConstraints();
+
+	void WarmStart();
+
+	void SolveBodyContactVelocityConstraints();
+
+	void StoreImpulses();
+
+	bool SolveBodyContactPositionConstraints();
+protected:
+	b3StackAllocator* m_allocator;
+
+	b3DenseVec3* m_positions;
+	b3DenseVec3* m_velocities;
+
+	u32 m_bodyContactCapacity;
+	u32 m_bodyContactCount;
+	b3NodeBodyContact** m_bodyContacts;
+	
+	b3SoftBodySolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
+	b3SoftBodySolverBodyContactPositionConstraint* m_bodyPositionConstraints;
+};
+
+#endif

include/bounce/softbody/softbody_mesh.h

@@ -0,0 +1,45 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_MESH_H
+#define B3_SOFT_BODY_MESH_H
+
+#include <bounce/common/math/vec3.h>
+
+struct b3SoftBodyMeshTetrahedron
+{
+	u32 v1, v2, v3, v4;
+};
+
+struct b3SoftBodyMesh
+{
+	u32 vertexCount;
+	b3Vec3* vertices;
+	u32 tetrahedronCount;
+	b3SoftBodyMeshTetrahedron* tetrahedrons;
+};
+
+struct b3QSoftBodyMesh : public b3SoftBodyMesh
+{
+	b3QSoftBodyMesh();
+	~b3QSoftBodyMesh();
+
+	void SetAsSphere(float32 radius, u32 subdivisions);
+};
+
+#endif

include/bounce/softbody/softbody_node.h

@@ -0,0 +1,259 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_NODE_H
+#define B3_SOFT_BODY_NODE_H
+
+#include <bounce/common/math/vec2.h>
+#include <bounce/common/math/vec3.h>
+#include <bounce/common/math/transform.h>
+
+class b3Shape;
+class b3SoftBody;
+class b3SoftBodyNode;
+
+// A contact between a node and a body
+class b3NodeBodyContact
+{
+public:
+	b3NodeBodyContact() { }
+	~b3NodeBodyContact() { }
+
+	b3SoftBodyNode* n1;
+	b3Shape* s2;
+
+	// Contact constraint
+	b3Vec3 normal1;
+	b3Vec3 localPoint1;
+	b3Vec3 localPoint2;
+	float32 normalImpulse;
+
+	// Friction constraint
+	b3Vec3 t1, t2;
+	b3Vec2 tangentImpulse;
+
+	bool active;
+};
+
+struct b3NodeBodyContactWorldPoint
+{
+	void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
+	{
+		b3Vec3 nA = c->normal1;
+		
+		b3Vec3 cA = xfA * c->localPoint1;
+		b3Vec3 cB = xfB * c->localPoint2;
+
+		b3Vec3 pA = cA + rA * nA;
+		b3Vec3 pB = cB - rB * nA;
+
+		point = 0.5f * (pA + pB);
+		normal = nA;
+		separation = b3Dot(cB - cA, nA) - rA - rB;
+	}
+
+	b3Vec3 point;
+	b3Vec3 normal;
+	float32 separation;
+};
+
+// Static node: Can be moved manually.
+// Dynamic node: Non-zero velocity determined by force, can be moved by the solver.
+enum b3SoftBodyNodeType
+{
+	e_staticSoftBodyNode,
+	e_dynamicSoftBodyNode
+};
+
+// A soft body node.
+class b3SoftBodyNode 
+{
+public:
+	// Set the node type.
+	void SetType(b3SoftBodyNodeType type);
+
+	// Get the node type.
+	b3SoftBodyNodeType GetType() const;
+
+	// Get the vertex index.
+	u32 GetVertex() const;
+
+	// Set the particle position. 
+	// If the particle is dynamic changing the position directly might lead 
+	// to physically incorrect simulation behaviour.
+	void SetPosition(const b3Vec3& position);
+
+	// Get the particle position.
+	const b3Vec3& GetPosition() const;
+
+	// Set the particle velocity.
+	void SetVelocity(const b3Vec3& velocity);
+
+	// Get the particle velocity.
+	const b3Vec3& GetVelocity() const;
+
+	// Get the particle mass.
+	float32 GetMass() const;
+
+	// Set the particle radius.
+	void SetRadius(float32 radius);
+	
+	// Get the particle radius.
+	float32 GetRadius() const;
+
+	// Set the particle coefficient of friction.
+	void SetFriction(float32 friction);
+	
+	// Get the particle coefficient of friction.
+	float32 GetFriction() const;
+	
+	// Apply a force.
+	void ApplyForce(const b3Vec3& force);
+private:
+	friend class b3SoftBody;
+	friend class b3SoftBodySolver;
+	friend class b3SoftBodyContactSolver;
+
+	b3SoftBodyNode() { }
+
+	~b3SoftBodyNode() { }
+
+	// Type
+	b3SoftBodyNodeType m_type;
+
+	// Position
+	b3Vec3 m_position;
+	
+	// Velocity
+	b3Vec3 m_velocity;
+
+	// Applied external force
+	b3Vec3 m_force;
+
+	// Mass
+	float32 m_mass;
+
+	// Inverse mass
+	float32 m_invMass;
+
+	// Radius
+	float32 m_radius;
+
+	// Coefficient of friction
+	float32 m_friction;
+	
+	// User data. 
+	void* m_userData;
+
+	// Soft body mesh vertex index.
+	u32 m_vertex;
+
+	// Node and body contact
+	b3NodeBodyContact m_bodyContact;
+
+	// Soft body
+	b3SoftBody* m_body;
+};
+
+inline void b3SoftBodyNode::SetType(b3SoftBodyNodeType type)
+{
+	if (m_type == type)
+	{
+		return;
+	}
+
+	m_type = type;
+	m_force.SetZero();
+
+	if (type == e_staticSoftBodyNode)
+	{
+		m_velocity.SetZero();
+	}
+
+	m_bodyContact.active = false;
+}
+
+inline b3SoftBodyNodeType b3SoftBodyNode::GetType() const
+{
+	return m_type;
+}
+
+inline u32 b3SoftBodyNode::GetVertex() const
+{
+	return m_vertex;
+}
+
+inline void b3SoftBodyNode::SetPosition(const b3Vec3& position)
+{
+	m_position = position;
+}
+
+inline const b3Vec3& b3SoftBodyNode::GetPosition() const
+{
+	return m_position;
+}
+
+inline void b3SoftBodyNode::SetVelocity(const b3Vec3& velocity)
+{
+	if (m_type == e_staticSoftBodyNode)
+	{
+		return;
+	}
+	m_velocity = velocity;
+}
+
+inline const b3Vec3& b3SoftBodyNode::GetVelocity() const
+{
+	return m_velocity;
+}
+
+inline float32 b3SoftBodyNode::GetMass() const
+{
+	return m_mass;
+}
+
+inline void b3SoftBodyNode::SetRadius(float32 radius)
+{
+	m_radius = radius;
+}
+
+inline float32 b3SoftBodyNode::GetRadius() const
+{
+	return m_radius;
+}
+
+inline void b3SoftBodyNode::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b3SoftBodyNode::GetFriction() const
+{
+	return m_friction;
+}
+
+inline void b3SoftBodyNode::ApplyForce(const b3Vec3& force)
+{
+	if (m_type != e_dynamicSoftBodyNode)
+	{
+		return;
+	}
+	m_force += force;
+}
+
+#endif

include/bounce/softbody/softbody_solver.h

@@ -0,0 +1,56 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_SOLVER_H
+#define B3_SOFT_BODY_SOLVER_H
+
+#include <bounce/common/math/mat22.h>
+#include <bounce/common/math/mat33.h>
+
+class b3StackAllocator;
+
+class b3SoftBodyMesh;
+
+class b3SoftBodyNode;
+struct b3SoftBodyElement;
+
+class b3NodeBodyContact;
+
+struct b3SoftBodySolverDef
+{
+	b3StackAllocator* stack;
+	const b3SoftBodyMesh* mesh;
+	b3SoftBodyNode* nodes;
+	b3SoftBodyElement* elements;
+};
+
+class b3SoftBodySolver
+{
+public:
+	b3SoftBodySolver(const b3SoftBodySolverDef& def);
+	~b3SoftBodySolver();
+
+	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
+private:
+	b3StackAllocator* m_allocator;
+	const b3SoftBodyMesh* m_mesh;
+	b3SoftBodyNode* m_nodes;
+	b3SoftBodyElement* m_elements;
+};
+
+#endif

include/bounce/softbody/sparse_mat33.h

@@ -0,0 +1,322 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SPARSE_MAT_33_H
+#define B3_SPARSE_MAT_33_H
+
+#include <bounce/common/math/mat33.h>
+#include <bounce/cloth/diag_mat33.h>
+#include <bounce/cloth/dense_vec3.h>
+#include <bounce/cloth/sparse_sym_mat33.h>
+
+// A sparse matrix.
+// Each row is a list of non-zero elements in the row.
+struct b3SparseMat33
+{
+	//
+	b3SparseMat33();
+
+	// 
+	b3SparseMat33(u32 m);
+
+	//
+	b3SparseMat33(const b3SparseMat33& _m);
+
+	//
+	~b3SparseMat33();
+
+	//
+	b3SparseMat33& operator=(const b3SparseMat33& _m);
+
+	// 
+	void Copy(const b3SparseMat33& _m);
+
+	//
+	void Destroy();
+
+	// 
+	b3Mat33& operator()(u32 i, u32 j);
+
+	// 
+	const b3Mat33& operator()(u32 i, u32 j) const;
+
+	// 
+	void operator+=(const b3SparseMat33& m);
+
+	// 
+	void operator-=(const b3SparseMat33& m);
+
+	u32 rowCount;
+	b3RowValueList* rows;
+};
+
+inline b3SparseMat33::b3SparseMat33()
+{
+	rowCount = 0;
+	rows = nullptr;
+}
+
+inline b3SparseMat33::b3SparseMat33(u32 m)
+{
+	rowCount = m;
+	rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		new (rows + i)b3RowValueList();
+	}
+}
+
+inline b3SparseMat33::b3SparseMat33(const b3SparseMat33& m)
+{
+	rowCount = m.rowCount;
+	rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		new (rows + i)b3RowValueList();
+	}
+
+	Copy(m);
+}
+
+inline b3SparseMat33::~b3SparseMat33()
+{
+	Destroy();
+}
+
+inline void b3SparseMat33::Destroy()
+{
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		b3RowValueList* vs = rows + i;
+
+		b3RowValue* v = vs->head;
+		while (v)
+		{
+			b3RowValue* v0 = v->next;
+			b3Free(v);
+			v = v0;
+		}
+
+		vs->~b3RowValueList();
+	}
+
+	b3Free(rows);
+}
+
+inline b3SparseMat33& b3SparseMat33::operator=(const b3SparseMat33& _m)
+{
+	if (_m.rows == rows)
+	{
+		return *this;
+	}
+
+	Destroy();
+
+	rowCount = _m.rowCount;
+	rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		new (rows + i)b3RowValueList();
+	}
+
+	Copy(_m);
+
+	return *this;
+}
+
+inline void b3SparseMat33::Copy(const b3SparseMat33& _m)
+{
+	B3_ASSERT(rowCount == _m.rowCount);
+
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		b3RowValueList* vs1 = _m.rows + i;
+		b3RowValueList* vs2 = rows + i;
+
+		B3_ASSERT(vs2->count == 0);
+
+		for (b3RowValue* v1 = vs1->head; v1; v1 = v1->next)
+		{
+			b3RowValue* v2 = (b3RowValue*)b3Alloc(sizeof(b3RowValue));
+
+			v2->column = v1->column;
+			v2->value = v1->value;
+
+			vs2->PushFront(v2);
+		}
+	}
+}
+
+inline const b3Mat33& b3SparseMat33::operator()(u32 i, u32 j) const
+{
+	B3_ASSERT(i < rowCount);
+	B3_ASSERT(j < rowCount);
+
+	b3RowValueList* vs = rows + i;
+
+	for (b3RowValue* v = vs->head; v; v = v->next)
+	{
+		if (v->column == j)
+		{
+			return v->value;
+		}
+	}
+
+	return b3Mat33_zero;
+}
+
+inline b3Mat33& b3SparseMat33::operator()(u32 i, u32 j)
+{
+	B3_ASSERT(i < rowCount);
+	B3_ASSERT(j < rowCount);
+
+	b3RowValueList* vs = rows + i;
+
+	for (b3RowValue* v = vs->head; v; v = v->next)
+	{
+		if (v->column == j)
+		{
+			return v->value;
+		}
+	}
+
+	b3RowValue* v = (b3RowValue*)b3Alloc(sizeof(b3RowValue));
+	v->column = j;
+	v->value.SetZero();
+
+	vs->PushFront(v);
+
+	return v->value;
+}
+
+inline void b3SparseMat33::operator+=(const b3SparseMat33& m)
+{
+	B3_ASSERT(rowCount == m.rowCount);
+
+	for (u32 i = 0; i < m.rowCount; ++i)
+	{
+		b3RowValueList* mvs = m.rows + i;
+
+		for (b3RowValue* v = mvs->head; v; v = v->next)
+		{
+			u32 j = v->column;
+
+			(*this)(i, j) += v->value;
+		}
+	}
+}
+
+inline void b3SparseMat33::operator-=(const b3SparseMat33& m)
+{
+	B3_ASSERT(rowCount == m.rowCount);
+
+	for (u32 i = 0; i < m.rowCount; ++i)
+	{
+		b3RowValueList* mvs = m.rows + i;
+
+		for (b3RowValue* v = mvs->head; v; v = v->next)
+		{
+			u32 j = v->column;
+
+			(*this)(i, j) -= v->value;
+		}
+	}
+}
+
+inline void b3Add(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b)
+{
+	out = a;
+	out += b;
+}
+
+inline void b3Sub(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b)
+{
+	out = a;
+	out -= b;
+}
+
+inline void b3Mul(b3DenseVec3& out, const b3SparseMat33& A, const b3DenseVec3& v)
+{
+	B3_ASSERT(A.rowCount == out.n);
+
+	out.SetZero();
+
+	for (u32 i = 0; i < A.rowCount; ++i)
+	{
+		b3RowValueList* vs = A.rows + i;
+
+		for (b3RowValue* vA = vs->head; vA; vA = vA->next)
+		{
+			u32 j = vA->column;
+			b3Mat33 a = vA->value;
+
+			out[i] += a * v[j];
+		}
+	}
+}
+
+inline void b3Mul(b3SparseMat33& out, float32 s, const b3SparseMat33& B)
+{
+	B3_ASSERT(out.rowCount == B.rowCount);
+
+	if (s == 0.0f)
+	{
+		return;
+	}
+
+	out = B;
+
+	for (u32 i = 0; i < out.rowCount; ++i)
+	{
+		b3RowValueList* vs = out.rows + i;
+		for (b3RowValue* v = vs->head; v; v = v->next)
+		{
+			v->value = s * v->value;
+		}
+	}
+}
+
+inline b3SparseMat33 operator+(const b3SparseMat33& A, const b3SparseMat33& B)
+{
+	b3SparseMat33 result(A.rowCount);
+	b3Add(result, A, B);
+	return result;
+}
+
+inline b3SparseMat33 operator-(const b3SparseMat33& A, const b3SparseMat33& B)
+{
+	b3SparseMat33 result(A.rowCount);
+	b3Sub(result, A, B);
+	return result;
+}
+
+inline b3SparseMat33 operator*(float32 A, const b3SparseMat33& B)
+{
+	b3SparseMat33 result(B.rowCount);
+	b3Mul(result, A, B);
+	return result;
+}
+
+inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v)
+{
+	b3DenseVec3 result(v.n);
+	b3Mul(result, A, v);
+	return result;
+}
+
+#endif