b2Rope.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2011 Erin Catto http://box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Rope/b2Rope.h>
00020 #include <Box2D/Common/b2Draw.h>
00021 
00022 b2Rope::b2Rope()
00023 {
00024         m_count = 0;
00025         m_ps = NULL;
00026         m_p0s = NULL;
00027         m_vs = NULL;
00028         m_ims = NULL;
00029         m_Ls = NULL;
00030         m_as = NULL;
00031         m_gravity.SetZero();
00032         m_k2 = 1.0f;
00033         m_k3 = 0.1f;
00034 }
00035 
00036 b2Rope::~b2Rope()
00037 {
00038         b2Free(m_ps);
00039         b2Free(m_p0s);
00040         b2Free(m_vs);
00041         b2Free(m_ims);
00042         b2Free(m_Ls);
00043         b2Free(m_as);
00044 }
00045 
00046 void b2Rope::Initialize(const b2RopeDef* def)
00047 {
00048         b2Assert(def->count >= 3);
00049         m_count = def->count;
00050         m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
00051         m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
00052         m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
00053         m_ims = (float32*)b2Alloc(m_count * sizeof(float32));
00054 
00055         for (int32 i = 0; i < m_count; ++i)
00056         {
00057                 m_ps[i] = def->vertices[i];
00058                 m_p0s[i] = def->vertices[i];
00059                 m_vs[i].SetZero();
00060 
00061                 float32 m = def->masses[i];
00062                 if (m > 0.0f)
00063                 {
00064                         m_ims[i] = 1.0f / m;
00065                 }
00066                 else
00067                 {
00068                         m_ims[i] = 0.0f;
00069                 }
00070         }
00071 
00072         int32 count2 = m_count - 1;
00073         int32 count3 = m_count - 2;
00074         m_Ls = (float32*)b2Alloc(count2 * sizeof(float32));
00075         m_as = (float32*)b2Alloc(count3 * sizeof(float32));
00076 
00077         for (int32 i = 0; i < count2; ++i)
00078         {
00079                 b2Vec2 p1 = m_ps[i];
00080                 b2Vec2 p2 = m_ps[i+1];
00081                 m_Ls[i] = b2Distance(p1, p2);
00082         }
00083 
00084         for (int32 i = 0; i < count3; ++i)
00085         {
00086                 b2Vec2 p1 = m_ps[i];
00087                 b2Vec2 p2 = m_ps[i + 1];
00088                 b2Vec2 p3 = m_ps[i + 2];
00089 
00090                 b2Vec2 d1 = p2 - p1;
00091                 b2Vec2 d2 = p3 - p2;
00092 
00093                 float32 a = b2Cross(d1, d2);
00094                 float32 b = b2Dot(d1, d2);
00095 
00096                 m_as[i] = b2Atan2(a, b);
00097         }
00098 
00099         m_gravity = def->gravity;
00100         m_damping = def->damping;
00101         m_k2 = def->k2;
00102         m_k3 = def->k3;
00103 }
00104 
00105 void b2Rope::Step(float32 h, int32 iterations)
00106 {
00107         if (h == 0.0)
00108         {
00109                 return;
00110         }
00111 
00112         float32 d = expf(- h * m_damping);
00113 
00114         for (int32 i = 0; i < m_count; ++i)
00115         {
00116                 m_p0s[i] = m_ps[i];
00117                 if (m_ims[i] > 0.0f)
00118                 {
00119                         m_vs[i] += h * m_gravity;
00120                 }
00121                 m_vs[i] *= d;
00122                 m_ps[i] += h * m_vs[i];
00123 
00124         }
00125 
00126         for (int32 i = 0; i < iterations; ++i)
00127         {
00128                 SolveC2();
00129                 SolveC3();
00130                 SolveC2();
00131         }
00132 
00133         float32 inv_h = 1.0f / h;
00134         for (int32 i = 0; i < m_count; ++i)
00135         {
00136                 m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]);
00137         }
00138 }
00139 
00140 void b2Rope::SolveC2()
00141 {
00142         int32 count2 = m_count - 1;
00143 
00144         for (int32 i = 0; i < count2; ++i)
00145         {
00146                 b2Vec2 p1 = m_ps[i];
00147                 b2Vec2 p2 = m_ps[i + 1];
00148 
00149                 b2Vec2 d = p2 - p1;
00150                 float32 L = d.Normalize();
00151 
00152                 float32 im1 = m_ims[i];
00153                 float32 im2 = m_ims[i + 1];
00154 
00155                 if (im1 + im2 == 0.0f)
00156                 {
00157                         continue;
00158                 }
00159 
00160                 float32 s1 = im1 / (im1 + im2);
00161                 float32 s2 = im2 / (im1 + im2);
00162 
00163                 p1 -= m_k2 * s1 * (m_Ls[i] - L) * d;
00164                 p2 += m_k2 * s2 * (m_Ls[i] - L) * d;
00165 
00166                 m_ps[i] = p1;
00167                 m_ps[i + 1] = p2;
00168         }
00169 }
00170 
00171 void b2Rope::SetAngle(float32 angle)
00172 {
00173         int32 count3 = m_count - 2;
00174         for (int32 i = 0; i < count3; ++i)
00175         {
00176                 m_as[i] = angle;
00177         }
00178 }
00179 
00180 void b2Rope::SolveC3()
00181 {
00182         int32 count3 = m_count - 2;
00183 
00184         for (int32 i = 0; i < count3; ++i)
00185         {
00186                 b2Vec2 p1 = m_ps[i];
00187                 b2Vec2 p2 = m_ps[i + 1];
00188                 b2Vec2 p3 = m_ps[i + 2];
00189 
00190                 float32 m1 = m_ims[i];
00191                 float32 m2 = m_ims[i + 1];
00192                 float32 m3 = m_ims[i + 2];
00193 
00194                 b2Vec2 d1 = p2 - p1;
00195                 b2Vec2 d2 = p3 - p2;
00196 
00197                 float32 L1sqr = d1.LengthSquared();
00198                 float32 L2sqr = d2.LengthSquared();
00199 
00200                 if (L1sqr * L2sqr == 0.0f)
00201                 {
00202                         continue;
00203                 }
00204 
00205                 float32 a = b2Cross(d1, d2);
00206                 float32 b = b2Dot(d1, d2);
00207 
00208                 float32 angle = b2Atan2(a, b);
00209 
00210                 b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
00211                 b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
00212 
00213                 b2Vec2 J1 = -Jd1;
00214                 b2Vec2 J2 = Jd1 - Jd2;
00215                 b2Vec2 J3 = Jd2;
00216 
00217                 float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3);
00218                 if (mass == 0.0f)
00219                 {
00220                         continue;
00221                 }
00222 
00223                 mass = 1.0f / mass;
00224 
00225                 float32 C = angle - m_as[i];
00226 
00227                 while (C > b2_pi)
00228                 {
00229                         angle -= 2 * b2_pi;
00230                         C = angle - m_as[i];
00231                 }
00232 
00233                 while (C < -b2_pi)
00234                 {
00235                         angle += 2.0f * b2_pi;
00236                         C = angle - m_as[i];
00237                 }
00238 
00239                 float32 impulse = - m_k3 * mass * C;
00240 
00241                 p1 += (m1 * impulse) * J1;
00242                 p2 += (m2 * impulse) * J2;
00243                 p3 += (m3 * impulse) * J3;
00244 
00245                 m_ps[i] = p1;
00246                 m_ps[i + 1] = p2;
00247                 m_ps[i + 2] = p3;
00248         }
00249 }
00250 
00251 void b2Rope::Draw(b2Draw* draw) const
00252 {
00253         b2Color c(0.4f, 0.5f, 0.7f);
00254 
00255         for (int32 i = 0; i < m_count - 1; ++i)
00256         {
00257                 draw->DrawSegment(m_ps[i], m_ps[i+1], c);
00258         }
00259 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35