public void Init(ContactSolverDef def) { //Console.WriteLine("Initializing contact solver"); Step = def.Step; Count = def.Count; if (PositionConstraints.Length < Count) { ContactPositionConstraint[] old = PositionConstraints; PositionConstraints = new ContactPositionConstraint[MathUtils.Max(old.Length * 2, Count)]; Array.Copy(old, 0, PositionConstraints, 0, old.Length); for (int i = old.Length; i < PositionConstraints.Length; i++) { PositionConstraints[i] = new ContactPositionConstraint(); } } if (VelocityConstraints.Length < Count) { ContactVelocityConstraint[] old = VelocityConstraints; VelocityConstraints = new ContactVelocityConstraint[MathUtils.Max(old.Length * 2, Count)]; Array.Copy(old, 0, VelocityConstraints, 0, old.Length); for (int i = old.Length; i < VelocityConstraints.Length; i++) { VelocityConstraints[i] = new ContactVelocityConstraint(); } } Positions = def.Positions; Velocities = def.Velocities; Contacts = def.Contacts; for (int i = 0; i < Count; ++i) { //Console.WriteLine("contacts: " + m_count); Contact contact = Contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = VelocityConstraints[i]; vc.Friction = contact.Friction; vc.Restitution = contact.Restitution; vc.TangentSpeed = contact.TangentSpeed; vc.IndexA = bodyA.IslandIndex; vc.IndexB = bodyB.IslandIndex; vc.InvMassA = bodyA.InvMass; vc.InvMassB = bodyB.InvMass; vc.InvIA = bodyA.InvI; vc.InvIB = bodyB.InvI; vc.ContactIndex = i; vc.PointCount = pointCount; vc.K.SetZero(); vc.NormalMass.SetZero(); ContactPositionConstraint pc = PositionConstraints[i]; pc.IndexA = bodyA.IslandIndex; pc.IndexB = bodyB.IslandIndex; pc.InvMassA = bodyA.InvMass; pc.InvMassB = bodyB.InvMass; pc.LocalCenterA.Set(bodyA.Sweep.LocalCenter); pc.LocalCenterB.Set(bodyB.Sweep.LocalCenter); pc.InvIA = bodyA.InvI; pc.InvIB = bodyB.InvI; pc.LocalNormal.Set(manifold.LocalNormal); pc.LocalPoint.Set(manifold.LocalPoint); pc.PointCount = pointCount; pc.RadiusA = radiusA; pc.RadiusB = radiusB; pc.Type = manifold.Type; //Console.WriteLine("contact point count: " + pointCount); for (int j = 0; j < pointCount; j++) { ManifoldPoint cp = manifold.Points[j]; ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; if (Step.WarmStarting) { //Debug.Assert(cp.normalImpulse == 0); //Console.WriteLine("contact normal impulse: " + cp.normalImpulse); vcp.NormalImpulse = Step.DtRatio * cp.NormalImpulse; vcp.TangentImpulse = Step.DtRatio * cp.TangentImpulse; } else { vcp.NormalImpulse = 0; vcp.TangentImpulse = 0; } vcp.RA.SetZero(); vcp.RB.SetZero(); vcp.NormalMass = 0; vcp.TangentMass = 0; vcp.VelocityBias = 0; pc.LocalPoints[j].Set(cp.LocalPoint); } } }
public void init(ContactSolverDef def) { //Console.WriteLine("Initializing contact solver"); m_step = def.step; m_count = def.count; if (m_positionConstraints.Length < m_count) { ContactPositionConstraint[] old = m_positionConstraints; m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.Length * 2, m_count)]; Array.Copy(old, 0, m_positionConstraints, 0, old.Length); for (int i = old.Length; i < m_positionConstraints.Length; i++) { m_positionConstraints[i] = new ContactPositionConstraint(); } } if (m_velocityConstraints.Length < m_count) { ContactVelocityConstraint[] old = m_velocityConstraints; m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.Length * 2, m_count)]; Array.Copy(old, 0, m_velocityConstraints, 0, old.Length); for (int i = old.Length; i < m_velocityConstraints.Length; i++) { m_velocityConstraints[i] = new ContactVelocityConstraint(); } } m_positions = def.positions; m_velocities = def.velocities; m_contacts = def.contacts; for (int i = 0; i < m_count; ++i) { //Console.WriteLine("contacts: " + m_count); Contact contact = m_contacts[i]; Fixture fixtureA = contact.m_fixtureA; Fixture fixtureB = contact.m_fixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.m_radius; float radiusB = shapeB.m_radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold = contact.Manifold; int pointCount = manifold.pointCount; Debug.Assert(pointCount > 0); ContactVelocityConstraint vc = m_velocityConstraints[i]; vc.friction = contact.m_friction; vc.restitution = contact.m_restitution; vc.tangentSpeed = contact.m_tangentSpeed; vc.indexA = bodyA.m_islandIndex; vc.indexB = bodyB.m_islandIndex; vc.invMassA = bodyA.m_invMass; vc.invMassB = bodyB.m_invMass; vc.invIA = bodyA.m_invI; vc.invIB = bodyB.m_invI; vc.contactIndex = i; vc.pointCount = pointCount; vc.K.setZero(); vc.normalMass.setZero(); ContactPositionConstraint pc = m_positionConstraints[i]; pc.indexA = bodyA.m_islandIndex; pc.indexB = bodyB.m_islandIndex; pc.invMassA = bodyA.m_invMass; pc.invMassB = bodyB.m_invMass; pc.localCenterA.set_Renamed(bodyA.m_sweep.localCenter); pc.localCenterB.set_Renamed(bodyB.m_sweep.localCenter); pc.invIA = bodyA.m_invI; pc.invIB = bodyB.m_invI; pc.localNormal.set_Renamed(manifold.localNormal); pc.localPoint.set_Renamed(manifold.localPoint); pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.type; //Console.WriteLine("contact point count: " + pointCount); for (int j = 0; j < pointCount; j++) { ManifoldPoint cp = manifold.points[j]; ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.points[j]; if (m_step.warmStarting) { //Debug.Assert(cp.normalImpulse == 0); //Console.WriteLine("contact normal impulse: " + cp.normalImpulse); vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse; vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse; } else { vcp.normalImpulse = 0; vcp.tangentImpulse = 0; } vcp.rA.setZero(); vcp.rB.setZero(); vcp.normalMass = 0; vcp.tangentMass = 0; vcp.velocityBias = 0; pc.localPoints[j].set_Renamed(cp.localPoint); } } }