From aec685f7367dabad488483bca810872989dceaaf Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Mon, 2 Apr 2018 12:46:56 -0300 Subject: [PATCH] add dense vector and sparse matrix --- include/bounce/dynamics/cloth/dense_vec3.h | 174 +++++++++++++++++++ include/bounce/dynamics/cloth/sparse_mat33.h | 151 ++++++++++++++++ 2 files changed, 325 insertions(+) create mode 100644 include/bounce/dynamics/cloth/dense_vec3.h create mode 100644 include/bounce/dynamics/cloth/sparse_mat33.h diff --git a/include/bounce/dynamics/cloth/dense_vec3.h b/include/bounce/dynamics/cloth/dense_vec3.h new file mode 100644 index 0000000..34820e1 --- /dev/null +++ b/include/bounce/dynamics/cloth/dense_vec3.h @@ -0,0 +1,174 @@ +/* +* 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_DENSE_VEC_3_H +#define B3_DENSE_VEC_3_H + +#include + +struct b3DenseVec3 +{ + b3DenseVec3() + { + n = 0; + v = nullptr; + } + + b3DenseVec3(u32 _n) + { + n = _n; + v = (b3Vec3*)b3Alloc(n * sizeof(b3Vec3)); + } + + b3DenseVec3(const b3DenseVec3& _v) + { + n = _v.n; + v = (b3Vec3*)b3Alloc(n * sizeof(b3Vec3)); + + Copy(_v); + } + + ~b3DenseVec3() + { + b3Free(v); + } + + const b3Vec3& operator[](u32 i) const + { + B3_ASSERT(i < n); + return v[i]; + } + + b3Vec3& operator[](u32 i) + { + B3_ASSERT(i < n); + return v[i]; + } + + b3DenseVec3& operator=(const b3DenseVec3& _v) + { + if (_v.v == v) + { + return *this; + } + + if (n == _v.n) + { + Copy(_v); + return *this; + } + + b3Free(v); + + n = _v.n; + v = (b3Vec3*)b3Alloc(n * sizeof(b3Vec3)); + + Copy(_v); + + return *this; + } + + void Copy(const b3DenseVec3& _v) + { + B3_ASSERT(n == _v.n); + memcpy(v, _v.v, n * sizeof(b3Vec3)); + } + + void SetZero() + { + for (u32 i = 0; i < n; ++i) + { + v[i].SetZero(); + } + } + + b3Vec3* v; + u32 n; +}; + +inline void b3Add(b3DenseVec3& out, const b3DenseVec3& a, const b3DenseVec3& b) +{ + B3_ASSERT(out.n == a.n && a.n == b.n); + + for (u32 i = 0; i < a.n; ++i) + { + out[i] = a[i] + b[i]; + } +} + +inline void b3Sub(b3DenseVec3& out, const b3DenseVec3& a, const b3DenseVec3& b) +{ + B3_ASSERT(out.n == a.n && a.n == b.n); + + for (u32 i = 0; i < a.n; ++i) + { + out[i] = a[i] - b[i]; + } +} + +inline void b3Mul(b3DenseVec3& out, float32 a, const b3DenseVec3& b) +{ + B3_ASSERT(out.n == b.n); + + for (u32 i = 0; i < b.n; ++i) + { + out[i] = a * b[i]; + } +} + +inline void b3Negate(b3DenseVec3& out, const b3DenseVec3& v) +{ + b3Mul(out, -1.0f, v); +} + +inline float32 b3Dot(const b3DenseVec3& a, const b3DenseVec3& b) +{ + B3_ASSERT(a.n == b.n); + + float32 result = 0.0f; + + for (u32 i = 0; i < a.n; ++i) + { + result += b3Dot(a[i], b[i]); + } + + return result; +} + +inline b3DenseVec3 operator+(const b3DenseVec3& a, const b3DenseVec3& b) +{ + b3DenseVec3 result(a.n); + b3Add(result, a, b); + return result; +} + +inline b3DenseVec3 operator-(const b3DenseVec3& a, const b3DenseVec3& b) +{ + b3DenseVec3 result(a.n); + b3Sub(result, a, b); + return result; +} + +inline b3DenseVec3 operator*(float32 a, const b3DenseVec3& b) +{ + b3DenseVec3 result(b.n); + b3Mul(result, a, b); + return result; +} + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/sparse_mat33.h b/include/bounce/dynamics/cloth/sparse_mat33.h new file mode 100644 index 0000000..e8ec392 --- /dev/null +++ b/include/bounce/dynamics/cloth/sparse_mat33.h @@ -0,0 +1,151 @@ +/* +* 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_SPARSE_MAT_33_H +#define B3_SPARSE_MAT_33_H + +#include +#include + +// A static sparse matrix stored in in Compressed Sparse Row (CSR) format. +// It's efficient when using in iterative solvers such as CG method, where +// the coefficient matrix must be multiplied with a vector at each iteration. +// See https://en.wikipedia.org/wiki/Sparse_matrix +struct b3SparseMat33 +{ + b3SparseMat33() { } + + b3SparseMat33(u32 _M, u32 _N, + u32 _valueCount, b3Mat33* _values, + u32* _row_ptrs, u32* _cols) + { + M = _M; + N = _N; + + values = _values; + valueCount = _valueCount; + + row_ptrs = _row_ptrs; + cols = _cols; + } + + ~b3SparseMat33() + { + + } + + // Output any given row of the original matrix. + // The given buffer must have size of greater or equal than M. + void AssembleRow(b3Mat33* out, u32 row) const; + + // Decompresses this matrix into its original form. + // The output matrix is stored in row-major order. + // The given buffer must have size of greater or equal than M * N. + void AssembleMatrix(b3Mat33* out) const; + + // Output the block diagonal part of the original matrix. + // This matrix must be a square matrix. + // The given buffer must have size of greater or equal than M. + void AssembleDiagonal(b3Mat33* out) const; + + // Multiplies this matrix with a given (compatible) vector. + void Mul(b3DenseVec3& out, const b3DenseVec3& v) const; + + // Dimensions of the original 2D matrix + u32 M; + u32 N; + + // Non-zero values + b3Mat33* values; + u32 valueCount; + + // Sparsity structure + u32* row_ptrs; // pointers to the first non-zero value of each row (size is M + 1) + u32* cols; // column indices for each non-zero value (size is valueCount) +}; + +inline void b3SparseMat33::AssembleRow(b3Mat33* out, u32 row) const +{ + B3_ASSERT(row < M + 1); + + // Start with zero row + for (u32 i = 0; i < N; ++i) + { + out[i].SetZero(); + } + + for (u32 i = row_ptrs[row]; i < row_ptrs[row + 1]; ++i) + { + u32 col = cols[i]; + + out[col] = values[i]; + } +} + +inline void b3SparseMat33::AssembleMatrix(b3Mat33* out) const +{ + for (u32 i = 0; i < M; ++i) + { + AssembleRow(out + i * N, i); + } +} + +inline void b3SparseMat33::AssembleDiagonal(b3Mat33* out) const +{ + B3_ASSERT(M == N); + + for (u32 row = 0; row < M; ++row) + { + out[row].SetZero(); + + for (u32 row_ptr = row_ptrs[row]; row_ptr < row_ptrs[row + 1]; ++row_ptr) + { + if (cols[row_ptr] == row) + { + out[row] = values[row_ptr]; + break; + } + } + } +} + +inline void b3SparseMat33::Mul(b3DenseVec3& out, const b3DenseVec3& v) const +{ + B3_ASSERT(N == out.n); + + for (u32 i = 0; i < N; ++i) + { + out[i].SetZero(); + + for (u32 j = row_ptrs[i]; j < row_ptrs[i + 1]; ++j) + { + u32 col = cols[j]; + + out[i] += values[j] * v[col]; + } + } +} + +inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v) +{ + b3DenseVec3 result(v.n); + A.Mul(result, v); + return result; +} + +#endif \ No newline at end of file