JoltPhysics/Jolt/Physics/Collision/SortReverseAndStore.h

// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT

#pragma once

namespace JPH {

/// This function will sort values from high to low and only keep the ones that are less than inMaxValue
/// @param inValues Values to be sorted
/// @param inMaxValue Values need to be less than this to keep them
/// @param ioIdentifiers 4 identifiers that will be sorted in the same way as the values
/// @param outValues The values are stored here from high to low
/// @return The number of values that were kept
JPH_INLINE int SortReverseAndStore(Vec4Arg inValues, float inMaxValue, UVec4 &ioIdentifiers, float *outValues)
{	
	// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
	Vec4::sSort4Reverse(inValues, ioIdentifiers);

	// Count how many results are less than the max value
	UVec4 closer = Vec4::sLess(inValues, Vec4::sReplicate(inMaxValue));
	int num_results = closer.CountTrues();

	// Shift the values so that only the ones that are less than max are kept
	inValues = inValues.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat();
	ioIdentifiers = ioIdentifiers.ShiftComponents4Minus(num_results);

	// Store the values
	inValues.StoreFloat4((Float4 *)outValues);

	return num_results;
}

} // JPH