| public static Dot ( Vector2D, v1, Vector2D, v2 ) : double | ||
| v1 | Vector2D, | |
| v2 | Vector2D, | |
| 리턴 | double | |
protected bool PointinTriangle(Vector2D A, Vector2D B, Vector2D C, Vector2D P)
{
Vector2D v0 = C - A;
Vector2D v1 = B - A;
Vector2D v2 = P - A;
float dot00 = (float)v0.Dot(v0);
float dot01 = (float)v0.Dot(v1);
float dot02 = (float)v0.Dot(v2);
float dot11 = (float)v1.Dot(v1);
float dot12 = (float)v1.Dot(v2);
float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * inverDeno;
if (u < 0 || u > 1) // if u out of range, return directly
{
return(false);
}
float v = (dot00 * dot12 - dot01 * dot02) * inverDeno;
if (v < 0 || v > 1) // if v out of range, return directly
{
return(false);
}
return(u + v <= 1);
}
// This is a variant of Fit() where the d1 value is specified.
// Note: has not been tested extensively, particularly the special case
// where one of the arcs beomes a semi-circle...
void Fit(double d1)
{
Vector2D p1 = Point1;
Vector2D p2 = Point2;
Vector2D t1 = Tangent1;
Vector2D t2 = Tangent2;
// fit biarc
Vector2D v = p2 - p1;
double vMagSqr = v.LengthSquared;
// set d1 equal to d2
Vector2D t = t1 + t2;
double tMagSqr = t.LengthSquared;
double vDotT1 = v.Dot(t1);
double vDotT2 = v.Dot(t2);
double t1DotT2 = t1.Dot(t2);
double denominator = (vDotT2 - d1 * (t1DotT2 - 1.0));
if (math.MathUtil.EpsilonEqual(denominator, 0.0, math.MathUtil.ZeroTolerancef))
{
// the second arc is a semicircle
FitD1 = d1;
FitD2 = double.PositiveInfinity;
Vector2D joint = p1 + d1 * t1;
joint += (vDotT2 - d1 * t1DotT2) * t2;
// construct arcs
// [TODO] this might not be right for semi-circle...
SetArcFromEdge(0, p1, t1, joint, true);
SetArcFromEdge(1, p2, t2, joint, false);
}
else
{
double d2 = (0.5 * vMagSqr - d1 * vDotT1) / denominator;
double invLen = 1.0 / (d1 + d2);
Vector2D joint = (d1 * d2) * (t1 - t2);
joint += d1 * p2;
joint += d2 * p1;
joint *= invLen;
FitD1 = d1;
FitD2 = d2;
// draw arcs
SetArcFromEdge(0, p1, t1, joint, true);
SetArcFromEdge(1, p2, t2, joint, false);
}
}
/// <summary>
/// This behaviour returns a force that will steer the agent towards to agent thats trying to evade the pursuing agent.
/// </summary>
/// <returns>Steering Force</returns>
public Vector2D Pursuit(FlyingEntity evader)
{
/* If the evader is ahead and facing the agent then we can just seek
* for the evader's current position. */
Vector2D ToEvader = evader.Pos - _flyingEntity.Pos;
double RelativeHeading = _flyingEntity.Heading.Dot(evader.Heading);
if ((ToEvader.Dot(_flyingEntity.Heading) > 0) &&
(RelativeHeading < -0.95)) // Acos(0.95)= 18 degrees
{
return(Seek(evader.Pos));
}
// If this agent isn't ahead the position of the evader will be predicted.
/* The lookahead time is propotional to the distance between the evader
* and the pursuer; and is inversely proportional to the sum of the
* agent's velocities */
double LookAheadTime = ToEvader.Length() /
(_flyingEntity.MaxSpeed + evader.Speed());
// Now seek to the predicted future position of the evader.
return(Seek(evader.Pos + evader.Velocity * LookAheadTime));
}
public void Intersects(ref Line line, out bool result)
{
Scalar distance;
Vector2D.Dot(ref line.Normal, ref Position, out distance);
result = (distance + line.D) <= Radius;
}
public double MinimumDistanceSqr(Vector2D point, out double t)
{
t = 0;
// Return minimum distance between line segment vw and point p
double l2 = (first - second).sqrMagnitude; // i.e. |w-v|^2 - avoid a sqrt
if (l2 == 0.0)
{
return((point - first).magnitude); // v == w case
}
// Consider the line extending the segment, parameterized as v + t (w - v).
// We find projection of point p onto the line.
// It falls where t = [(p-v) . (w-v)] / |w-v|^2
t = Vector2D.Dot(point - first, second - first) / l2;
if (t < 0.0)
{
return((point - first).magnitude); // Beyond the 'v' end of the segment
}
else if (t > 1.0)
{
return((point - second).magnitude); // Beyond the 'w' end of the segment
}
var projection = first + t * (second - first); // Projection falls on the segment
return((point - projection).sqrMagnitude);
}
public bool TestPoint(Transform xf, Vector2D p)
{
Vector2D center = xf.Position + b2Mat22.Mul(xf.Rotation, m_p);
Vector2D d = p - center;
return(Vector2D.Dot(d, d) <= Radius * Radius);
}
/// <summary>
/// Rotates the entity to a target direction
/// </summary>
/// <param name="target">The target to be rotated to</param>
/// <returns>Returns true of it faces the target</returns>
public bool RotateHeadingToFacePosition(Vector2D target)
{
Vector2D toTarget = Vector2D.Vec2DNormalize(target - position);
float angle = (float)Math.Acos(heading.Dot(toTarget));
if (angle < 0.1)
{
return(true);
}
if (angle > maxTurnRate)
{
angle = maxTurnRate;
}
C2DMatrix RotationMatrix = new C2DMatrix();
RotationMatrix.Rotate(angle * heading.Sign(toTarget));
RotationMatrix.TransformVector2Ds(ref heading);
RotationMatrix.TransformVector2Ds(ref velocity);
side = heading.Perp();
return(false);
}
//--------------------------- RotateHeadingToFacePosition ---------------------
//
// given a target position, this method rotates the entity's heading and
// side vectors by an amount not greater than m_dMaxTurnRate until it
// directly faces the target.
//
// returns true when the heading is facing in the desired direction
//-----------------------------------------------------------------------------
public bool RotateHeadingToFacePosition(Vector2D target)
{
Vector2D toTarget = Vector2D.Vec2DNormalize(target - m_vPos);
//first determine the angle between the heading vector and the target
double angle = Math.Acos(m_vHeading.Dot(toTarget));
//return true if the player is facing the target
if (angle < 0.00001)
{
return(true);
}
//clamp the amount to turn to the max turn rate
if (angle > m_dMaxTurnRate)
{
angle = m_dMaxTurnRate;
}
//The next few lines use a rotation matrix to rotate the player's heading
//vector accordingly
C2DMatrix RotationMatrix = new C2DMatrix();
//notice how the direction of rotation has to be determined when creating
//the rotation matrix
RotationMatrix.Rotate(angle * m_vHeading.Sign(toTarget));
RotationMatrix.TransformVector2D(m_vHeading);
RotationMatrix.TransformVector2D(m_vVelocity);
//finally recreate m_vSide
m_vSide = m_vHeading.Perp();
return(false);
}
public static void GetInertia(Vector2D[] vertexes, out Scalar result)
{
if (vertexes == null)
{
throw new ArgumentNullException("vertexes");
}
if (vertexes.Length == 0)
{
throw new ArgumentOutOfRangeException("vertexes");
}
if (vertexes.Length == 1)
{
result = 0;
return;
}
Scalar denom = 0;
Scalar numer = 0;
Scalar a, b, c, d;
Vector2D v1, v2;
v1 = vertexes[vertexes.Length - 1];
for (int index = 0; index < vertexes.Length; index++, v1 = v2)
{
v2 = vertexes[index];
Vector2D.Dot(ref v2, ref v2, out a);
Vector2D.Dot(ref v2, ref v1, out b);
Vector2D.Dot(ref v1, ref v1, out c);
Vector2D.ZCross(ref v1, ref v2, out d);
d = Math.Abs(d);
numer += d;
denom += (a + b + c) * d;
}
result = denom / (numer * 6);
}
public static int WhichSide(Triangle2d V, Vector2D P, Vector2D D)
{
// Vertices are projected to the form P+t*D. Return value is +1 if all
// t > 0, -1 if all t < 0, 0 otherwise, in which case the line splits the
// triangle.
int positive = 0, negative = 0, zero = 0;
for (int i = 0; i < 3; ++i)
{
double t = D.Dot(V[i] - P);
if (t > (double)0)
{
++positive;
}
else if (t < (double)0)
{
++negative;
}
else
{
++zero;
}
if (positive > 0 && negative > 0)
{
return(0);
}
}
return(zero == 0 ? (positive > 0 ? 1 : -1) : 0);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
// Projection of P-C onto plane is Q-C = P-C - Dot(N,P-C)*N.
Vector2D PmC = point - circle.Center;
double lengthPmC = PmC.Length;
if (lengthPmC > math.MathUtil.Epsilon)
{
CircleClosest = circle.Center + circle.Radius * PmC / lengthPmC;
AllCirclePointsEquidistant = false;
}
else
{
// All circle points are equidistant from P. Return one of them.
CircleClosest = circle.Center + circle.Radius;
AllCirclePointsEquidistant = true;
}
Vector2D diff = point - CircleClosest;
double sqrDistance = diff.Dot(diff);
// Account for numerical round-off error.
if (sqrDistance < 0)
{
sqrDistance = 0;
}
DistanceSquared = sqrDistance;
return(sqrDistance);
}
private static Vector2D[] FindIncidentEdge(
PolygonShape polygon1, Transform2D transform1, int edge1,
PolygonShape polygon2, Transform2D transform2)
{
Debug.Assert(0 <= edge1 && edge1 < polygon1.Normals.Length);
// 将 polygon1 的本地坐标系转换到 polygon2 的本地坐标系
var normal1 = transform2.Q.InverseRotate(transform1.Q.Rotate(polygon1.Normals[edge1]));
// 找到和法线最垂直的边索引，即入射边
var minS = float.MaxValue;
var sIndex = 0;
for (int i = 0; i < polygon2.Normals.Length; ++i)
{
var s = Vector2D.Dot(normal1, polygon2.Normals[i]);
if (s < minS)
{
minS = s;
sIndex = i;
}
}
var sIndexNext = sIndex + 1 < polygon2.Verticles.Length ? sIndex + 1 : 0;
var incidentEdge = new Vector2D[2];
incidentEdge[0] = transform2.Transform(polygon1.Verticles[sIndex]);
incidentEdge[1] = transform2.Transform(polygon1.Verticles[sIndexNext]);
return(incidentEdge);
}
/// <summary>
/// Finds the distance of a specified point from the line segment and the
/// closest point on the segment to the specified point.
/// </summary>
/// <param name="p">The test point.</param>
/// <param name="closestPt">Closest point on the segment to c.</param>
/// <returns>Returns the distance from p to the closest point on the segment.</returns>
public double Distance(Point2D p, Point2D closestPt)
{
if (closestPt == null)
closestPt = new Point2D();
// Construct vector v (AB) and w (AP)
var v = new Vector2D(A, B);
var w = new Vector2D(A, p);
// Numerator of the component of w onto v. If <= 0 then A
// is the closest point. By separating into the numerator
// and denominator of the component we avoid a division unless
// it is necessary.
double n = w.Dot(v);
if (n <= 0.0f)
{
closestPt.Set(A);
return w.Norm();
}
// Get the denominator of the component. If the component >= 1
// (d <= n) then point B is the closest point
double d = v.Dot(v);
if (d <= n)
{
closestPt.Set(B);
return new Vector2D(B, p).Norm();
}
// Closest point is along the segment. The point is the projection of
// w onto v.
closestPt.Set(v.Mult(n / d));
closestPt.Add(A);
return new Vector2D(closestPt, p).Norm();
}
void SetArcFromEdge(int i, Vector2D p1, Vector2D t1, Vector2D p2, bool fromP1)
{
Vector2D chord = p2 - p1;
Vector2D n1 = new Vector2D(-t1.y, t1.x);
double chordDotN1 = chord.Dot(n1);
if (math.MathUtil.EpsilonEqual(chordDotN1, 0, Epsilon))
{
// straight line case
Set_arc(i, new Arc(p1, p2));
}
else
{
double radius = chord.LengthSquared / (2.0 * chordDotN1);
Vector2D center = p1 + radius * n1;
Vector2D p1Offset = p1 - center;
Vector2D p2Offset = p2 - center;
var p1Ang1 = Math.Atan2(p1Offset.y, p1Offset.x);
var p2Ang1 = Math.Atan2(p2Offset.y, p2Offset.x);
if (p1Offset.x * t1.y - p1Offset.y * t1.x > 0)
{
Set_arc(i, new Arc(center, Math.Abs(radius), p1Ang1, p2Ang1, !fromP1));
}
else
{
Set_arc(i, new Arc(center, Math.Abs(radius), p1Ang1, p2Ang1, fromP1));
}
}
}
private Vector3D GerstnerWave(Vector4D wave, Vector3D p, Vector3D tangent, Vector3D binormal, float time /* Time since level load */)
{
float steepness = wave.Z;
float wavelength = wave.W;
float k = 2 * (float)(Math.PI / wavelength);
float c = (float)Math.Sqrt(9.8 / k);
Vector2D d = new Vector2D(wave.X, wave.Y).Normalized();
float f = k * (Vector2D.Dot(d, new Vector2D(p.X, p.Z)) - c * time);
float a = steepness / k;
tangent += new Vector3D(
(float)(-d.X * d.X * (steepness * Math.Sin(f))),
(float)(d.X * (steepness * Math.Cos(f))),
(float)(-d.X * d.Y * (steepness * Math.Sin(f)))
);
binormal += new Vector3D(
(float)(-d.X * d.Y * (steepness * Math.Sin(f))),
(float)(d.Y * (steepness * Math.Cos(f))),
(float)(-d.Y * d.Y * (steepness * Math.Sin(f)))
);
return(new Vector3D(
(float)(d.X * (a * Math.Cos(f))),
(float)(a * Math.Sin(f)),
(float)(d.Y * (a * Math.Cos(f)))
));
}
/// <summary>
/// Determines if a ray interescts a circle
/// </summary>
/// <param name="rayOrig">The origin of the ray</param>
/// <param name="rayDir">The direction of the ray</param>
/// <param name="center">The center of the circle</param>
/// <param name="r">The radius of the circle</param>
/// <returns></returns>
public static bool Intersects(Vector2D rayOrig, Vector2D rayDir, Vector2D center, double r)
{
// ray-circle intersection test
// P: hit point
// ray: P = O + tV
// circle: (P-C)dot(P-C)-r^2 = 0
// substitute to solve for t gives a quadratic equation:
// a = VdotV
// b = 2(O-C)dotV
// c = (O-C)dot(O-C)-r^2
// if the discriminant is negative, miss (no solution for P)
// otherwise, if both roots are positive, hit
double a = rayDir.Dot(rayDir);
double b = ((rayOrig - center) * 2.0).Dot(rayDir);
double c = (rayOrig - center).Dot(rayOrig - center) - r * r;
// discriminant
double disc = b * b - 4.0 * a * c;
if (disc < 0.0)
{
return(false);
}
// find the signs of the roots
// technically we should also divide by 2a
// but all we care about is the sign, not the magnitude
double root1 = -b + Math.Sqrt(disc);
double root2 = -b - Math.Sqrt(disc);
return(root1 > 0.0 && root2 > 0.0);
}
public bool RayCast(out b2RayCastOutput castOutput, b2RayCastInput input, Transform transform, int childIndex)
{
Vector2D position = transform.Position + b2Mat22.Mul(transform.Rotation, m_p);
Vector2D s = input.p1 - position;
float b = Vector2D.Dot(s, s) - Radius * Radius;
castOutput.fraction = 0.0f;
castOutput.Normal = null;
// Solve quadratic equation
Vector2D r = input.p2 - input.p1;
float c = Vector2D.Dot(s, r);
float rr = Vector2D.Dot(r, r);
float sigma = c * c - rr * b;
if (sigma < 0.0f || rr < float.Epsilon)
{
return(false);
}
float a = -(float)(c + System.Math.Sqrt(sigma));
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
castOutput.fraction = a;
castOutput.Normal = s + r * a;
castOutput.Normal = castOutput.Normal.Normal();
return(true);
}
return(false);
}
public static MassInfo FromPolygon(Vector2D[] vertexes, Scalar mass)
{
if (vertexes == null)
{
throw new ArgumentNullException("vertexes");
}
if (vertexes.Length == 0)
{
throw new ArgumentOutOfRangeException("vertexes");
}
if (vertexes.Length == 1)
{
return(new MassInfo(mass, 0));
}
Scalar denom = 0.0f;
Scalar numer = 0.0f;
for (int j = vertexes.Length - 1, i = 0; i < vertexes.Length; j = i, i++)
{
Scalar a, b, c;
Vector2D P0 = vertexes[j];
Vector2D P1 = vertexes[i];
Vector2D.Dot(ref P1, ref P1, out a);
Vector2D.Dot(ref P1, ref P0, out b);
Vector2D.Dot(ref P0, ref P0, out c);
a += b + c;
Vector2D.ZCross(ref P0, ref P1, out b);
b = Math.Abs(b);
denom += (b * a);
numer += b;
}
return(new MassInfo(mass, (mass * denom) / (numer * 6)));
}
public void Dot()
{
Scalar expected = -(Value * Value * 2);
UnitHelper.RefOperationTester <Vector2D, Vector2D, Scalar>(op1, op2, expected, Vector2D.Dot, "Dot");
Assert.AreEqual(expected, op1 * op2, "Operator");
Assert.AreEqual(expected, Vector2D.Dot(op1, op2), "Method");
}
public void ComputeMass(b2MassData massData, float density)
{
massData.Mass = density * (float)System.Math.PI * Radius * Radius;
massData.Center = m_p;
// Inertia about the local origin
massData.I = massData.Mass * (0.5f * Radius * Radius + Vector2D.Dot(m_p, m_p));
}
private static void RefExtensionMethods()
{
var p = new Vector2D(1, 2);
var q = new Vector2D(3, 4);
Console.WriteLine(p.Dot(q));
Console.WriteLine(p.Cross(q));
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector2D diff = line1.Origin - line2.Origin;
double a01 = -line1.Direction.Dot(line2.Direction);
double b0 = diff.Dot(line1.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1.0 - a01 * a01);
double b1, s0, s1, sqrDist;
if (det >= math.MathUtil.ZeroTolerance)
{
// Lines are not parallel.
b1 = -diff.Dot(line2.Direction);
double invDet = ((double)1) / det;
s0 = (a01 * b1 - b0) * invDet;
s1 = (a01 * b0 - b1) * invDet;
sqrDist = (double)0;
}
else
{
// Lines are parallel, select any closest pair of points.
s0 = -b0;
s1 = (double)0;
sqrDist = b0 * s0 + c;
// Account for numerical round-off errors.
if (sqrDist < (double)0)
{
sqrDist = (double)0;
}
}
Line1Parameter = s0;
Line1Closest = line1.Origin + s0 * line1.Direction;
Line2Parameter = s1;
Line2Closest = line2.Origin + s1 * line2.Direction;
DistanceSquared = sqrDist;
return(sqrDist);
}
// Evaluate the quadratic function Q(X) = (X-K)^T * M * (X-K) - 1.
public double Evaluate(Vector2D point)
{
Vector2D diff = point - Center;
double ratio0 = Axis0.Dot(diff) / Extent[0];
double ratio1 = Axis1.Dot(diff) / Extent[1];
double value = ratio0 * ratio0 + ratio1 * ratio1 - (double)1;
return(value);
}
public void Vector2DotTest2()
{
Vector2D <float> a = new Vector2D <float>(float.MinValue, float.MinValue);
Vector2D <float> b = new Vector2D <float>(float.MaxValue, float.MaxValue);
float actual = Vector2D.Dot(a, b);
Assert.True(float.IsNegativeInfinity(actual), "Vector2f.Dot did not return the expected value.");
}
public void Vector2DotTest2()
{
Vector2D a = new Vector2D(double.MinValue, double.MinValue);
Vector2D b = new Vector2D(double.MaxValue, double.MaxValue);
double actual = Vector2D.Dot(a, b);
Assert.True(double.IsNegativeInfinity(actual), "Vector2D.Dot did not return the expected value.");
}
public void Vector2DotTest1()
{
Vector2D a = new Vector2D(1.55f, 1.55f);
Vector2D b = new Vector2D(-1.55f, 1.55f);
double expected = 0.0f;
double actual = Vector2D.Dot(a, b);
Assert.AreEqual(expected, actual);
}
public void Vector2DotTest1()
{
Vector2D <float> a = new Vector2D <float>(1.55f, 1.55f);
Vector2D <float> b = new Vector2D <float>(-1.55f, 1.55f);
float expected = 0.0f;
float actual = Vector2D.Dot(a, b);
Assert.Equal(expected, actual);
}
public void StaticDotTest(double x1, double y1, double x2, double y2, double result)
{
var vector1 = new Vector2D(x1, y1);
vector1.Normalize();
var vector2 = new Vector2D(x2, y2);
vector2.Normalize();
Assert.AreEqual(result, Vector2D.Dot(vector1, vector2), Helper.E);
}
public static VertexInfo GetVertexInfoOfRange(Vector2D[][] polygons)
{
if (polygons == null)
{
throw new ArgumentNullException("polygons");
}
if (polygons.Length == 0)
{
throw new ArgumentOutOfRangeException("polygons");
}
Scalar a, b, c, d;
Scalar denom = 0;
Scalar numer = 0;
Scalar areaTotal = 0;
Vector2D centroid = Vector2D.Zero;
for (int index1 = 0; index1 < polygons.Length; ++index1)
{
Vector2D[] vertexes = polygons[index1];
if (vertexes == null)
{
throw new ArgumentNullException("polygons");
}
if (vertexes.Length < 3)
{
throw new ArgumentOutOfRangeException("polygons", "There must be at least 3 vertexes");
}
Scalar area = 0;
Vector2D v1 = vertexes[vertexes.Length - 1];
Vector2D v2;
for (int index = 0; index < vertexes.Length; ++index, v1 = v2)
{
v2 = vertexes[index];
Vector2D.Dot(ref v2, ref v2, out a);
Vector2D.Dot(ref v2, ref v1, out b);
Vector2D.Dot(ref v1, ref v1, out c);
Vector2D.ZCross(ref v1, ref v2, out d);
area += d;
centroid.X += ((v1.X + v2.X) * d);
centroid.Y += ((v1.Y + v2.Y) * d);
d = Math.Abs(d);
numer += d;
denom += (a + b + c) * d;
}
areaTotal += Math.Abs(area);
}
areaTotal *= .5f;
d = 1 / (areaTotal * 6);
centroid.X *= d;
centroid.Y *= d;
return(new VertexInfo(
centroid,
(numer == 0) ? (1) : (denom / (numer * 6)),
areaTotal));
}
public void Vector2DotTest()
{
Vector2D <float> a = new Vector2D <float>(1.0f, 2.0f);
Vector2D <float> b = new Vector2D <float>(3.0f, 4.0f);
float expected = 11.0f;
float actual;
actual = Vector2D.Dot(a, b);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Dot did not return the expected value.");
}
public void Vector2DotTest()
{
Vector2D a = new Vector2D(1.0f, 2.0f);
Vector2D b = new Vector2D(3.0f, 4.0f);
double expected = 11.0f;
double actual;
actual = Vector2D.Dot(a, b);
Assert.True(MathHelper.Equal(expected, actual), "Vector2D.Dot did not return the expected value.");
}
private Vector3D MapToSphere(Vector2D pt)
{
Vector2D v = new Vector2D();
v.x = (w - pt.x) * adjustWidth;
v.y = (h - pt.y) * adjustHeight;
double length = v.Dot(v);
if (length > 1.0)
{
double norm = 1.0 / Math.Sqrt(length);
return new Vector3D(v.x * norm, v.y * norm, 0);
}
else
{
return new Vector3D(v.x, v.y, Math.Sqrt(1.0 - length));
}
}
public void Dot()
{
Vector2D a = new Vector2D(1.0, 2.0);
Vector2D b = new Vector2D(4.0, 5.0);
double dot = a.Dot(b);
Assert.AreEqual(1.0 * 4.0 + 2.0 * 5.0, dot, 1e-14);
}
