/** * Build vertices to represent an oriented box. * * @param hx the half-width. * @param hy the half-height. * @param center the center of the box in local coordinates. * @param angle the rotation of the box in local coordinates. */ public void setAsBox(double hx, double hy, Vec2 center, double angle) { m_count = 4; m_vertices[0].set(-hx, -hy); m_vertices[1].set(hx, -hy); m_vertices[2].set(hx, hy); m_vertices[3].set(-hx, hy); m_normals[0].set(0.0d, -1.0d); m_normals[1].set(1.0d, 0.0d); m_normals[2].set(0.0d, 1.0d); m_normals[3].set(-1.0d, 0.0d); m_centroid.set(center); Transform xf = poolt1; xf.p.set(center); xf.q.set(angle); // Transform vertices and normals. for (int i = 0; i < m_count; ++i) { Transform.mulToOut(xf, m_vertices[i], m_vertices[i]); Rot.mulToOut(xf.q, m_normals[i], m_normals[i]); } }
public void getWorldVectorToOut(Vec2 localVector, Vec2 out_) { Rot.mulToOut(m_xf.q, localVector, out_); }
public static void mulToOut(Transform A, Transform B, Transform out_) { Rot.mul(A.q, B.q, out_.q); Rot.mulToOut(A.q, B.p, out_.p); out_.p.addLocal(A.p); }
public override bool solvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; Rot qA = pool.popRot(); Rot qB = pool.popRot(); Vec2 temp = pool.popVec2(); qA.set(aA); qB.set(aB); Rot.mulToOut(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOut(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d2.set(cB).subLocal(cA).addLocal(rB).subLocal(rA); Vec2 ay = pool.popVec2(); Rot.mulToOut(qA, m_localYAxisA, ay); double sAy = Vec2.cross(temp.set(d2).addLocal(rA), ay); double sBy = Vec2.cross(rB, ay); double C = Vec2.dot(d2, ay); double k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; double impulse; if (k != 0.0d) { impulse = -C / k; } else { impulse = 0.0d; } Vec2 P = pool.popVec2(); P.x = impulse * ay.x; P.y = impulse * ay.y; double LA = impulse * sAy; double LB = impulse * sBy; cA.x -= m_invMassA * P.x; cA.y -= m_invMassA * P.y; aA -= m_invIA * LA; cB.x += m_invMassB * P.x; cB.y += m_invMassB * P.y; aB += m_invIB * LB; pool.pushVec2(3); pool.pushRot(2); // data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; // data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(MathUtils.abs(C) <= Settings.linearSlop); }
// pooling public override void initVelocityConstraints(SolverData data) { m_indexA = m_bodyA.m_islandIndex; m_indexB = m_bodyB.m_islandIndex; m_localCenterA.set(m_bodyA.m_sweep.localCenter); m_localCenterB.set(m_bodyB.m_sweep.localCenter); m_invMassA = m_bodyA.m_invMass; m_invMassB = m_bodyB.m_invMass; m_invIA = m_bodyA.m_invI; m_invIB = m_bodyB.m_invI; double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 vA = data.velocities[m_indexA].v; double wA = data.velocities[m_indexA].w; Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; Vec2 vB = data.velocities[m_indexB].v; double wB = data.velocities[m_indexB].w; Rot qA = pool.popRot(); Rot qB = pool.popRot(); Vec2 temp = pool.popVec2(); qA.set(aA); qB.set(aB); // Compute the effective masses. Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d2.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); // Point to line constraint { Rot.mulToOut(qA, m_localYAxisA, m_ay); m_sAy = Vec2.cross(temp.set(d2).addLocal(rA), m_ay); m_sBy = Vec2.cross(rB, m_ay); m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; if (m_mass > 0.0d) { m_mass = 1.0d / m_mass; } } // Spring constraint m_springMass = 0.0d; m_bias = 0.0d; m_gamma = 0.0d; if (m_frequencyHz > 0.0d) { Rot.mulToOut(qA, m_localXAxisA, m_ax); m_sAx = Vec2.cross(temp.set(d2).addLocal(rA), m_ax); m_sBx = Vec2.cross(rB, m_ax); double invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; if (invMass > 0.0d) { m_springMass = 1.0d / invMass; double C = Vec2.dot(d2, m_ax); // Frequency double omega = 2.0d * MathUtils.PI * m_frequencyHz; // Damping coefficient double d = 2.0d * m_springMass * m_dampingRatio * omega; // Spring stiffness double k = m_springMass * omega * omega; // magic formulas double h = data.step.dt; m_gamma = h * (d + h * k); if (m_gamma > 0.0d) { m_gamma = 1.0d / m_gamma; } m_bias = C * h * k * m_gamma; m_springMass = invMass + m_gamma; if (m_springMass > 0.0d) { m_springMass = 1.0d / m_springMass; } } } else { m_springImpulse = 0.0d; } // Rotational motor if (m_enableMotor) { m_motorMass = iA + iB; if (m_motorMass > 0.0d) { m_motorMass = 1.0d / m_motorMass; } } else { m_motorMass = 0.0d; m_motorImpulse = 0.0d; } if (data.step.warmStarting) { Vec2 P = pool.popVec2(); // Account for variable time step. m_impulse *= data.step.dtRatio; m_springImpulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; P.x = m_impulse * m_ay.x + m_springImpulse * m_ax.x; P.y = m_impulse * m_ay.y + m_springImpulse * m_ax.y; double LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; double LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; vA.x -= m_invMassA * P.x; vA.y -= m_invMassA * P.y; wA -= m_invIA * LA; vB.x += m_invMassB * P.x; vB.y += m_invMassB * P.y; wB += m_invIB * LB; pool.pushVec2(1); } else { m_impulse = 0.0d; m_springImpulse = 0.0d; m_motorImpulse = 0.0d; } pool.pushRot(2); pool.pushVec2(1); // data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; // data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }