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