/// <summary>
/// Add value to counter and saturate to maximum specified.
/// </summary>
/// <param name="value">Value to add to counter.</param>
/// <param name="max">Maximum value of counter.</param>
/// <returns></returns>
public int AddSat(int value, int max = int.MaxValue)
{
int oldVal;
int newVal = *Counter;
do
{
oldVal = newVal;
newVal = newVal >= max ? max : math.min(max, newVal + value);
newVal = Interlocked.CompareExchange(ref UnsafeUtilityEx.AsRef <int>(Counter), newVal, oldVal);
}while (oldVal != newVal && oldVal != max);
return(oldVal);
}
/// <summary>
/// Subtract value from counter and staturate to minimum specified.
/// </summary>
/// <param name="value">Value to subtract from counter.</param>
/// <param name="min">Minumum value of counter.</param>
/// <returns></returns>
public int SubSat(int value, int min = int.MinValue)
{
int oldVal;
int newVal = *Counter;
do
{
oldVal = newVal;
newVal = newVal <= min ? min : math.max(min, newVal - value);
newVal = Interlocked.CompareExchange(ref UnsafeUtilityEx.AsRef <int>(Counter), newVal, oldVal);
}while (oldVal != newVal && oldVal != min);
return(oldVal);
}
/// <summary>
/// Subtract value from counter and staturate to minimum specified.
/// </summary>
/// <param name="value">Value to subtract from counter.</param>
/// <param name="min">Minumum value of counter.</param>
/// <returns></returns>
public long SubSat(long value, long min = long.MinValue)
{
long oldVal;
long newVal = *Counter;
do
{
oldVal = newVal;
newVal = newVal <= min ? min : math.max(min, newVal - value);
newVal = Interlocked.CompareExchange(ref UnsafeUtilityEx.AsRef <long>(Counter), newVal, oldVal);
}while (oldVal != newVal && oldVal != min);
return(oldVal);
}
/// <summary>
/// Add value to counter and saturate to maximum specified.
/// </summary>
/// <param name="value">Value to add to counter.</param>
/// <param name="max">Maximum value of counter.</param>
/// <returns></returns>
public long AddSat(long value, long max = long.MaxValue)
{
long oldVal;
long newVal = *Counter;
do
{
oldVal = newVal;
newVal = newVal >= max ? max : math.min(max, newVal + value);
newVal = Interlocked.CompareExchange(ref UnsafeUtilityEx.AsRef <long>(Counter), newVal, oldVal);
}while (oldVal != newVal && oldVal != max);
return(oldVal);
}
/// <summary>
/// Adds value to counter.
/// </summary>
/// <param name="value">Value to add to counter.</param>
/// <returns></returns>
public int Add(int value)
{
return(Interlocked.Add(ref UnsafeUtilityEx.AsRef <int>(Counter), value) - value);
}
/// <summary>
/// Adds value to counter.
/// </summary>
/// <param name="value">Value to add to counter.</param>
/// <returns></returns>
public long Add(long value)
{
return(Interlocked.Add(ref UnsafeUtilityEx.AsRef <long>(Counter), value) - value);
}
internal unsafe void KNearest(float3 queryPosition, NativeSlice <int> result, ref KnnQueryTemp temp)
{
int k = result.Length;
temp.Heap.Clear();
var points = Points;
var permutation = m_permutation;
var rootNode = RootNode;
var nodePtr = m_nodes.GetUnsafePtr();
// Biggest Smallest Squared Radius
float bssr = float.PositiveInfinity;
float3 rootClosestPoint = rootNode.Bounds.ClosestPoint(queryPosition);
PushToHeap(m_rootNodeIndex[0], rootClosestPoint, queryPosition, ref temp);
// searching
while (temp.MinHeap.Count > 0)
{
KdQueryNode queryNode = temp.MinHeap.PopObj();
if (queryNode.Distance > bssr)
{
continue;
}
ref KdNode node = ref UnsafeUtilityEx.ArrayElementAsRef <KdNode>(nodePtr, queryNode.NodeIndex);
if (!node.Leaf)
{
int partitionAxis = node.PartitionAxis;
float partitionCoord = node.PartitionCoordinate;
float3 tempClosestPoint = queryNode.TempClosestPoint;
if (tempClosestPoint[partitionAxis] - partitionCoord < 0)
{
// we already know we are on the side of negative bound/node,
// so we don't need to test for distance
// push to stack for later querying
PushToHeap(node.NegativeChildIndex, tempClosestPoint, queryPosition, ref temp);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
if (node.Count != 0)
{
PushToHeap(node.PositiveChildIndex, tempClosestPoint, queryPosition, ref temp);
}
}
else
{
// we already know we are on the side of positive bound/node,
// so we don't need to test for distance
// push to stack for later querying
PushToHeap(node.PositiveChildIndex, tempClosestPoint, queryPosition, ref temp);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
if (node.Count != 0)
{
PushToHeap(node.NegativeChildIndex, tempClosestPoint, queryPosition, ref temp);
}
}
}
else
{
for (int i = node.Start; i < node.End; i++)
{
int index = permutation[i];
float sqrDist = math.lengthsq(points[index] - queryPosition);
if (sqrDist <= bssr)
{
temp.Heap.PushObj(index, sqrDist);
if (temp.Heap.Count == k)
{
bssr = temp.Heap.HeadValue;
}
}
}
}
}
