void b2Rope::Initialize(const b2RopeDef* def)
{
b2Assert(def->count >= 3);
m_count = def->count;
m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
m_ims = (float32*)b2Alloc(m_count * sizeof(float32));
for (int32 i = 0; i < m_count; ++i)
{
m_ps[i] = def->vertices[i];
m_p0s[i] = def->vertices[i];
m_vs[i].SetZero();
float32 m = def->masses[i];
if (m > 0.0f)
{
m_ims[i] = 1.0f / m;
}
else
{
m_ims[i] = 0.0f;
}
}
int32 count2 = m_count - 1;
int32 count3 = m_count - 2;
m_Ls = (float32*)b2Alloc(count2 * sizeof(float32));
m_as = (float32*)b2Alloc(count3 * sizeof(float32));
for (int32 i = 0; i < count2; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i+1];
m_Ls[i] = b2Distance(p1, p2);
}
for (int32 i = 0; i < count3; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i + 1];
b2Vec2 p3 = m_ps[i + 2];
b2Vec2 d1 = p2 - p1;
b2Vec2 d2 = p3 - p2;
float32 a = b2Cross(d1, d2);
float32 b = b2Dot(d1, d2);
m_as[i] = b2Atan2(a, b);
}
m_gravity = def->gravity;
m_damping = def->damping;
m_k2 = def->k2;
m_k3 = def->k3;
}
float32 connectEdges(b2EdgeShape * const & s1, b2EdgeShape * const & s2, float32 angle1)
{
float32 angle2 = b2Atan2(s2->GetDirectionVector().y, s2->GetDirectionVector().x);
b2Vec2 core = tanf((angle2 - angle1) * 0.5f) * s2->GetDirectionVector();
core = s1->settings->b2_toiSlop * (core - s2->GetNormalVector()) + s2->GetVertex1();
b2Vec2 cornerDir = s1->GetDirectionVector() + s2->GetDirectionVector();
cornerDir.Normalize();
bool convex = b2Dot(s1->GetDirectionVector(), s2->GetNormalVector()) > 0.0f;
s1->SetNextEdge(s2, core, cornerDir, convex);
s2->SetPrevEdge(s1, core, cornerDir, convex);
return angle2;
}
void b2Rope::SolveC3()
{
int32 count3 = m_count - 2;
for (int32 i = 0; i < count3; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i + 1];
b2Vec2 p3 = m_ps[i + 2];
float32 m1 = m_ims[i];
float32 m2 = m_ims[i + 1];
float32 m3 = m_ims[i + 2];
b2Vec2 d1 = p2 - p1;
b2Vec2 d2 = p3 - p2;
float32 L1sqr = d1.LengthSquared();
float32 L2sqr = d2.LengthSquared();
if (L1sqr * L2sqr == 0.0f)
{
continue;
}
float32 a = b2Cross(d1, d2);
float32 b = b2Dot(d1, d2);
float32 angle = b2Atan2(a, b);
b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
b2Vec2 J1 = -Jd1;
b2Vec2 J2 = Jd1 - Jd2;
b2Vec2 J3 = Jd2;
float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3);
if (mass == 0.0f)
{
continue;
}
mass = 1.0f / mass;
float32 C = angle - m_as[i];
while (C > b2_pi)
{
angle -= 2 * b2_pi;
C = angle - m_as[i];
}
while (C < -b2_pi)
{
angle += 2.0f * b2_pi;
C = angle - m_as[i];
}
float32 impulse = - m_k3 * mass * C;
p1 += (m1 * impulse) * J1;
p2 += (m2 * impulse) * J2;
p3 += (m3 * impulse) * J3;
m_ps[i] = p1;
m_ps[i + 1] = p2;
m_ps[i + 2] = p3;
}
}
static float32 vec2Angle( b2Vec2 v )
{
return b2Atan2(v.y, v.x);
}
b2Shape* b2Body::CreateShape(b2ShapeDef* def)
{
b2Assert(m_world->m_lock == false);
if (m_world->m_lock == true)
{
return NULL;
}
// TODO: Decide on a better place to initialize edgeShapes. (b2Shape::Create() can't
// return more than one shape to add to parent body... maybe it should add
// shapes directly to the body instead of returning them?)
if (def->type == e_edgeShape) {
b2EdgeChainDef* edgeDef = (b2EdgeChainDef*)def;
b2Vec2 v1;
b2Vec2 v2;
int i;
if (edgeDef->isALoop) {
v1 = edgeDef->vertices[edgeDef->vertexCount-1];
i = 0;
} else {
v1 = edgeDef->vertices[0];
i = 1;
}
b2EdgeShape* s0 = NULL;
b2EdgeShape* s1 = NULL;
b2EdgeShape* s2 = NULL;
float32 angle = 0.0f;
for (; i < edgeDef->vertexCount; i++) {
v2 = edgeDef->vertices[i];
void* mem = m_world->m_blockAllocator.Allocate(sizeof(b2EdgeShape));
s2 = new (mem) b2EdgeShape(v1, v2, def, settings);
if(m_lastShape)
m_lastShape->m_next = s2;
if(!m_shapeList)
m_shapeList = s2;
m_lastShape = s2;
++m_shapeCount;
s2->m_body = this;
s2->CreateProxy(m_world->m_broadPhase, m_xf);
s2->UpdateSweepRadius(m_sweep.localCenter);
if (s1 == NULL) {
s0 = s2;
angle = b2Atan2(s2->GetDirectionVector().y, s2->GetDirectionVector().x);
} else {
angle = connectEdges(s1, s2, angle);
}
s1 = s2;
v1 = v2;
}
if (edgeDef->isALoop) connectEdges(s1, s0, angle);
return s0;
}
b2Shape* s = b2Shape::Create(def, &m_world->m_blockAllocator, settings);
if(m_lastShape)
m_lastShape->m_next = s;
if(!m_shapeList)
m_shapeList = s;
m_lastShape = s;
++m_shapeCount;
s->m_body = this;
// Add the shape to the world's broad-phase.
s->CreateProxy(m_world->m_broadPhase, m_xf);
// Compute the sweep radius for CCD.
s->UpdateSweepRadius(m_sweep.localCenter);
return s;
}
