public void InterpretGoal(Tantric.Logic.Unit u, Tantric.Logic.UnitGoal goal, int elapsedMilliseconds) { switch (goal.Name.ToLower()) { case "moveto": // Move To is exclusive Microsoft.Xna.Framework.Vector2 dir = (Microsoft.Xna.Framework.Vector2)goal.GetArgument(1) - u.Position; Microsoft.Xna.Framework.Vector2 norm = dir; norm.Normalize(); norm *= elapsedMilliseconds; norm /= 1000; norm *= World.Objects.Human.HumanStatistics.GetStatistic("Human_Assembler", "Speed"); if (dir.LengthSquared() == 0) { goal.Satisfy(); break; } if (norm.LengthSquared() > dir.LengthSquared()) { u.Translate(dir); goal.Satisfy(); } else { u.Translate(norm); } break; default: break; } }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; // Cdot = v + cross(w, r) Vector2 Cdot = vA + MathUtils.Cross(wA, ref _rA); Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); Vector2 oldImpulse = _impulse; _impulse += impulse; float maxImpulse = data.step.dt * MaxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.Length(); } impulse = _impulse - oldImpulse; vA += _invMassA * impulse; wA += _invIA * MathUtils.Cross(ref _rA, ref impulse); data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; float h = data.step.dt; float inv_h = data.step.inv_dt; // Solve angular friction { float Cdot = wB - wA + inv_h * CorrectionFactor * _angularError; float impulse = -_angularMass * Cdot; float oldImpulse = _angularImpulse; float maxImpulse = h * _maxTorque; _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = _angularImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve linear friction { Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA) + inv_h * CorrectionFactor * _linearError; Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot); Vector2 oldImpulse = _linearImpulse; _linearImpulse += impulse; float maxImpulse = h * _maxForce; if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { _linearImpulse.Normalize(); _linearImpulse *= maxImpulse; } impulse = _linearImpulse - oldImpulse; vA -= mA * impulse; wA -= iA * MathUtils.Cross(ref _rA, ref impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(ref _rB, ref impulse); } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public void ProjectParentVelocityOnLastMoveCollisionTangent(float minimumVectorLengthSquared) { #if FRB_MDX if (LastMoveCollisionReposition.LengthSq() > minimumVectorLengthSquared && #else if (LastMoveCollisionReposition.LengthSquared() > minimumVectorLengthSquared && #endif Vector2.Dot( new Vector2(TopParent.Velocity.X, TopParent.Velocity.Y), LastMoveCollisionReposition) < 0) { Vector2 collisionAdjustmentNormalized = LastMoveCollisionReposition; collisionAdjustmentNormalized.Normalize(); float temporaryFloat = collisionAdjustmentNormalized.X; collisionAdjustmentNormalized.X = -collisionAdjustmentNormalized.Y; collisionAdjustmentNormalized.Y = temporaryFloat; float length = Vector2.Dot( new Vector2(TopParent.Velocity.X, TopParent.Velocity.Y), collisionAdjustmentNormalized); TopParent.Velocity.X = collisionAdjustmentNormalized.X * length; TopParent.Velocity.Y = collisionAdjustmentNormalized.Y * length; } }
public float LengthSquared() { return(_vector.LengthSquared()); }
//Extracted from Box2D /// <summary> /// Returns the convex hull from the given vertices. /// </summary> /// <param name="vertices">The vertices.</param> public static Vertices GetConvexHull(Vertices vertices) { if (vertices.Count <= 3) return vertices; // Find the right most point on the hull int i0 = 0; float x0 = vertices[0].X; for (int i = 1; i < vertices.Count; ++i) { float x = vertices[i].X; if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y)) { i0 = i; x0 = x; } } int[] hull = new int[vertices.Count]; int m = 0; int ih = i0; for (; ; ) { hull[m] = ih; int ie = 0; for (int j = 1; j < vertices.Count; ++j) { if (ie == ih) { ie = j; continue; } Vector2 r = vertices[ie] - vertices[hull[m]]; Vector2 v = vertices[j] - vertices[hull[m]]; float c = MathUtils.Cross(ref r, ref v); if (c < 0.0f) { ie = j; } // Collinearity check if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { ie = j; } } ++m; ih = ie; if (ie == i0) { break; } } Vertices result = new Vertices(m); // Copy vertices. for (int i = 0; i < m; ++i) { result.Add(vertices[hull[i]]); } return result; }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a Shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(BroadPhaseRayCastCallback callback, ref RayCastInput input) { Vector2 p1 = input.Point1; Vector2 p2 = input.Point2; Vector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vector2 t = p1 + maxFraction * (p2 - p1); Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } _raycastStack.Clear(); _raycastStack.Push(_root); while (_raycastStack.Count > 0) { int nodeId = _raycastStack.Pop(); if (nodeId == NullNode) { continue; } //TreeNode<T>* node = &_nodes[nodeId]; if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2 c = _nodes[nodeId].AABB.Center; Vector2 h = _nodes[nodeId].AABB.Extents; float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (_nodes[nodeId].IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; float value = callback(ref subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; Vector2 t = p1 + maxFraction * (p2 - p1); Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } } else { _raycastStack.Push(_nodes[nodeId].Child1); _raycastStack.Push(_nodes[nodeId].Child2); } } }
public override void Update(float dt) { Vector2 f = Vector2.Zero; foreach (Body worldBody in World.BodyList) { if (!IsActiveOn(worldBody)) { continue; } foreach (Body controllerBody in Bodies) { if (worldBody == controllerBody || (worldBody.BodyType == BodyType.Static && controllerBody.BodyType == BodyType.Static) || !controllerBody.Enabled) { continue; } Vector2 d = controllerBody.Position - worldBody.Position; float r2 = d.LengthSquared(); if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) { continue; } switch (GravityType) { case GravityType.DistanceSquared: f = Strength / r2 * worldBody.Mass * controllerBody.Mass * d; break; case GravityType.Linear: f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * controllerBody.Mass * d; break; } worldBody.ApplyForce(ref f); } foreach (Vector2 point in Points) { Vector2 d = point - worldBody.Position; float r2 = d.LengthSquared(); if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) { continue; } switch (GravityType) { case GravityType.DistanceSquared: f = Strength / r2 * worldBody.Mass * d; break; case GravityType.Linear: f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * d; break; } worldBody.ApplyForce(ref f); } } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) { cache = new SimplexCache(); if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled ++GJKCalls; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, ref input.ProxyA, ref input.TransformA, ref input.ProxyB, ref input.TransformB); // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3<int> saveA = new FixedArray3<int>(); FixedArray3<int> saveB = new FixedArray3<int>(); //float distanceSqr1 = Settings.MaxFloat; // Main iteration loop. int iter = 0; while (iter < Settings.MaxGJKIterations) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = simplex.V[i].IndexA; saveB[i] = simplex.V[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } //FPE: This code was not used anyway. // Compute closest point. //Vector2 p = simplex.GetClosestPoint(); //float distanceSqr2 = p.LengthSquared(); // Ensure progress //if (distanceSqr2 >= distanceSqr1) //{ //break; //} //distanceSqr1 = distanceSqr2; // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = simplex.V[simplex.Count]; vertex.IndexA = input.ProxyA.GetSupport(-Complex.Divide(ref d, ref input.TransformA.q)); vertex.WA = Transform.Multiply(input.ProxyA.Vertices[vertex.IndexA], ref input.TransformA); vertex.IndexB = input.ProxyB.GetSupport( Complex.Divide(ref d, ref input.TransformB.q)); vertex.WB = Transform.Multiply(input.ProxyB.Vertices[vertex.IndexB], ref input.TransformB); vertex.W = vertex.WB - vertex.WA; simplex.V[simplex.Count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled ++GJKIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled GJKMaxIters = Math.Max(GJKMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = (output.PointA - output.PointB).Length(); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.UseRadii) { float rA = input.ProxyA.Radius; float rB = input.ProxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; Vector2 normal = output.PointB - output.PointA; normal.Normalize(); output.PointA += rA * normal; output.PointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. Vector2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }