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

consistency

Browse Source
This commit is contained in:
Irlan 2018-07-09 19:21:50 -03:00
parent 36a6a898fc
commit a3a867cbdf
3 changed files with 188 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/bounce/dynamics/cloth/cloth_solver.h
View File

@ -81,9 +81,6 @@ private:
// Apply constraints.
void ApplyConstraints();
// Compute A and b in Ax = b.
void Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const;
// Solve Ax = b.
void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;

180
include/bounce/dynamics/cloth/sparse_sym_mat33.h
View File

@ -25,12 +25,24 @@
struct b3SparseSymMat33
{
//
b3SparseSymMat33();
//
b3SparseSymMat33(u32 m);
//
b3SparseSymMat33(const b3SparseSymMat33& _m);
//
~b3SparseSymMat33();
//
b3SparseSymMat33& operator=(const b3SparseSymMat33& _m);
//
void Copy(const b3SparseSymMat33& _m);
//
b3Mat33& operator()(u32 i, u32 j);
@ -38,8 +50,14 @@ struct b3SparseSymMat33
const b3Mat33& operator()(u32 i, u32 j) const;
//
void Diagonal(b3DiagMat33& out) const;
void operator+=(const b3SparseSymMat33& m);
//
void operator-=(const b3SparseSymMat33& m);
//
void Diagonal(b3DiagMat33& out) const;
u32 M;
u32* row_ptrs;
u32 value_capacity;
@ -48,6 +66,16 @@ struct b3SparseSymMat33
u32* value_columns;
};
inline b3SparseSymMat33::b3SparseSymMat33()
{
M = 0;
row_ptrs = nullptr;
value_count = 0;
value_capacity = 0;
values = nullptr;
value_columns = nullptr;
}
inline b3SparseSymMat33::b3SparseSymMat33(u32 m)
{
M = m;
@ -59,6 +87,19 @@ inline b3SparseSymMat33::b3SparseSymMat33(u32 m)
value_columns = (u32*)b3Alloc(value_capacity * sizeof(u32));
}
inline b3SparseSymMat33::b3SparseSymMat33(const b3SparseSymMat33& m)
{
M = m.M;
row_ptrs = (u32*)b3Alloc((M + 1) * sizeof(u32));
memset(row_ptrs, 0, (M + 1) * sizeof(u32));
value_count = m.value_count;
value_capacity = M * (M + 1) / 2;
values = (b3Mat33*)b3Alloc(value_capacity * sizeof(b3Mat33));
value_columns = (u32*)b3Alloc(value_capacity * sizeof(u32));
Copy(m);
}
inline b3SparseSymMat33::~b3SparseSymMat33()
{
b3Free(value_columns);
@ -66,6 +107,46 @@ inline b3SparseSymMat33::~b3SparseSymMat33()
b3Free(row_ptrs);
}
inline b3SparseSymMat33& b3SparseSymMat33::operator=(const b3SparseSymMat33& _m)
{
if (_m.values == values)
{
return *this;
}
if (M == _m.M)
{
Copy(_m);
return *this;
}
b3Free(row_ptrs);
b3Free(values);
b3Free(value_columns);
M = _m.M;
row_ptrs = (u32*)b3Alloc((M + 1) * sizeof(u32));
value_count = _m.value_count;
value_capacity = _m.value_capacity;
values = (b3Mat33*)b3Alloc(value_capacity * sizeof(b3Mat33));
value_columns = (u32*)b3Alloc(value_capacity * sizeof(u32));
Copy(_m);
return *this;
}
inline void b3SparseSymMat33::Copy(const b3SparseSymMat33& _m)
{
B3_ASSERT(M == _m.M);
B3_ASSERT(value_capacity == _m.value_capacity);
B3_ASSERT(value_count == _m.value_count);
memcpy(row_ptrs, _m.row_ptrs, (M + 1) * sizeof(u32));
memcpy(values, _m.values, value_count * sizeof(b3Mat33));
memcpy(value_columns, _m.value_columns, value_count * sizeof(u32));
}
inline const b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j) const
{
B3_ASSERT(i < M);
@ -166,6 +247,44 @@ inline b3Mat33& b3SparseSymMat33::operator()(u32 i, u32 j)
return values[row_value_begin + row_value_position];
}
inline void b3SparseSymMat33::operator+=(const b3SparseSymMat33& m)
{
B3_ASSERT(M == m.M);
for (u32 row = 0; row < m.M; ++row)
{
u32 row_value_begin = m.row_ptrs[row];
u32 row_value_count = m.row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = m.value_columns[row_value_index];
(*this)(row, row_value_column) += m.values[row_value_index];
}
}
}
inline void b3SparseSymMat33::operator-=(const b3SparseSymMat33& m)
{
B3_ASSERT(M == m.M);
for (u32 row = 0; row < m.M; ++row)
{
u32 row_value_begin = m.row_ptrs[row];
u32 row_value_count = m.row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = m.value_columns[row_value_index];
(*this)(row, row_value_column) -= m.values[row_value_index];
}
}
}
inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const
{
B3_ASSERT(M == out.n);
@ -175,6 +294,18 @@ inline void b3SparseSymMat33::Diagonal(b3DiagMat33& out) const
}
}
inline void b3Add(b3SparseSymMat33& out, const b3SparseSymMat33& a, const b3SparseSymMat33& b)
{
out = a;
out += b;
}
inline void b3Sub(b3SparseSymMat33& out, const b3SparseSymMat33& a, const b3SparseSymMat33& b)
{
out = a;
out -= b;
}
inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3& v)
{
B3_ASSERT(A.M == out.n);
@ -202,6 +333,51 @@ inline void b3Mul(b3DenseVec3& out, const b3SparseSymMat33& A, const b3DenseVec3
}
}
inline void b3Mul(b3SparseSymMat33& out, float32 A, const b3SparseSymMat33& B)
{
B3_ASSERT(out.M == B.M);
if (A == 0.0f)
{
return;
}
for (u32 row = 0; row < B.M; ++row)
{
u32 row_value_begin = B.row_ptrs[row];
u32 row_value_count = B.row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = B.value_columns[row_value_index];
out(row, row_value_column) = A * B.values[row_value_index];
}
}
}
inline b3SparseSymMat33 operator+(const b3SparseSymMat33& A, const b3SparseSymMat33& B)
{
b3SparseSymMat33 result(A.M);
b3Add(result, A, B);
return result;
}
inline b3SparseSymMat33 operator-(const b3SparseSymMat33& A, const b3SparseSymMat33& B)
{
b3SparseSymMat33 result(A.M);
b3Sub(result, A, B);
return result;
}
inline b3SparseSymMat33 operator*(float32 A, const b3SparseSymMat33& B)
{
b3SparseSymMat33 result(B.M);
b3Mul(result, A, B);
return result;
}
inline b3DenseVec3 operator*(const b3SparseSymMat33& A, const b3DenseVec3& v)
{
b3DenseVec3 result(v.n);

80
src/bounce/dynamics/cloth/cloth_solver.cpp
View File

@ -205,6 +205,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
m_solverData.dt = dt;
m_solverData.invdt = 1.0f / dt;
float32 h = dt;
for (u32 i = 0; i < m_particleCount; ++i)
{
b3Particle* p = m_particles[i];
@ -240,26 +242,26 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
// Solve Ax = b, where
// A = M - h * dfdv - h * h * dfdx
// b = h * (f0 + h * dfdx * v0 + dfdx * y)
// A
b3SparseSymMat33 A(m_particleCount);
for (u32 i = 0; i < m_particleCount; ++i)
{
A(i, i) = b3Diagonal(m_particles[i]->m_mass);
}
A += -h * dfdv - h * h * dfdx;
// b
b3DenseVec3 b(m_particleCount);
Compute_A_b(A, b);
b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy);
// x
b3DenseVec3 x(m_particleCount);
// Solve Ax = b
u32 iterations = 0;
Solve(x, iterations, A, b, S, z, sx0);
b3_clothSolverIterations = iterations;
// Compute the new state
// Clamp large translations?
float32 h = dt;
sv = sv + x;
sx = sx + h * sv + sy;
@ -296,68 +298,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
}
}
void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const
{
B3_PROFILE("Compute A, b");
b3DenseVec3& x = *m_solverData.x;
b3DenseVec3& v = *m_solverData.v;
b3DenseVec3& f = *m_solverData.f;
b3DenseVec3& y = *m_solverData.y;
b3SparseSymMat33& dfdx = *m_solverData.dfdx;
b3SparseSymMat33& dfdv = *m_solverData.dfdv;
float32 h = m_solverData.dt;
// Compute A
// A = M - h * dfdv - h * h * dfdx
// A = 0
//A.SetZero();
// A += M
for (u32 i = 0; i < m_particleCount; ++i)
{
A(i, i) += b3Diagonal(m_particles[i]->m_mass);
}
// A += - h * dfdv - h * h * dfdx
// A += - h * dfdv
for (u32 row = 0; row < dfdv.M; ++row)
{
u32 row_value_begin = dfdv.row_ptrs[row];
u32 row_value_count = dfdv.row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = dfdv.value_columns[row_value_index];
A(row, row_value_column) += -h * dfdv.values[row_value_index];
}
}
// A += - h * h * dfdx
for (u32 row = 0; row < dfdx.M; ++row)
{
u32 row_value_begin = dfdx.row_ptrs[row];
u32 row_value_count = dfdx.row_ptrs[row + 1] - row_value_begin;
for (u32 row_value = 0; row_value < row_value_count; ++row_value)
{
u32 row_value_index = row_value_begin + row_value;
u32 row_value_column = dfdx.value_columns[row_value_index];
A(row, row_value_column) += -h * h * dfdx.values[row_value_index];
}
}
// Compute b
// b = h * (f0 + h * dfdx * v + dfdx * y)
b = h * (f + h * (dfdx * v) + dfdx * y);
}
void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
{