Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
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 }