public PolygonShape() { m_type = ShapeType.Polygon; m_radius = Settings._polygonRadius; m_count = 0; m_centroid.SetZero(); }
// p = attached point, m = mouse point // C = p - m // Cdot = v // = v + cross(w, r) // J = [I r_skew] // Identity used: // w k % (rx i + ry j) = w * (-ry i + rx j) internal MouseJoint(MouseJointDef def) : base(def) { Utilities.Assert(def.target.IsValid()); Utilities.Assert(Utilities.IsValid(def.maxForce) && def.maxForce >= 0.0f); Utilities.Assert(Utilities.IsValid(def.frequencyHz) && def.frequencyHz >= 0.0f); Utilities.Assert(Utilities.IsValid(def.dampingRatio) && def.dampingRatio >= 0.0f); m_targetA = def.target; m_localAnchorB = Utilities.MulT(m_bodyB.GetTransform(), m_targetA); m_maxForce = def.maxForce; m_impulse.SetZero(); m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_beta = 0.0f; m_gamma = 0.0f; }
internal override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB); // Get the pulley axes. Vec2 uA = cA + rA - m_groundAnchorA; Vec2 uB = cB + rB - m_groundAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings._linearSlop) { uA *= 1.0f / lengthA; } else { uA.SetZero(); } if (lengthB > 10.0f * Settings._linearSlop) { uB *= 1.0f / lengthB; } else { uB.SetZero(); } // Compute effective mass. float ruA = Utilities.Cross(rA, uA); float ruB = Utilities.Cross(rB, uB); float mA = m_invMassA + m_invIA * ruA * ruA; float mB = m_invMassB + m_invIB * ruB * ruB; float mass = mA + m_ratio * m_ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = m_constant - lengthA - m_ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; Vec2 PA = -impulse * uA; Vec2 PB = -m_ratio * impulse * uB; cA += m_invMassA * PA; aA += m_invIA * Utilities.Cross(rA, PA); cB += m_invMassB * PB; aB += m_invIB * Utilities.Cross(rB, PB); 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(linearError < Settings._linearSlop); }
public bool RayCast(out RayCastOutput output, RayCastInput input) { float tmin = -Single.MaxValue; float tmax = Single.MaxValue; Vec2 p = input.p1; Vec2 d = input.p2 - input.p1; Vec2 absD = Utilities.Abs(d); output = new RayCastOutput(); Vec2 normal = new Vec2(); for (int i = 0; i < 2; ++i) { if (Math.Abs(i) < Single.Epsilon) { // Parallel. if (p[i] < lowerBound[i] || upperBound[i] < p[i]) { return false; } } else { float inv_d = 1.0f / d[i]; float t1 = (lowerBound[i] - p[i]) * inv_d; float t2 = (upperBound[i] - p[i]) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { float temp = t1; t1 = t2; t2 = temp; s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); normal[i] = s; tmin = t1; } // Pull the max down tmax = Math.Min(tmax, t2); if (tmin > tmax) { return false; } } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.maxFraction < tmin) { return false; } // Intersection. output.fraction = tmin; output.normal = normal; return true; }
public bool RayCast(out RayCastOutput output, RayCastInput input) { float tmin = -Single.MaxValue; float tmax = Single.MaxValue; Vec2 p = input.p1; Vec2 d = input.p2 - input.p1; Vec2 absD = Utilities.Abs(d); output = new RayCastOutput(); Vec2 normal = new Vec2(); for (int i = 0; i < 2; ++i) { if (Math.Abs(i) < Single.Epsilon) { // Parallel. if (p[i] < lowerBound[i] || upperBound[i] < p[i]) { return(false); } } else { float inv_d = 1.0f / d[i]; float t1 = (lowerBound[i] - p[i]) * inv_d; float t2 = (upperBound[i] - p[i]) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { float temp = t1; t1 = t2; t2 = temp; s = 1.0f; } // Push the min up if (t1 > tmin) { normal.SetZero(); normal[i] = s; tmin = t1; } // Pull the max down tmax = Math.Min(tmax, t2); if (tmin > tmax) { return(false); } } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.maxFraction < tmin) { return(false); } // Intersection. output.fraction = tmin; output.normal = normal; return(true); }
internal override bool SolvePositionConstraints(SolverData data){ Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Vec2 cC = data.positions[m_indexC].c; float aC = data.positions[m_indexC].a; Vec2 cD = data.positions[m_indexD].c; float aD = data.positions[m_indexD].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); Rot qC= new Rot(aC); Rot qD = new Rot(aD); float linearError = 0.0f; float coordinateA, coordinateB; Vec2 JvAC = new Vec2(); Vec2 JvBD = new Vec2(); float JwA, JwB, JwC, JwD; float mass = 0.0f; if (m_typeA == JointType.e_revoluteJoint) { JvAC.SetZero(); JwA = 1.0f; JwC = 1.0f; mass += m_iA + m_iC; coordinateA = aA - aC - m_referenceAngleA; } else { Vec2 u = Utilities.Mul(qC, m_localAxisC); Vec2 rC = Utilities.Mul(qC, m_localAnchorC - m_lcC); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_lcA); JvAC = u; JwC = Utilities.Cross(rC, u); JwA = Utilities.Cross(rA, u); mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; Vec2 pC = m_localAnchorC - m_lcC; Vec2 pA = Utilities.MulT(qC, rA + (cA - cC)); coordinateA = Utilities.Dot(pA - pC, m_localAxisC); } if (m_typeB == JointType.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 { Vec2 u = Utilities.Mul(qD, m_localAxisD); Vec2 rD = Utilities.Mul(qD, m_localAnchorD - m_lcD); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_lcB); JvBD = m_ratio * u; JwD = m_ratio * Utilities.Cross(rD, u); JwB = m_ratio * Utilities.Cross(rB, u); mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; Vec2 pD = m_localAnchorD - m_lcD; Vec2 pB = Utilities.MulT(qD, rB + (cB - cD)); coordinateB = Utilities.Dot(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 <Settings._linearSlop; }
/// Set this to the identity transform. public void SetIdentity() { p.SetZero(); q.SetIdentity(); }
internal override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Vec2 cC = data.positions[m_indexC].c; float aC = data.positions[m_indexC].a; Vec2 cD = data.positions[m_indexD].c; float aD = data.positions[m_indexD].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); Rot qC = new Rot(aC); Rot qD = new Rot(aD); float linearError = 0.0f; float coordinateA, coordinateB; Vec2 JvAC = new Vec2(); Vec2 JvBD = new Vec2(); float JwA, JwB, JwC, JwD; float mass = 0.0f; if (m_typeA == JointType.e_revoluteJoint) { JvAC.SetZero(); JwA = 1.0f; JwC = 1.0f; mass += m_iA + m_iC; coordinateA = aA - aC - m_referenceAngleA; } else { Vec2 u = Utilities.Mul(qC, m_localAxisC); Vec2 rC = Utilities.Mul(qC, m_localAnchorC - m_lcC); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_lcA); JvAC = u; JwC = Utilities.Cross(rC, u); JwA = Utilities.Cross(rA, u); mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; Vec2 pC = m_localAnchorC - m_lcC; Vec2 pA = Utilities.MulT(qC, rA + (cA - cC)); coordinateA = Utilities.Dot(pA - pC, m_localAxisC); } if (m_typeB == JointType.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 { Vec2 u = Utilities.Mul(qD, m_localAxisD); Vec2 rD = Utilities.Mul(qD, m_localAnchorD - m_lcD); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_lcB); JvBD = m_ratio * u; JwD = m_ratio * Utilities.Cross(rD, u); JwB = m_ratio * Utilities.Cross(rB, u); mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; Vec2 pD = m_localAnchorD - m_lcD; Vec2 pB = Utilities.MulT(qD, rB + (cB - cD)); coordinateB = Utilities.Dot(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 < Settings._linearSlop); }