public void Abs() { Vector2d v = new Vector2d(-1, -2); v.Abs(); Assert.AreEqual(v, new Vector2d(1, 2)); }
public static void Solve(Circle ball1, Circle ball2) { Vector2d v = ball1.centerOfMass - ball2.centerOfMass; double l = v.Abs(); if (l < ball1.r + ball2.r) { Vector2d v0 = v / l, v1 = Vector2d.Scalar(ball1.forwardSpeed, v0) * v0, v2 = Vector2d.Scalar(ball2.forwardSpeed, v0) * v0; if (Vector2d.Scalar(v1 - v2, v0) > 0) { return; } Vector2d nv1 = ((ball1.m - ball2.m) * v1 + 2 * v2 * ball2.m) / (ball1.m + ball2.m), nv2 = ball1.m / ball2.m * (v1 - nv1) + v2; ball1.forwardSpeed = ball1.forwardSpeed - v1 + nv1; ball2.forwardSpeed = ball2.forwardSpeed - v2 + nv2; } }
public void Draw() { if (!hide) { Vector2d v = m - aim; double c = v.Abs(), a = v.x, cos = a / c; if (!(aim == lastAim && cos == lastCos)) { lastCos = cos; lastAim = aim; Vector2d v0 = v / v.Abs(), nv1 = new Vector2d(-v0.y, v0.x), // Развернули против часовой стрелки nv2 = new Vector2d(v0.y, -v0.x); // По часовой стрелке Point2d start = this.aim + v0 * (r + 0.1), end = this.aim + v0 * length; corners[0] = start + nv1 * startR; corners[1] = end + nv1 * endR; corners[2] = end + nv2 * endR; corners[3] = start + nv2 * startR; } Gl.glBegin(Gl.GL_QUADS); Gl.glColor3f(0, 0, 0); foreach (var p in corners) { Gl.glVertex2d(p.x, p.y); } Gl.glEnd(); } }
private void AnT_MouseDown(object sender, System.Windows.Forms.MouseEventArgs e) { double x = game.width * (e.X) / game.AnT.Width, y = game.height * (game.AnT.Height - e.Y) / game.AnT.Height; BlackEngine.Physics.Objects.Circle circle = (BlackEngine.Physics.Objects.Circle)game.ball.physicalObject; Vector2d v = circle.centerOfMass - new Point2d(x, y), v0 = v / v.Abs(); circle.forwardSpeed = v0 * 7; waitHandle.Set(); }
public static void Solve(Circle ball, GravityTrap trap) { Vector2d v = trap.center - ball.centerOfMass; double l = v.Abs(); if (l < trap.r) { if (l < 0.1) { ball.forwardSpeed = new Vector2d(); BallBroked(ball); } Vector2d v0 = v / l, d = v0 * 0.05; ball.forwardSpeed = new Vector2d(); ball.centerOfMass += d; } }
private void AnT_MouseMove(object sender, System.Windows.Forms.MouseEventArgs e) { double x = game.width * (e.X) / game.AnT.Width, y = game.height * (game.AnT.Height - e.Y) / game.AnT.Height; BlackEngine.Physics.Objects.Circle circle = (BlackEngine.Physics.Objects.Circle)game.ball.physicalObject; Vector2d v = circle.centerOfMass - new Point2d(x, y), v0 = v / v.Abs(); trackBall.forwardSpeed = v0 * 7; List <Point2d> way = CalcWay(circle); game.ballWay.way = way; game.cue.m = new Point2d(x, y); }
/// <summary> /// Calculates the L2 (Euclidiean) geodesic distance from the given sources. /// </summary> /// <param name="cost"></param> /// <param name="sources"></param> /// <param name="result"></param> /// <param name="exclude"></param> public static void CalculateL2(GridField2d <double> cost, IEnumerable <int> sources, double[] result, IEnumerable <int> exclude = null) { // impl ref // http://www.numerical-tours.com/matlab/fastmarching_0_implementing/ var costVals = cost.Values; (var nx, var ny) = cost.Count; (var dx, var dy) = Vector2d.Abs(cost.Scale); var eikonal = new Eikonal2d(dx, dy); var queue = new PriorityQueue <double, int>(); result.SetRange(double.PositiveInfinity, cost.CountXY); // enqueue sources foreach (int i in sources) { result[i] = 0.0; queue.Insert(0.0, i); } // exclude if (exclude != null) { foreach (var i in exclude) { result[i] = 0.0; } } // breadth first search from sources while (queue.Count > 0) { (double d0, int i0) = queue.RemoveMin(); if (result[i0] < d0) { continue; // skip if lower value has been assigned } (int x0, int y0) = cost.ToGridSpace(i0); if (x0 > 0) { TryUpdateX(i0 - 1); } if (x0 < nx - 1) { TryUpdateX(i0 + 1); } if (y0 > 0) { TryUpdateY(i0 - nx); } if (y0 < ny - 1) { TryUpdateY(i0 + nx); } // process x neighbor void TryUpdateX(int index) { var d1 = result[index]; if (d1 < d0) { return; // no backtracking } double d2; double minY = GetMinY(index); // will return infinity if neither neighbor has been visited if (minY > double.MaxValue || !eikonal.Solve(d0, minY, costVals[index], out d2)) { d2 = d0 + dx * costVals[index]; } // add to queue if less than current min if (d2 < d1) { result[index] = d2; queue.Insert(d2, index); } } // process y neighbor void TryUpdateY(int index) { var d1 = result[index]; if (d1 < d0) { return; // no backtracking } double d2; double minX = GetMinX(index); // will return infinity if neither neighbor has been visited if (minX > double.MaxValue || !eikonal.Solve(minX, d0, costVals[index], out d2)) { d2 = d0 + dy * costVals[index]; } // add to queue if less than current min if (d2 < d1) { result[index] = d2; queue.Insert(d2, index); } } // returns the minimum adjacent value in the x double GetMinX(int index) { if (x0 == 0) { return(result[index + 1]); } else if (x0 == nx - 1) { return(result[index - 1]); } return(Math.Min(result[index - 1], result[index + 1])); } // returns the minimum adjacent value in the y double GetMinY(int index) { if (y0 == 0) { return(result[index + nx]); } else if (y0 == ny - 1) { return(result[index - nx]); } return(Math.Min(result[index - nx], result[index + nx])); } } }
/// <summary> /// Calculates the L1 (Manhattan) geodesic distance from the given sources. /// </summary> /// <param name="cost"></param> /// <param name="sources"></param> /// <param name="result"></param> /// <param name="exclude"></param> public static void CalculateL1(GridField2d <double> cost, IEnumerable <int> sources, double[] result, IEnumerable <int> exclude = null) { // impl ref // http://www.numerical-tours.com/matlab/fastmarching_0_implementing/ var costVals = cost.Values; (var nx, var ny) = cost.Count; (double dx, double dy) = Vector2d.Abs(cost.Scale); var queue = new PriorityQueue <double, int>(); result.SetRange(double.PositiveInfinity, cost.CountXY); // enqueue sources foreach (int i in sources) { result[i] = 0.0; queue.Insert(0.0, i); } // exclude if (exclude != null) { foreach (int i in exclude) { result[i] = 0.0; } } // breadth first search from sources while (queue.Count > 0) { (var d0, int i0) = queue.RemoveMin(); if (result[i0] < d0) { continue; // skip if lower value has been assigned } (int x0, int y0) = cost.ToGridSpace(i0); // -x if (x0 > 0) { TryUpdate(d0 + dx * costVals[i0 - 1], i0 - 1); } // +x if (x0 < nx - 1) { TryUpdate(d0 + dx * costVals[i0 + 1], i0 + 1); } // -y if (y0 > 0) { TryUpdate(d0 + dy * costVals[i0 - nx], i0 - nx); } // +y if (y0 < ny - 1) { TryUpdate(d0 + dy * costVals[i0 + nx], i0 + nx); } // add to queue if less than current min void TryUpdate(double distance, int index) { if (distance < result[index]) { result[index] = distance; queue.Insert(distance, index); } } } }
/// <summary> /// Calculates the L1 (Manhattan) geodesic distance from the given sources. /// </summary> /// <param name="grid"></param> /// <param name="sources"></param> /// <param name="result"></param> /// <param name="exclude"></param> public static void CalculateL1(Grid2d grid, IEnumerable <int> sources, double[] result, IEnumerable <int> exclude = null) { // impl ref // http://www.numerical-tours.com/matlab/fastmarching_0_implementing/ (var nx, var ny) = grid.Count; (var dx, var dy) = Vector2d.Abs(grid.Scale); var queue = new Queue <int>(); result.SetRange(double.PositiveInfinity, grid.CountXY); // enqueue sources foreach (int i in sources) { result[i] = 0.0; queue.Enqueue(i); } // exclude if (exclude != null) { foreach (int i in exclude) { result[i] = 0.0; } } // breadth first search from sources while (queue.Count > 0) { int i0 = queue.Dequeue(); var d0 = result[i0]; (int x0, int y0) = grid.ToGridSpace(i0); // -x if (x0 > 0) { TryUpdate(d0 + dx, i0 - 1); } // +x if (x0 < nx - 1) { TryUpdate(d0 + dx, i0 + 1); } // -y if (y0 > 0) { TryUpdate(d0 + dy, i0 - nx); } // +y if (y0 < ny - 1) { TryUpdate(d0 + dy, i0 + nx); } // add to queue if less than current min void TryUpdate(double distance, int index) { if (distance < result[index]) { result[index] = distance; queue.Enqueue(index); } } } }
/// <summary> /// Raycast against this AABB using the specified points and maxfraction (found in input) /// </summary> /// <param name="output">The results of the raycast.</param> /// <param name="input">The parameters for the raycast.</param> /// <returns>True if the ray intersects the AABB</returns> public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true) { // From Real-time Collision Detection, p179. output = new RayCastOutput(); var tmin = -long.MaxValue; var tmax = long.MaxValue; var p = input.Point1; var d = input.Point2 - input.Point1; var absD = Vector2d.Abs(d); var normal = new Vector2d(0, 0); for (int i = 0; i < 2; ++i) { long absD_i = i == 0 ? absD.x : absD.y; long lowerBound_i = i == 0 ? LowerBound.x : LowerBound.y; long upperBound_i = i == 0 ? UpperBound.x : UpperBound.y; long p_i = i == 0 ? p.x : p.y; if (absD_i < 1) { // Parallel. if (p_i < lowerBound_i || upperBound_i < p_i) { return(false); } } else { long d_i = i == 0 ? d.x : d.y; long inv_d = FixedMath.One.Div(d_i); long t1 = (lowerBound_i - p_i).Mul(inv_d); long t2 = (upperBound_i - p_i).Mul(inv_d); // Sign of the normal vector. long s = -FixedMath.One; if (t1 > t2) { Utility.Swap(ref t1, ref t2); s = FixedMath.One; } // Push the min up if (t1 > tmin) { if (i == 0) { normal.x = s; } else { normal.y = 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 (doInteriorCheck && (tmin < 0 || input.MaxFraction < tmin)) { return(false); } // Intersection. output.Fraction = tmin; output.Normal = normal; return(true); }
/// <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(Func <RayCastInput, int, long> callback, ref RayCastInput input) { Vector2d p1 = input.Point1; Vector2d p2 = input.Point2; Vector2d r = p2 - p1; Debug.Assert(r.SqrMagnitude() > 0); r.Normalize(); // v is perpendicular to the segment. Vector2d absV = Vector2d.Abs(new Vector2d(-r.y, r.x)); //Velcro: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) var maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vector2d t = p1 + maxFraction * (p2 - p1); Vector2d.Min(ref p1, ref t, out segmentAABB.LowerBound); Vector2d.Max(ref p1, ref t, out segmentAABB.UpperBound); segmentAABB.UpdateCenterAndExtents(); } _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 node.AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2d c = node.AABB.Center; Vector2d h = node.AABB.Extents; var separation = Math.Abs(Vector2d.Dot(new Vector2d(-r.y, r.x), p1 - c)) - Vector2d.Dot(absV, h); if (separation > 0) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; long value = callback(subInput, nodeId); if (value == 0) { // the client has terminated the raycast. return; } if (value > 0) { // Update segment bounding box. maxFraction = value; Vector2d t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Vector2d.Min(p1, t); segmentAABB.UpperBound = Vector2d.Max(p1, t); segmentAABB.UpdateCenterAndExtents(); } } else { _raycastStack.Push(node.Child1); _raycastStack.Push(node.Child2); } } }
public void Vector2dAbsWorks() { var v = new Vector2d(3, -4); Assert.True(v.Abs().Equivalent(new Vector2d(3, 4))); }