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);
}
