Warning, file /education/gcompris/external/qml-box2d/Box2D/Rope/b2Rope.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
0001 /* 0002 * Copyright (c) 2011 Erin Catto http://box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Rope/b2Rope.h> 0020 #include <Box2D/Common/b2Draw.h> 0021 0022 b2Rope::b2Rope() 0023 { 0024 m_count = 0; 0025 m_ps = NULL; 0026 m_p0s = NULL; 0027 m_vs = NULL; 0028 m_ims = NULL; 0029 m_Ls = NULL; 0030 m_as = NULL; 0031 m_gravity.SetZero(); 0032 m_k2 = 1.0f; 0033 m_k3 = 0.1f; 0034 } 0035 0036 b2Rope::~b2Rope() 0037 { 0038 b2Free(m_ps); 0039 b2Free(m_p0s); 0040 b2Free(m_vs); 0041 b2Free(m_ims); 0042 b2Free(m_Ls); 0043 b2Free(m_as); 0044 } 0045 0046 void b2Rope::Initialize(const b2RopeDef* def) 0047 { 0048 b2Assert(def->count >= 3); 0049 m_count = def->count; 0050 m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); 0051 m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); 0052 m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); 0053 m_ims = (float32*)b2Alloc(m_count * sizeof(float32)); 0054 0055 for (int32 i = 0; i < m_count; ++i) 0056 { 0057 m_ps[i] = def->vertices[i]; 0058 m_p0s[i] = def->vertices[i]; 0059 m_vs[i].SetZero(); 0060 0061 float32 m = def->masses[i]; 0062 if (m > 0.0f) 0063 { 0064 m_ims[i] = 1.0f / m; 0065 } 0066 else 0067 { 0068 m_ims[i] = 0.0f; 0069 } 0070 } 0071 0072 int32 count2 = m_count - 1; 0073 int32 count3 = m_count - 2; 0074 m_Ls = (float32*)b2Alloc(count2 * sizeof(float32)); 0075 m_as = (float32*)b2Alloc(count3 * sizeof(float32)); 0076 0077 for (int32 i = 0; i < count2; ++i) 0078 { 0079 b2Vec2 p1 = m_ps[i]; 0080 b2Vec2 p2 = m_ps[i+1]; 0081 m_Ls[i] = b2Distance(p1, p2); 0082 } 0083 0084 for (int32 i = 0; i < count3; ++i) 0085 { 0086 b2Vec2 p1 = m_ps[i]; 0087 b2Vec2 p2 = m_ps[i + 1]; 0088 b2Vec2 p3 = m_ps[i + 2]; 0089 0090 b2Vec2 d1 = p2 - p1; 0091 b2Vec2 d2 = p3 - p2; 0092 0093 float32 a = b2Cross(d1, d2); 0094 float32 b = b2Dot(d1, d2); 0095 0096 m_as[i] = b2Atan2(a, b); 0097 } 0098 0099 m_gravity = def->gravity; 0100 m_damping = def->damping; 0101 m_k2 = def->k2; 0102 m_k3 = def->k3; 0103 } 0104 0105 void b2Rope::Step(float32 h, int32 iterations) 0106 { 0107 if (h == 0.0) 0108 { 0109 return; 0110 } 0111 0112 float32 d = expf(- h * m_damping); 0113 0114 for (int32 i = 0; i < m_count; ++i) 0115 { 0116 m_p0s[i] = m_ps[i]; 0117 if (m_ims[i] > 0.0f) 0118 { 0119 m_vs[i] += h * m_gravity; 0120 } 0121 m_vs[i] *= d; 0122 m_ps[i] += h * m_vs[i]; 0123 0124 } 0125 0126 for (int32 i = 0; i < iterations; ++i) 0127 { 0128 SolveC2(); 0129 SolveC3(); 0130 SolveC2(); 0131 } 0132 0133 float32 inv_h = 1.0f / h; 0134 for (int32 i = 0; i < m_count; ++i) 0135 { 0136 m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); 0137 } 0138 } 0139 0140 void b2Rope::SolveC2() 0141 { 0142 int32 count2 = m_count - 1; 0143 0144 for (int32 i = 0; i < count2; ++i) 0145 { 0146 b2Vec2 p1 = m_ps[i]; 0147 b2Vec2 p2 = m_ps[i + 1]; 0148 0149 b2Vec2 d = p2 - p1; 0150 float32 L = d.Normalize(); 0151 0152 float32 im1 = m_ims[i]; 0153 float32 im2 = m_ims[i + 1]; 0154 0155 if (im1 + im2 == 0.0f) 0156 { 0157 continue; 0158 } 0159 0160 float32 s1 = im1 / (im1 + im2); 0161 float32 s2 = im2 / (im1 + im2); 0162 0163 p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; 0164 p2 += m_k2 * s2 * (m_Ls[i] - L) * d; 0165 0166 m_ps[i] = p1; 0167 m_ps[i + 1] = p2; 0168 } 0169 } 0170 0171 void b2Rope::SetAngle(float32 angle) 0172 { 0173 int32 count3 = m_count - 2; 0174 for (int32 i = 0; i < count3; ++i) 0175 { 0176 m_as[i] = angle; 0177 } 0178 } 0179 0180 void b2Rope::SolveC3() 0181 { 0182 int32 count3 = m_count - 2; 0183 0184 for (int32 i = 0; i < count3; ++i) 0185 { 0186 b2Vec2 p1 = m_ps[i]; 0187 b2Vec2 p2 = m_ps[i + 1]; 0188 b2Vec2 p3 = m_ps[i + 2]; 0189 0190 float32 m1 = m_ims[i]; 0191 float32 m2 = m_ims[i + 1]; 0192 float32 m3 = m_ims[i + 2]; 0193 0194 b2Vec2 d1 = p2 - p1; 0195 b2Vec2 d2 = p3 - p2; 0196 0197 float32 L1sqr = d1.LengthSquared(); 0198 float32 L2sqr = d2.LengthSquared(); 0199 0200 if (L1sqr * L2sqr == 0.0f) 0201 { 0202 continue; 0203 } 0204 0205 float32 a = b2Cross(d1, d2); 0206 float32 b = b2Dot(d1, d2); 0207 0208 float32 angle = b2Atan2(a, b); 0209 0210 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); 0211 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); 0212 0213 b2Vec2 J1 = -Jd1; 0214 b2Vec2 J2 = Jd1 - Jd2; 0215 b2Vec2 J3 = Jd2; 0216 0217 float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); 0218 if (mass == 0.0f) 0219 { 0220 continue; 0221 } 0222 0223 mass = 1.0f / mass; 0224 0225 float32 C = angle - m_as[i]; 0226 0227 while (C > b2_pi) 0228 { 0229 angle -= 2 * b2_pi; 0230 C = angle - m_as[i]; 0231 } 0232 0233 while (C < -b2_pi) 0234 { 0235 angle += 2.0f * b2_pi; 0236 C = angle - m_as[i]; 0237 } 0238 0239 float32 impulse = - m_k3 * mass * C; 0240 0241 p1 += (m1 * impulse) * J1; 0242 p2 += (m2 * impulse) * J2; 0243 p3 += (m3 * impulse) * J3; 0244 0245 m_ps[i] = p1; 0246 m_ps[i + 1] = p2; 0247 m_ps[i + 2] = p3; 0248 } 0249 } 0250 0251 void b2Rope::Draw(b2Draw* draw) const 0252 { 0253 b2Color c(0.4f, 0.5f, 0.7f); 0254 0255 for (int32 i = 0; i < m_count - 1; ++i) 0256 { 0257 draw->DrawSegment(m_ps[i], m_ps[i+1], c); 0258 } 0259 }