// Cache here per time step to reduce cache misses. // Vec2 m_localCenterA, m_localCenterB; // double m_invMassA, m_invIA; // double m_invMassB, m_invIB; public Joint(IWorldPool worldPool, JointDef def) { pool = worldPool; m_type = def.type; m_prev = null; m_next = null; m_bodyA = def.bodyA; m_bodyB = def.bodyB; m_collideConnected = def.collideConnected; m_islandFlag = false; m_userData = def.userData; m_edgeA = new JointEdge(); m_edgeA.joint = null; m_edgeA.other = null; m_edgeA.prev = null; m_edgeA.next = null; m_edgeB = new JointEdge(); m_edgeB.joint = null; m_edgeB.other = null; m_edgeB.prev = null; m_edgeB.next = null; // m_localCenterA = new Vec2(); // m_localCenterB = new Vec2(); }
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) : base(argWorld, def) { m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_localXAxisA = new Vec2(def.localAxisA); m_localXAxisA.normalize(); m_localYAxisA = new Vec2(); Vec2.crossToOutUnsafe(1d, m_localXAxisA, m_localYAxisA); m_referenceAngle = def.referenceAngle; m_impulse = new Vec3(); m_motorMass = 0.0d; m_motorImpulse = 0.0d; m_lowerTranslation = def.lowerTranslation; m_upperTranslation = def.upperTranslation; m_maxMotorForce = def.maxMotorForce; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = LimitState.INACTIVE; m_K = new Mat33(); m_axis = new Vec2(); m_perp = new Vec2(); }
public float MotorMass; // effective mass for motor/limit translational constraint. public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) : base(argWorld, def) { LocalAnchorA = new Vec2(def.LocalAnchorA); LocalAnchorB = new Vec2(def.LocalAnchorB); LocalXAxisA = new Vec2(def.LocalAxisA); LocalXAxisA.Normalize(); LocalYAxisA = new Vec2(); Vec2.CrossToOutUnsafe(1f, LocalXAxisA, LocalYAxisA); ReferenceAngle = def.ReferenceAngle; Impulse = new Vec3(); MotorMass = 0.0f; MotorImpulse = 0.0f; LowerTranslation = def.LowerTranslation; UpperTranslation = def.UpperTranslation; m_maxMotorForce = def.MaxMotorForce; m_motorSpeed = def.MotorSpeed; m_limitEnabled = def.EnableLimit; m_motorEnabled = def.EnableMotor; LimitState = LimitState.Inactive; K = new Mat33(); Axis = new Vec2(); Perp = new Vec2(); }
// Cache here per time step to reduce cache misses. // Vec2 m_localCenterA, m_localCenterB; // float m_invMassA, m_invIA; // float m_invMassB, m_invIB; protected Joint(IWorldPool worldPool, JointDef def) { Debug.Assert(def.bodyA != def.bodyB); pool = worldPool; m_type = def.type; m_prev = null; m_next = null; m_bodyA = def.bodyA; m_bodyB = def.bodyB; m_collideConnected = def.collideConnected; m_islandFlag = false; m_userData = def.userData; m_edgeA = new JointEdge(); m_edgeA.joint = null; m_edgeA.other = null; m_edgeA.prev = null; m_edgeA.next = null; m_edgeB = new JointEdge(); m_edgeB.joint = null; m_edgeB.other = null; m_edgeB.prev = null; m_edgeB.next = null; // m_localCenterA = new Vec2(); // m_localCenterB = new Vec2(); }
// Cache here per time step to reduce cache misses. // final Vec2 m_localCenterA, m_localCenterB; // float m_invMassA, m_invIA; // float m_invMassB, m_invIB; protected internal Joint(IWorldPool argWorldPool, JointDef def) { Debug.Assert(def.BodyA != def.BodyB); Pool = argWorldPool; Type = def.Type; Prev = null; Next = null; BodyA = def.BodyA; BodyB = def.BodyB; CollideConnected = def.CollideConnected; IslandFlag = false; UserData = def.UserData; Index = 0; EdgeA = new JointEdge(); EdgeA.Joint = null; EdgeA.Other = null; EdgeA.Prev = null; EdgeA.Next = null; EdgeB = new JointEdge(); EdgeB.Joint = null; EdgeB.Other = null; EdgeB.Prev = null; EdgeB.Next = null; // m_localCenterA = new Vec2(); // m_localCenterB = new Vec2(); }
public Contact(IWorldPool argPool) { m_fixtureA = null; m_fixtureB = null; m_nodeA = new ContactEdge(); m_nodeB = new ContactEdge(); m_manifold = new Manifold(); pool = argPool; }
protected internal Contact(IWorldPool argPool) { FixtureA = null; FixtureB = null; NodeA = new ContactEdge(); NodeB = new ContactEdge(); Manifold = new Manifold(); Pool = argPool; }
public Collision(IWorldPool argPool) { incidentEdge[0] = new ClipVertex(); incidentEdge[1] = new ClipVertex(); clipPoints1[0] = new ClipVertex(); clipPoints1[1] = new ClipVertex(); clipPoints2[0] = new ClipVertex(); clipPoints2[1] = new ClipVertex(); pool = argPool; }
public DistanceJoint(IWorldPool argWorld, DistanceJointDef def) : base(argWorld, def) { m_localAnchorA = def.localAnchorA.clone(); m_localAnchorB = def.localAnchorB.clone(); m_length = def.length; m_impulse = 0.0d; m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_gamma = 0.0d; m_bias = 0.0d; }
/// <param name="argWorld"></param> /// <param name="def"></param> protected internal WeldJoint(IWorldPool argWorld, WeldJointDef def) : base(argWorld, def) { LocalAnchorA = new Vec2(def.LocalAnchorA); LocalAnchorB = new Vec2(def.LocalAnchorB); m_referenceAngle = def.ReferenceAngle; Frequency = def.FrequencyHz; DampingRatio = def.DampingRatio; m_impulse = new Vec3(); m_impulse.SetZero(); }
/// <param name="argWorldPool"></param> /// <param name="def"></param> public FrictionJoint(IWorldPool argWorldPool, FrictionJointDef def) : base(argWorldPool, def) { m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_linearImpulse = new Vec2(); m_angularImpulse = 0.0f; m_maxForce = def.maxForce; m_maxTorque = def.maxTorque; }
public FrictionJoint(IWorldPool argWorldPool, FrictionJointDef def) : base(argWorldPool, def) { m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_linearImpulse = new Vec2(); m_angularImpulse = 0.0d; m_maxForce = def.maxForce; m_maxTorque = def.maxTorque; }
public WeldJoint(IWorldPool argWorld, WeldJointDef def) : base(argWorld, def) { m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_referenceAngle = def.referenceAngle; m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_impulse = new Vec3(); m_impulse.setZero(); }
public DistanceJoint(IWorldPool argWorld, DistanceJointDef def) : base(argWorld, def) { LocalAnchorA = def.LocalAnchorA.Clone(); LocalAnchorB = def.LocalAnchorB.Clone(); Length = def.Length; Impulse = 0.0f; FrequencyHz = def.FrequencyHz; DampingRatio = def.DampingRatio; Gamma = 0.0f; Bias = 0.0f; }
public MotorJoint(IWorldPool pool, MotorJointDef def) : base(pool, def) { m_linearOffset.set(def.linearOffset); m_angularOffset = def.angularOffset; m_angularImpulse = 0.0f; m_maxForce = def.maxForce; m_maxTorque = def.maxTorque; m_correctionFactor = def.correctionFactor; }
public RopeJoint(IWorldPool worldPool, RopeJointDef def) : base(worldPool, def) { m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_maxLength = def.maxLength; m_mass = 0.0d; m_impulse = 0.0d; m_state = LimitState.INACTIVE; m_length = 0.0d; }
internal RopeJoint(IWorldPool worldPool, RopeJointDef def) : base(worldPool, def) { m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_maxLength = def.maxLength; m_mass = 0.0f; m_impulse = 0.0f; m_state = LimitState.INACTIVE; m_length = 0.0f; }
public MouseJoint(IWorldPool argWorld, MouseJointDef def) : base(argWorld, def) { m_targetA.set(def.target); Transform.mulTransToOutUnsafe(m_bodyB.getTransform(), m_targetA, m_localAnchorB); m_maxForce = def.maxForce; m_impulse.setZero(); m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_beta = 0; m_gamma = 0; }
public PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def) : base(argWorldPool, def) { m_groundAnchorA.set(def.groundAnchorA); m_groundAnchorB.set(def.groundAnchorB); m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_ratio = def.ratio; m_lengthA = def.lengthA; m_lengthB = def.lengthB; m_constant = def.lengthA + m_ratio * def.lengthB; m_impulse = 0.0d; }
/// <param name="argWorldPool"></param> /// <param name="def"></param> public PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def) : base(argWorldPool, def) { m_groundAnchorA.Set(def.GroundAnchorA); m_groundAnchorB.Set(def.GroundAnchorB); LocalAnchorA.Set(def.LocalAnchorA); LocalAnchorB.Set(def.LocalAnchorB); Debug.Assert(def.Ratio != 0.0f); m_ratio = def.Ratio; m_lengthA = def.LengthA; m_LengthB = def.LengthB; m_constant = def.LengthA + m_ratio * def.LengthB; m_impulse = 0.0f; }
public RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def) : base(argWorld, def) { LocalAnchorA.Set(def.LocalAnchorA); LocalAnchorB.Set(def.LocalAnchorB); ReferenceAngle = def.ReferenceAngle; MotorImpulse = 0; LowerAngle = def.LowerAngle; UpperAngle = def.UpperAngle; m_maxMotorTorque = def.MaxMotorTorque; m_motorSpeed = def.MotorSpeed; m_limitEnabled = def.EnableLimit; m_motorEnabled = def.EnableMotor; LimitState = LimitState.Inactive; }
/// <param name="argWorldPool"></param> /// <param name="def"></param> public PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def) : base(argWorldPool, def) { m_groundAnchorA.set_Renamed(def.groundAnchorA); m_groundAnchorB.set_Renamed(def.groundAnchorB); m_localAnchorA.set_Renamed(def.localAnchorA); m_localAnchorB.set_Renamed(def.localAnchorB); Debug.Assert(def.ratio != 0.0f); m_ratio = def.ratio; m_lengthA = def.lengthA; m_LengthB = def.lengthB; m_constant = def.lengthA + m_ratio * def.lengthB; m_impulse = 0.0f; }
public RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def) : base(argWorld, def) { m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_referenceAngle = def.referenceAngle; m_motorImpulse = 0; m_lowerAngle = def.lowerAngle; m_upperAngle = def.upperAngle; m_maxMotorTorque = def.maxMotorTorque; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = LimitState.INACTIVE; }
public WheelJoint(IWorldPool argPool, WheelJointDef def) : base(argPool, def) { m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_localXAxisA.set(def.localAxisA); Vec2.crossToOutUnsafe(1.0d, m_localXAxisA, m_localYAxisA); m_motorMass = 0.0d; m_motorImpulse = 0.0d; m_maxMotorTorque = def.maxMotorTorque; m_motorSpeed = def.motorSpeed; m_enableMotor = def.enableMotor; m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; }
protected internal MouseJoint(IWorldPool argWorld, MouseJointDef def) : base(argWorld, def) { Debug.Assert(def.Target.Valid); Debug.Assert(def.MaxForce >= 0); Debug.Assert(def.FrequencyHz >= 0); Debug.Assert(def.DampingRatio >= 0); m_targetA.Set(def.Target); Transform.MulTransToOutUnsafe(BodyB.GetTransform(), m_targetA, m_localAnchorB); m_maxForce = def.MaxForce; m_impulse.SetZero(); Frequency = def.FrequencyHz; DampingRatio = def.DampingRatio; m_beta = 0; m_gamma = 0; }
protected internal MouseJoint(IWorldPool argWorld, MouseJointDef def) : base(argWorld, def) { Debug.Assert(def.target.Valid); Debug.Assert(def.maxForce >= 0); Debug.Assert(def.frequencyHz >= 0); Debug.Assert(def.dampingRatio >= 0); m_targetA.set_Renamed(def.target); Transform.mulTransToOutUnsafe(m_bodyB.getTransform(), m_targetA, m_localAnchorB); m_maxForce = def.maxForce; m_impulse.setZero(); m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_beta = 0; m_gamma = 0; }
internal RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def) : base(argWorld, def) { m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_referenceAngle = def.referenceAngle; m_motorImpulse = 0; m_lowerAngle = def.lowerAngle; m_upperAngle = def.upperAngle; m_maxMotorTorque = def.maxMotorTorque; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = LimitState.INACTIVE; }
/// <summary> /// Construct a world object. /// </summary> /// <param name="gravity">the world gravity vector.</param> /// <param name="argPool"> </param> public World(Vec2 gravity, IWorldPool argPool) { contactStacks = new ContactRegister[ShapeTypesCount][]; for (int i = 0; i < ShapeTypesCount; i++) { contactStacks[i] = new ContactRegister[ShapeTypesCount]; } Pool = argPool; DestructionListener = null; DebugDraw = null; BodyList = null; JointList = null; BodyCount = 0; JointCount = 0; WarmStarting = true; ContinuousPhysics = true; m_subStepping = false; m_stepComplete = true; SleepingAllowed = true; m_gravity.Set(gravity); Flags = CLEAR_FORCES; invDt0 = 0f; ContactManager = new ContactManager(this); Profile = new Profile(); InitializeRegisters(); }
public DistanceJoint(IWorldPool argWorld, DistanceJointDef def) : base(argWorld, def) { m_localAnchorA = def.localAnchorA.Clone(); m_localAnchorB = def.localAnchorB.Clone(); m_length = def.length; m_impulse = 0.0f; m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_gamma = 0.0f; m_bias = 0.0f; }
protected internal Contact(IWorldPool argPool) { m_fixtureA = null; m_fixtureB = null; m_nodeA = new ContactEdge(); m_nodeB = new ContactEdge(); m_manifold = new Manifold(); pool = argPool; }
public ChainAndCircleContact(IWorldPool argPool) : base(argPool) { }
/// <summary> /// From Real-time Collision Detection, p179. /// </summary> /// <param name="output"></param> /// <param name="input"></param> /// <param name="argPool"></param> public bool Raycast(RayCastOutput output, RayCastInput input, IWorldPool argPool) { //UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'" float tmin = Single.Epsilon; float tmax = Single.MaxValue; Vec2 p = argPool.PopVec2(); Vec2 d = argPool.PopVec2(); Vec2 absD = argPool.PopVec2(); Vec2 normal = argPool.PopVec2(); p.Set(input.P1); d.Set(input.P2).SubLocal(input.P1); Vec2.AbsToOut(d, absD); // x then y if (absD.X < Settings.EPSILON) { // Parallel. if (p.X < LowerBound.X || UpperBound.X < p.X) { argPool.PushVec2(4); return(false); } } else { float inv_d = 1.0f / d.X; float t1 = (LowerBound.X - p.X) * inv_d; float t2 = (UpperBound.X - p.X) * 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.X = s; tmin = t1; } // Pull the max down tmax = MathUtils.Min(tmax, t2); if (tmin > tmax) { argPool.PushVec2(4); return(false); } } if (absD.Y < Settings.EPSILON) { // Parallel. if (p.Y < LowerBound.Y || UpperBound.Y < p.Y) { argPool.PushVec2(4); return(false); } } else { float inv_d = 1.0f / d.Y; float t1 = (LowerBound.Y - p.Y) * inv_d; float t2 = (UpperBound.Y - p.Y) * 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.Y = s; tmin = t1; } // Pull the max down tmax = MathUtils.Min(tmax, t2); if (tmin > tmax) { argPool.PushVec2(4); return(false); } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.MaxFraction < tmin) { argPool.PushVec2(4); return(false); } // Intersection. output.Fraction = tmin; output.Normal.X = normal.X; output.Normal.Y = normal.Y; argPool.PushVec2(4); return(true); }
public GearJoint(IWorldPool argWorldPool, GearJointDef def) : base(argWorldPool, def) { m_joint1 = def.joint1; m_joint2 = def.joint2; m_typeA = m_joint1.getType(); m_typeB = m_joint2.getType(); double coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in Joint. m_bodyC = m_joint1.getBodyA(); m_bodyA = m_joint1.getBodyB(); // Get geometry of joint1 Transform xfA = m_bodyA.m_xf; double aA = m_bodyA.m_sweep.a; Transform xfC = m_bodyC.m_xf; double aC = m_bodyC.m_sweep.a; if (m_typeA == JointType.REVOLUTE) { var revolute = (RevoluteJoint)def.joint1; m_localAnchorC.set(revolute.m_localAnchorA); m_localAnchorA.set(revolute.m_localAnchorB); m_referenceAngleA = revolute.m_referenceAngle; m_localAxisC.setZero(); coordinateA = aA - aC - m_referenceAngleA; } else { Vec2 pA = pool.popVec2(); Vec2 temp = pool.popVec2(); var prismatic = (PrismaticJoint)def.joint1; m_localAnchorC.set(prismatic.m_localAnchorA); m_localAnchorA.set(prismatic.m_localAnchorB); m_referenceAngleA = prismatic.m_referenceAngle; m_localAxisC.set(prismatic.m_localXAxisA); Vec2 pC = m_localAnchorC; Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, temp); temp.addLocal(xfA.p).subLocal(xfC.p); Rot.mulTransUnsafe(xfC.q, temp, pA); coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC); pool.pushVec2(2); } m_bodyD = m_joint2.getBodyA(); m_bodyB = m_joint2.getBodyB(); // Get geometry of joint2 Transform xfB = m_bodyB.m_xf; double aB = m_bodyB.m_sweep.a; Transform xfD = m_bodyD.m_xf; double aD = m_bodyD.m_sweep.a; if (m_typeB == JointType.REVOLUTE) { var revolute = (RevoluteJoint)def.joint2; m_localAnchorD.set(revolute.m_localAnchorA); m_localAnchorB.set(revolute.m_localAnchorB); m_referenceAngleB = revolute.m_referenceAngle; m_localAxisD.setZero(); coordinateB = aB - aD - m_referenceAngleB; } else { Vec2 pB = pool.popVec2(); Vec2 temp = pool.popVec2(); var prismatic = (PrismaticJoint)def.joint2; m_localAnchorD.set(prismatic.m_localAnchorA); m_localAnchorB.set(prismatic.m_localAnchorB); m_referenceAngleB = prismatic.m_referenceAngle; m_localAxisD.set(prismatic.m_localXAxisA); Vec2 pD = m_localAnchorD; Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, temp); temp.addLocal(xfB.p).subLocal(xfD.p); Rot.mulTransUnsafe(xfD.q, temp, pB); coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); pool.pushVec2(2); } m_ratio = def.ratio; m_constant = coordinateA + m_ratio * coordinateB; m_impulse = 0.0d; }
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) : base(argWorld, def) { m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_localXAxisA = new Vec2(def.localAxisA); m_localXAxisA.normalize(); m_localYAxisA = new Vec2(); Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA); m_referenceAngle = def.referenceAngle; m_impulse = new Vec3(); m_motorMass = 0.0f; m_motorImpulse = 0.0f; m_lowerTranslation = def.lowerTranslation; m_upperTranslation = def.upperTranslation; m_maxMotorForce = def.maxMotorForce; m_motorSpeed = def.motorSpeed; m_enableLimit = def.enableLimit; m_enableMotor = def.enableMotor; m_limitState = LimitState.INACTIVE; m_K = new Mat33(); m_axis = new Vec2(); m_perp = new Vec2(); }
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) : base(argWorld, def) { LocalAnchorA = new Vec2(def.LocalAnchorA); LocalAnchorB = new Vec2(def.LocalAnchorB); LocalXAxisA = new Vec2(def.LocalAxisA); LocalXAxisA.Normalize(); LocalYAxisA = new Vec2(); Vec2.CrossToOutUnsafe(1f, LocalXAxisA, LocalYAxisA); ReferenceAngle = def.ReferenceAngle; Impulse = new Vec3(); MotorMass = 0.0f; MotorImpulse = 0.0f; LowerTranslation = def.LowerTranslation; UpperTranslation = def.UpperTranslation; m_maxMotorForce = def.MaxMotorForce; m_motorSpeed = def.MotorSpeed; m_limitEnabled = def.EnableLimit; m_motorEnabled = def.EnableMotor; LimitState = LimitState.Inactive; K = new Mat33(); Axis = new Vec2(); Perp = new Vec2(); }
internal GearJoint(IWorldPool argWorldPool, GearJointDef def) : base(argWorldPool, def) { m_joint1 = def.joint1; m_joint2 = def.joint2; m_typeA = m_joint1.getType(); m_typeB = m_joint2.getType(); Debug.Assert(m_typeA == JointType.REVOLUTE || m_typeA == JointType.PRISMATIC); Debug.Assert(m_typeB == JointType.REVOLUTE || m_typeB == JointType.PRISMATIC); float coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in Joint. m_bodyC = m_joint1.getBodyA(); m_bodyA = m_joint1.getBodyB(); // Get geometry of joint1 Transform xfA = m_bodyA.m_xf; float aA = m_bodyA.m_sweep.a; Transform xfC = m_bodyC.m_xf; float aC = m_bodyC.m_sweep.a; if (m_typeA == JointType.REVOLUTE) { RevoluteJoint revolute = (RevoluteJoint) def.joint1; m_localAnchorC.set(revolute.m_localAnchorA); m_localAnchorA.set(revolute.m_localAnchorB); m_referenceAngleA = revolute.m_referenceAngle; m_localAxisC.setZero(); coordinateA = aA - aC - m_referenceAngleA; } else { Vec2 pA = pool.popVec2(); Vec2 temp = pool.popVec2(); PrismaticJoint prismatic = (PrismaticJoint) def.joint1; m_localAnchorC.set(prismatic.m_localAnchorA); m_localAnchorA.set(prismatic.m_localAnchorB); m_referenceAngleA = prismatic.m_referenceAngle; m_localAxisC.set(prismatic.m_localXAxisA); Vec2 pC = m_localAnchorC; Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, ref temp); temp.addLocal(xfA.p); temp.subLocal(xfC.p); Rot.mulTransUnsafe(xfC.q, temp, ref pA); pA.subLocal(pC); coordinateA = Vec2.dot(pA, m_localAxisC); pool.pushVec2(2); } m_bodyD = m_joint2.getBodyA(); m_bodyB = m_joint2.getBodyB(); // Get geometry of joint2 Transform xfB = m_bodyB.m_xf; float aB = m_bodyB.m_sweep.a; Transform xfD = m_bodyD.m_xf; float aD = m_bodyD.m_sweep.a; if (m_typeB == JointType.REVOLUTE) { RevoluteJoint revolute = (RevoluteJoint) def.joint2; m_localAnchorD.set(revolute.m_localAnchorA); m_localAnchorB.set(revolute.m_localAnchorB); m_referenceAngleB = revolute.m_referenceAngle; m_localAxisD.setZero(); coordinateB = aB - aD - m_referenceAngleB; } else { Vec2 pB = pool.popVec2(); Vec2 temp = pool.popVec2(); PrismaticJoint prismatic = (PrismaticJoint) def.joint2; m_localAnchorD.set(prismatic.m_localAnchorA); m_localAnchorB.set(prismatic.m_localAnchorB); m_referenceAngleB = prismatic.m_referenceAngle; m_localAxisD.set(prismatic.m_localXAxisA); Vec2 pD = m_localAnchorD; Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, ref temp); temp.addLocal(xfB.p); temp.subLocal(xfD.p); Rot.mulTransUnsafe(xfD.q, temp, ref pB); pB.subLocal(pD); coordinateB = Vec2.dot(pB, m_localAxisD); pool.pushVec2(2); } m_ratio = def.ratio; m_constant = coordinateA + m_ratio*coordinateB; m_impulse = 0.0f; }
/// <summary> /// Construct a world object. /// </summary> /// <param name="gravity">the world gravity vector.</param> /// <param name="doSleep">improve performance by not simulating inactive bodies.</param> public World(Vec2 gravity, IWorldPool argPool) { contactStacks = new ContactRegister[ShapeTypesCount][]; for (int i = 0; i < ShapeTypesCount; i++) { contactStacks[i] = new ContactRegister[ShapeTypesCount]; } pool = argPool; m_destructionListener = null; m_debugDraw = null; m_bodyList = null; m_jointList = null; m_bodyCount = 0; m_jointCount = 0; m_warmStarting = true; m_continuousPhysics = true; m_subStepping = false; m_stepComplete = true; m_allowSleep = true; m_gravity.set_Renamed(gravity); m_flags = CLEAR_FORCES; m_inv_dt0 = 0f; m_contactManager = new ContactManager(this); m_profile = new Profile(); initializeRegisters(); }
/** * From Real-time Collision Detection, p179. * * @param output * @param input */ public bool raycast(RayCastOutput output, RayCastInput input, IWorldPool argPool) { double tmin = -double.MaxValue; double tmax = double.MaxValue; Vec2 p = argPool.popVec2(); Vec2 d = argPool.popVec2(); Vec2 absD = argPool.popVec2(); Vec2 normal = argPool.popVec2(); p.set(input.p1); d.set(input.p2).subLocal(input.p1); Vec2.absToOut(d, absD); // x then y if (absD.x < Settings.EPSILON) { // Parallel. if (p.x < lowerBound.x || upperBound.x < p.x) { argPool.pushVec2(4); return(false); } } else { double inv_d = 1.0d / d.x; double t1 = (lowerBound.x - p.x) * inv_d; double t2 = (upperBound.x - p.x) * inv_d; // Sign of the normal vector. double s = -1.0d; if (t1 > t2) { double temp = t1; t1 = t2; t2 = temp; s = 1.0d; } // Push the min up if (t1 > tmin) { normal.setZero(); normal.x = s; tmin = t1; } // Pull the max down tmax = MathUtils.min(tmax, t2); if (tmin > tmax) { argPool.pushVec2(4); return(false); } } if (absD.y < Settings.EPSILON) { // Parallel. if (p.y < lowerBound.y || upperBound.y < p.y) { argPool.pushVec2(4); return(false); } } else { double inv_d = 1.0d / d.y; double t1 = (lowerBound.y - p.y) * inv_d; double t2 = (upperBound.y - p.y) * inv_d; // Sign of the normal vector. double s = -1.0d; if (t1 > t2) { double temp = t1; t1 = t2; t2 = temp; s = 1.0d; } // Push the min up if (t1 > tmin) { normal.setZero(); normal.y = s; tmin = t1; } // Pull the max down tmax = MathUtils.min(tmax, t2); if (tmin > tmax) { argPool.pushVec2(4); return(false); } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0d || input.maxFraction < tmin) { argPool.pushVec2(4); return(false); } // Intersection. output.fraction = tmin; output.normal.x = normal.x; output.normal.y = normal.y; argPool.pushVec2(4); return(true); }
public PolygonContact(IWorldPool argPool) : base(argPool) { }
/// <summary> /// From Real-time Collision Detection, p179. /// </summary> /// <param name="output"></param> /// <param name="input"></param> public bool raycast(RayCastOutput output, RayCastInput input, IWorldPool argPool) { //UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'" float tmin = Single.Epsilon; float tmax = Single.MaxValue; Vec2 p = argPool.popVec2(); Vec2 d = argPool.popVec2(); Vec2 absD = argPool.popVec2(); Vec2 normal = argPool.popVec2(); p.set_Renamed(input.p1); d.set_Renamed(input.p2).subLocal(input.p1); Vec2.absToOut(d, absD); // x then y if (absD.x < Settings.EPSILON) { // Parallel. if (p.x < lowerBound.x || upperBound.x < p.x) { argPool.pushVec2(4); return false; } } else { float inv_d = 1.0f / d.x; float t1 = (lowerBound.x - p.x) * inv_d; float t2 = (upperBound.x - p.x) * 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.x = s; tmin = t1; } // Pull the max down tmax = MathUtils.min(tmax, t2); if (tmin > tmax) { argPool.pushVec2(4); return false; } } if (absD.y < Settings.EPSILON) { // Parallel. if (p.y < lowerBound.y || upperBound.y < p.y) { argPool.pushVec2(4); return false; } } else { float inv_d = 1.0f / d.y; float t1 = (lowerBound.y - p.y) * inv_d; float t2 = (upperBound.y - p.y) * 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.y = s; tmin = t1; } // Pull the max down tmax = MathUtils.min(tmax, t2); if (tmin > tmax) { argPool.pushVec2(4); return false; } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (tmin < 0.0f || input.maxFraction < tmin) { argPool.pushVec2(4); return false; } // Intersection. output.fraction = tmin; output.normal.x = normal.x; output.normal.y = normal.y; argPool.pushVec2(4); return true; }
public EdgeAndCircleContact(IWorldPool argPool) : base(argPool) { }
public ChainAndPolygonContact(IWorldPool argPool) : base(argPool) { }
public DefaultWorldPool(int argSize, int argContainerSize) { world = this; pcstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new PolygonContact(world) }; ccstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new CircleContact(world) }; cpstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new PolygonAndCircleContact(world) }; ecstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new EdgeAndCircleContact(world) }; epstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new EdgeAndPolygonContact(world) }; chcstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new ChainAndCircleContact(world) }; chpstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE) { newInstance = () => new ChainAndPolygonContact(world) }; vecs = new OrderedStack <Vec2>(argSize, argContainerSize) { newInstance = () => new Vec2() }; vec3s = new OrderedStack <Vec3>(argSize, argContainerSize) { newInstance = () => new Vec3() }; mats = new OrderedStack <Mat22>(argSize, argContainerSize) { newInstance = () => new Mat22() }; aabbs = new OrderedStack <AABB>(argSize, argContainerSize) { newInstance = () => new AABB() }; rots = new OrderedStack <Rot>(argSize, argContainerSize) { newInstance = () => new Rot() }; mat33s = new OrderedStack <Mat33>(argSize, argContainerSize) { newInstance = () => new Mat33() }; dist = new Distance(); collision = new Collision(this); toi = new TimeOfImpact(this); }
public PolyShapesCallback(IWorldPool argWorld) { m_count = 0; p = argWorld; }
public PolygonAndCircleContact(IWorldPool argPool) : base(argPool) { }
public TimeOfImpact(IWorldPool argPool) { pool = argPool; }
public CircleContact(IWorldPool argPool) : base(argPool) { }
internal WheelJoint(IWorldPool argPool, WheelJointDef def) : base(argPool, def) { m_localAnchorA.set(def.localAnchorA); m_localAnchorB.set(def.localAnchorB); m_localXAxisA.set(def.localAxisA); Vec2.crossToOutUnsafe(1.0f, m_localXAxisA, ref m_localYAxisA); m_motorMass = 0.0f; m_motorImpulse = 0.0f; m_maxMotorTorque = def.maxMotorTorque; m_motorSpeed = def.motorSpeed; m_enableMotor = def.enableMotor; m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; }
/// <param name="argWorld"></param> /// <param name="def"></param> protected internal WeldJoint(IWorldPool argWorld, WeldJointDef def) : base(argWorld, def) { m_localAnchorA = new Vec2(def.localAnchorA); m_localAnchorB = new Vec2(def.localAnchorB); m_referenceAngle = def.referenceAngle; m_frequencyHz = def.frequencyHz; m_dampingRatio = def.dampingRatio; m_impulse = new Vec3(); m_impulse.setZero(); }
public EdgeAndPolygonContact(IWorldPool argPool) : base(argPool) { }