From 41aa214a401ff3892dd237a6b343abe1cb05a8e5 Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Fri, 23 Mar 2018 15:27:41 -0300 Subject: [PATCH] add an experiment --- examples/testbed/framework/test_entries.cpp | 8 +- examples/testbed/tests/implicit_spring.h | 177 ++++++++++++++++++++ 2 files changed, 183 insertions(+), 2 deletions(-) create mode 100644 examples/testbed/tests/implicit_spring.h diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index d2a31b9..a704118 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -58,8 +58,10 @@ #include #include #include -#include #include +#include +#include +//#include //#include TestEntry g_tests[] = @@ -104,8 +106,10 @@ TestEntry g_tests[] = { "Initial Overlap", &InitialOverlap::Create }, { "Single Pendulum", &SinglePendulum::Create }, { "Multiple Pendulum", &MultiplePendulum::Create }, - { "Cloth", &Cloth::Create }, { "Rope", &Rope::Create }, + { "Implicit Spring", &ImplicitSpring::Create }, + { "Cloth", &Cloth::Create }, + //{ "Spring Cloth", &SpringCloth::Create }, //{ "Tree", &Tree::Create }, { NULL, NULL } }; \ No newline at end of file diff --git a/examples/testbed/tests/implicit_spring.h b/examples/testbed/tests/implicit_spring.h new file mode 100644 index 0000000..a057509 --- /dev/null +++ b/examples/testbed/tests/implicit_spring.h @@ -0,0 +1,177 @@ +/* +* 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 IMPLICIT_SPRING_H +#define IMPLICIT_SPRING_H + +extern DebugDraw* g_debugDraw; +extern Camera g_camera; +extern Settings g_settings; + +class ImplicitSpring : public Test +{ +public: + ImplicitSpring() + { + g_camera.m_zoom = 20.0f; + + m_x.Set(0.0f, 5.0f, 0.0f); + + m_v.SetZero(); + + m_k = 100.0f; + + m_iterations = 0; + } + + void Solve(float32 h) + { + // ODE + // f(Y) = dY / dt = [v] + // [-k * x] + + // 1. Apply Implicit Euler + + // Y(t + h) = Y(t) + h * f( Y(t + h) ) + // G( Y(t + h) ) = Y(t + h) - Y(t) - h * f( Y(t + h) ) = 0 + + // 2. Solve G = 0 + + // Newton-Raphson Iteration + // + // Y(t + h) = + // Y(t + h)_0 - G( Y(t + h)_0 ) / G'( Y(t + h)_0 ) = + // Y(t + h)_0 - G'( Y(t + h)_0 )^-1 * ( Y(t + h)_0 - Y(t) - h * f( Y(t + h)_0 ) + + // G'( Y ) = I - h * del_f / del_Y + + // del_f / del_Y = [del_f1 / del_x del_f1 / del_v] = [0 I] + // [del_f2 / del_x del_f2 / del_v] [-k * I 0] + + // G'( Y ) = [I 0] - [0 h * I] = [I -h * I] + // [0 I] [-h * k * I 0] [h * k * I I] + + // Compute Jacobian + b3Mat33 I = b3Mat33_identity; + + b3Mat33 A, B, C, D; + + A = I; + B = -h * I; + C = h * m_k * I; + D = I; + + // Invert + // Block matrix inversion + b3Mat33 invD = b3Inverse(D); + b3Mat33 B_invD = B * invD; + + b3Mat33 invJ_A = b3Inverse(A - B_invD * C); + b3Mat33 invJ_B = -invJ_A * B_invD; + b3Mat33 invJ_C = -invD * C * invJ_A; + b3Mat33 invJ_D = invD + invD * C * invJ_A * B_invD; + + // Initial guess + b3Vec3 f1 = m_v; + b3Vec3 f2 = -m_k * m_x; + + b3Vec3 Y1 = m_x + h * f1; + b3Vec3 Y2 = m_v + h * f2; + + const float32 kTol = 0.05f; + + const u32 kMaxIterations = 20; + + float32 eps0 = 0.0f; + + float32 eps1 = B3_MAX_FLOAT; + + m_iterations = 0; + + while (m_iterations < kMaxIterations && eps1 > kTol * kTol * eps0) + { + // Evaluate f(Y_n-1) + f1 = Y2; + f2 = -m_k * Y1; + + // Residual vector + b3Vec3 G1 = Y1 - m_x - h * f1; + b3Vec3 G2 = Y2 - m_v - h * f2; + + eps1 = b3Dot(G1, G1) + b3Dot(G2, G2); + + // Solve Ax = b + b3Vec3 x1 = invJ_A * G1 + invJ_B * G2; + b3Vec3 x2 = invJ_C * G1 + invJ_D * G2; + + Y1 -= x1; + Y2 -= x2; + + ++m_iterations; + } + + // Update state + m_x = Y1; + m_v = Y2; + } + + void Step() + { + float32 h = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f; + + if (g_settings.pause) + { + if (g_settings.singleStep) + { + g_settings.singleStep = false; + } + else + { + h = 0.0f; + } + } + + Solve(h); + + g_debugDraw->DrawSolidSphere(m_x, 0.25f, b3Color_white); + + b3Vec3 b3Vec3_zero; + b3Vec3_zero.SetZero(); + g_debugDraw->DrawSegment(b3Vec3_zero, m_x, b3Color_white); + + char text[64]; + sprintf(text, "Iterations = %u", m_iterations); + g_debugDraw->DrawString(text, b3Color_white); + } + + static Test* Create() + { + return new ImplicitSpring(); + } + + // State + b3Vec3 m_x, m_v; + + // Stiffness + float32 m_k; + + // + u32 m_iterations; +}; + +#endif \ No newline at end of file