public static void ClampMagnitude(ref FixVec2 vector, Fix maxMagnitude)
{
var magnitude = vector.Magnitude;
if (magnitude > maxMagnitude)
{
vector.Normalize();
vector *= maxMagnitude;
}
}
private void FixVec2Test()
{
Debug.Log("------------FixVec2Test-----------");
FixVec2 res1 = new FixVec2(2.222f, 4.444f);
Debug.Log(res1);
res1 = res1.Normalize();
Debug.Log(res1);
Debug.Log(res1.ToVector2().ToString("f3"));
Vector2 res2 = new Vector2(2.222f, 4.444f);
Debug.Log(res2.ToString("f3"));
res2.Normalize();
Debug.Log(res2.ToString("f3"));
Debug.Log("\n");
}
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity accumulatedSteering)
{
_linear = FixVec2.Zero;
_position = owner.Get <Position>().Value;
Proximity.FindNeighbors(owner, _proximityCallback);
var maxVelocity = owner.Get <MaxVelocity>().Value;
if (_linear.MagnitudeSqr == 0)
{
return(SteeringVelocity.Zero);
}
_linear.Normalize();
_linear.Scale(ref maxVelocity);
return(new SteeringVelocity(_linear));
}
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity accumulatedSteering)
{
_averageVelocity = FixVec2.Zero;
var neighborCount = Proximity.FindNeighbors(owner, _proximityCallback);
if (neighborCount > 0)
{
var maxVelocity = owner.Get <MaxVelocity>().Value;
_averageVelocity /= neighborCount;
if (_averageVelocity.MagnitudeSqr == 0)
{
return(SteeringVelocity.Zero);
}
_averageVelocity.Normalize();
_averageVelocity.Scale(ref maxVelocity);
}
return(new SteeringVelocity(_averageVelocity));
}
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity accumulatedSteering)
{
_centroid = FixVec2.Zero;
var neighborCount = Proximity.FindNeighbors(owner, _proximityCallback);
if (neighborCount > 0)
{
_centroid /= neighborCount;
_centroid -= owner.Get <Position>().Value;
if (_centroid.MagnitudeSqr == 0)
{
return(SteeringVelocity.Zero);
}
_centroid.Normalize();
var maxVelocity = owner.Get <MaxVelocity>().Value;
_centroid.Scale(ref maxVelocity);
}
return(new SteeringVelocity(_centroid));
}
internal TFRaycastHit2D Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB, TFLayerMask mask)
{
TFRaycastHit2D hit = new TFRaycastHit2D();
FixVec2 r = pointB - pointA;
if (r.GetMagnitudeSquared() <= Fix.zero)
{
return(hit);
}
r.Normalize();
// v is perpendicular to the segment.
FixVec2 v = FixVec2.Cross(Fix.one, r);
FixVec2 abs_v = FixVec2.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Fix maxFraction = Fix.one;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
FixVec2 t = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, t);
segmentAABB.max = FixVec2.Max(pointA, t);
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
List <TFRaycastOutput> hitNodes = new List <TFRaycastOutput>(2);
while (stack.Count > 0)
{
var nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
var node = nodes[nodeId];
if (!node.aabb.Overlaps(segmentAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
var c = node.aabb.GetCenter();
var h = node.aabb.GetExtents();
var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h);
if (separation > Fix.zero)
{
continue;
}
if (node.IsLeaf())
{
// If value is >= 0, then we hit the node.
TFRaycastHit2D rHit;
Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId, out rHit, mask);
if (value == Fix.zero)
{
// The client has terminated the ray cast.
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
break;
}
if (value == maxFraction)
{
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
}
else if (value > Fix.zero)
{
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
// Update segment bounding box.
maxFraction = value;
FixVec2 g = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, g);
segmentAABB.max = FixVec2.Max(pointA, g);
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
// Decide which node was the closest to the starting point.
Fix closestNode = maxFraction;
for (int i = 0; i < hitNodes.Count; i++)
{
if (hitNodes[i].hit.fraction < closestNode)
{
closestNode = hitNodes[i].hit.fraction;
hit = hitNodes[i].hit;
}
}
return(hit);
}
internal void Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB)
{
FixVec2 r = pointB - pointA;
if (r.GetMagnitudeSquared() <= Fix.zero)
{
return;
}
r.Normalize();
// v is perpendicular to the segment.
FixVec2 v = FixVec2.Cross(Fix.one, r);
FixVec2 abs_v = FixVec2.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Fix maxFraction = Fix.one;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
FixVec2 t = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, t);
segmentAABB.max = FixVec2.Max(pointA, t);
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
while (stack.Count > 0)
{
var nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
var node = nodes[nodeId];
if (!node.aabb.Overlaps(segmentAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
var c = node.aabb.GetCenter();
var h = node.aabb.GetExtents();
var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h);
if (separation > Fix.zero)
{
continue;
}
if (node.IsLeaf())
{
Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId);
if (value == Fix.zero)
{
// The client has terminated the ray cast.
return;
}
if (value > Fix.zero)
{
// Update segment bounding box.
maxFraction = value;
FixVec2 g = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, g);
segmentAABB.max = FixVec2.Max(pointA, g);
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
}