static int test_quat_precision() { int Error(0); { glm::f32quat q1; glm::lowp_f32quat qA(q1); glm::mediump_f32quat qB(q1); glm::highp_f32quat qC(q1); glm::f32quat q2(qA); glm::f32quat q3(qB); glm::f32quat q4(qC); Error += glm::all(glm::equal(q1, q2)) ? 0 : 1; Error += glm::all(glm::equal(q1, q3)) ? 0 : 1; Error += glm::all(glm::equal(q1, q4)) ? 0 : 1; } return Error; }
vecN<vecN<vecN<OutputType, 2>, 4>, 2> split_cubicT(c_array<const vecN<InputType, 2> > pts) { FASTUIDRAWassert(pts.size() == 4); vecN<vecN<vecN<OutputType, 2>, 4>, 2> return_value; vecN<IntermediateType, 2> p0(pts[0]), p1(pts[1]), p2(pts[2]), p3(pts[3]); vecN<IntermediateType, 2> p01, p23, pA, pB, pC; const IntermediateType two(2), three(3), four(4), eight(8); p01 = (p0 + p1) / two; p23 = (p2 + p3) / two; pA = (p0 + two * p1 + p2) / four; pB = (p1 + two * p2 + p3) / four; pC = (p0 + three * p1 + three * p2 + p3) / eight; vecN<OutputType, 2> q0(pts[0]), q01(p01), qA(pA), qC(pC); vecN<OutputType, 2> qB(pB), q23(p23), q3(pts[3]); return_value[0] = vecN<vecN<OutputType, 2>, 4>(q0, q01, qA, qC); return_value[1] = vecN<vecN<OutputType, 2>, 4>(qC, qB, q23, q3); return return_value; }
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Vec2 cC = data.positions[m_indexC].c; float aC = data.positions[m_indexC].a; b2Vec2 cD = data.positions[m_indexD].c; float aD = data.positions[m_indexD].a; b2Rot qA(aA), qB(aB), qC(aC), qD(aD); float linearError = 0.0f; float coordinateA, coordinateB; b2Vec2 JvAC, JvBD; float JwA, JwB, JwC, JwD; float mass = 0.0f; if (m_typeA == e_revoluteJoint) { JvAC.SetZero(); JwA = 1.0f; JwC = 1.0f; mass += m_iA + m_iC; coordinateA = aA - aC - m_referenceAngleA; } else { b2Vec2 u = b2Mul(qC, m_localAxisC); b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); JvAC = u; JwC = b2Cross(rC, u); JwA = b2Cross(rA, u); mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; b2Vec2 pC = m_localAnchorC - m_lcC; b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); coordinateA = b2Dot(pA - pC, m_localAxisC); } if (m_typeB == e_revoluteJoint) { JvBD.SetZero(); JwB = m_ratio; JwD = m_ratio; mass += m_ratio * m_ratio * (m_iB + m_iD); coordinateB = aB - aD - m_referenceAngleB; } else { b2Vec2 u = b2Mul(qD, m_localAxisD); b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); JvBD = m_ratio * u; JwD = m_ratio * b2Cross(rD, u); JwB = m_ratio * b2Cross(rB, u); mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; b2Vec2 pD = m_localAnchorD - m_lcD; b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); coordinateB = b2Dot(pB - pD, m_localAxisD); } float C = (coordinateA + m_ratio * coordinateB) - m_constant; float impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; } cA += m_mA * impulse * JvAC; aA += m_iA * impulse * JwA; cB += m_mB * impulse * JvBD; aB += m_iB * impulse * JwB; cC -= m_mC * impulse * JvAC; aC -= m_iC * impulse * JwC; cD -= m_mD * impulse * JvBD; aD -= m_iD * impulse * JwD; data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; data.positions[m_indexC].c = cC; data.positions[m_indexC].a = aC; data.positions[m_indexD].c = cD; data.positions[m_indexD].a = aD; // TODO_ERIN not implemented return linearError < b2_linearSlop; }
void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_indexC = m_bodyC->m_islandIndex; m_indexD = m_bodyD->m_islandIndex; m_lcA = m_bodyA->m_sweep.localCenter; m_lcB = m_bodyB->m_sweep.localCenter; m_lcC = m_bodyC->m_sweep.localCenter; m_lcD = m_bodyD->m_sweep.localCenter; m_mA = m_bodyA->m_invMass; m_mB = m_bodyB->m_invMass; m_mC = m_bodyC->m_invMass; m_mD = m_bodyD->m_invMass; m_iA = m_bodyA->m_invI; m_iB = m_bodyB->m_invI; m_iC = m_bodyC->m_invI; m_iD = m_bodyD->m_invI; //b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; //b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; //b2Vec2 cC = data.positions[m_indexC].c; float aC = data.positions[m_indexC].a; b2Vec2 vC = data.velocities[m_indexC].v; float wC = data.velocities[m_indexC].w; //b2Vec2 cD = data.positions[m_indexD].c; float aD = data.positions[m_indexD].a; b2Vec2 vD = data.velocities[m_indexD].v; float wD = data.velocities[m_indexD].w; b2Rot qA(aA), qB(aB), qC(aC), qD(aD); m_mass = 0.0f; if (m_typeA == e_revoluteJoint) { m_JvAC.SetZero(); m_JwA = 1.0f; m_JwC = 1.0f; m_mass += m_iA + m_iC; } else { b2Vec2 u = b2Mul(qC, m_localAxisC); b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); m_JvAC = u; m_JwC = b2Cross(rC, u); m_JwA = b2Cross(rA, u); m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; } if (m_typeB == e_revoluteJoint) { m_JvBD.SetZero(); m_JwB = m_ratio; m_JwD = m_ratio; m_mass += m_ratio * m_ratio * (m_iB + m_iD); } else { b2Vec2 u = b2Mul(qD, m_localAxisD); b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); m_JvBD = m_ratio * u; m_JwD = m_ratio * b2Cross(rD, u); m_JwB = m_ratio * b2Cross(rB, u); m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; } // Compute effective mass. m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; if (data.step.warmStarting) { vA += (m_mA * m_impulse) * m_JvAC; wA += m_iA * m_impulse * m_JwA; vB += (m_mB * m_impulse) * m_JvBD; wB += m_iB * m_impulse * m_JwB; vC -= (m_mC * m_impulse) * m_JvAC; wC -= m_iC * m_impulse * m_JwC; vD -= (m_mD * m_impulse) * m_JvBD; wD -= m_iD * m_impulse * m_JwD; } else { m_impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; data.velocities[m_indexC].v = vC; data.velocities[m_indexC].w = wC; data.velocities[m_indexD].v = vD; data.velocities[m_indexD].w = wD; }