2715 lines
99 KiB
C++
2715 lines
99 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include <Jolt/Jolt.h>
|
|
|
|
#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
|
|
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
|
|
#include <Jolt/Physics/Collision/RayCast.h>
|
|
#include <Jolt/Physics/Collision/ShapeCast.h>
|
|
#include <Jolt/Physics/Collision/CastResult.h>
|
|
#include <Jolt/Physics/Collision/CollidePointResult.h>
|
|
#include <Jolt/Physics/Collision/ShapeFilter.h>
|
|
#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
|
|
#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
|
|
#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
|
|
#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
|
|
#include <Jolt/Physics/Collision/TransformedShape.h>
|
|
#include <Jolt/Physics/Collision/ActiveEdges.h>
|
|
#include <Jolt/Physics/Collision/CollisionDispatch.h>
|
|
#include <Jolt/Physics/Collision/SortReverseAndStore.h>
|
|
#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
|
|
#include <Jolt/Core/Profiler.h>
|
|
#include <Jolt/Core/StringTools.h>
|
|
#include <Jolt/Core/StreamIn.h>
|
|
#include <Jolt/Core/StreamOut.h>
|
|
#include <Jolt/Core/TempAllocator.h>
|
|
#include <Jolt/Core/ScopeExit.h>
|
|
#include <Jolt/Geometry/AABox4.h>
|
|
#include <Jolt/Geometry/RayTriangle.h>
|
|
#include <Jolt/Geometry/RayAABox.h>
|
|
#include <Jolt/Geometry/OrientedBox.h>
|
|
#include <Jolt/ObjectStream/TypeDeclarations.h>
|
|
|
|
//#define JPH_DEBUG_HEIGHT_FIELD
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
bool HeightFieldShape::sDrawTriangleOutlines = false;
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
using namespace HeightFieldShapeConstants;
|
|
|
|
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings)
|
|
{
|
|
JPH_ADD_BASE_CLASS(HeightFieldShapeSettings, ShapeSettings)
|
|
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mHeightSamples)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mOffset)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mScale)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMinHeightValue)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaxHeightValue)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialsCapacity)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialIndices)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials)
|
|
JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mActiveEdgeCosThresholdAngle)
|
|
}
|
|
|
|
const uint HeightFieldShape::sGridOffsets[] =
|
|
{
|
|
0, // level: 0, max x/y: 0, offset: 0
|
|
1, // level: 1, max x/y: 1, offset: 1
|
|
5, // level: 2, max x/y: 3, offset: 1 + 4
|
|
21, // level: 3, max x/y: 7, offset: 1 + 4 + 16
|
|
85, // level: 4, max x/y: 15, offset: 1 + 4 + 16 + 64
|
|
341, // level: 5, max x/y: 31, offset: 1 + 4 + 16 + 64 + 256
|
|
1365, // level: 6, max x/y: 63, offset: 1 + 4 + 16 + 64 + 256 + 1024
|
|
5461, // level: 7, max x/y: 127, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096
|
|
21845, // level: 8, max x/y: 255, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
87381, // level: 9, max x/y: 511, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
349525, // level: 10, max x/y: 1023, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
1398101, // level: 11, max x/y: 2047, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
5592405, // level: 12, max x/y: 4095, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
22369621, // level: 13, max x/y: 8191, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
89478485, // level: 14, max x/y: 16383, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
|
|
};
|
|
|
|
HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices, const PhysicsMaterialList &inMaterialList) :
|
|
mOffset(inOffset),
|
|
mScale(inScale),
|
|
mSampleCount(inSampleCount)
|
|
{
|
|
mHeightSamples.assign(inSamples, inSamples + Square(inSampleCount));
|
|
|
|
if (!inMaterialList.empty() && inMaterialIndices != nullptr)
|
|
{
|
|
mMaterialIndices.assign(inMaterialIndices, inMaterialIndices + Square(inSampleCount - 1));
|
|
mMaterials = inMaterialList;
|
|
}
|
|
else
|
|
{
|
|
JPH_ASSERT(inMaterialList.empty());
|
|
JPH_ASSERT(inMaterialIndices == nullptr);
|
|
}
|
|
}
|
|
|
|
ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const
|
|
{
|
|
if (mCachedResult.IsEmpty())
|
|
Ref<Shape> shape = new HeightFieldShape(*this, mCachedResult);
|
|
return mCachedResult;
|
|
}
|
|
|
|
void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const
|
|
{
|
|
// Determine min and max value
|
|
outMinValue = mMinHeightValue;
|
|
outMaxValue = mMaxHeightValue;
|
|
for (float h : mHeightSamples)
|
|
if (h != cNoCollisionValue)
|
|
{
|
|
outMinValue = min(outMinValue, h);
|
|
outMaxValue = max(outMaxValue, h);
|
|
}
|
|
|
|
// Prevent dividing by zero by setting a minimal height difference
|
|
float height_diff = max(outMaxValue - outMinValue, 1.0e-6f);
|
|
|
|
// Calculate the scale factor to quantize to 16 bits
|
|
outQuantizationScale = float(cMaxHeightValue16) / height_diff;
|
|
}
|
|
|
|
uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError) const
|
|
{
|
|
// Start with 1 bit per sample
|
|
uint32 bits_per_sample = 1;
|
|
|
|
// Determine total range
|
|
float min_value, max_value, scale;
|
|
DetermineMinAndMaxSample(min_value, max_value, scale);
|
|
if (min_value < max_value)
|
|
{
|
|
// Loop over all blocks
|
|
for (uint y = 0; y < mSampleCount; y += mBlockSize)
|
|
for (uint x = 0; x < mSampleCount; x += mBlockSize)
|
|
{
|
|
// Determine min and max block value + take 1 sample border just like we do while building the hierarchical grids
|
|
float block_min_value = FLT_MAX, block_max_value = -FLT_MAX;
|
|
for (uint bx = x; bx < min(x + mBlockSize + 1, mSampleCount); ++bx)
|
|
for (uint by = y; by < min(y + mBlockSize + 1, mSampleCount); ++by)
|
|
{
|
|
float h = mHeightSamples[by * mSampleCount + bx];
|
|
if (h != cNoCollisionValue)
|
|
{
|
|
block_min_value = min(block_min_value, h);
|
|
block_max_value = max(block_max_value, h);
|
|
}
|
|
}
|
|
|
|
if (block_min_value < block_max_value)
|
|
{
|
|
// Quantize then dequantize block min/max value
|
|
block_min_value = min_value + floor((block_min_value - min_value) * scale) / scale;
|
|
block_max_value = min_value + ceil((block_max_value - min_value) * scale) / scale;
|
|
float block_height = block_max_value - block_min_value;
|
|
|
|
// Loop over the block again
|
|
for (uint bx = x; bx < x + mBlockSize; ++bx)
|
|
for (uint by = y; by < y + mBlockSize; ++by)
|
|
{
|
|
// Get the height
|
|
float height = mHeightSamples[by * mSampleCount + bx];
|
|
if (height != cNoCollisionValue)
|
|
{
|
|
for (;;)
|
|
{
|
|
// Determine bitmask for sample
|
|
uint32 sample_mask = (1 << bits_per_sample) - 1;
|
|
|
|
// Quantize
|
|
float quantized_height = floor((height - block_min_value) * float(sample_mask) / block_height);
|
|
quantized_height = Clamp(quantized_height, 0.0f, float(sample_mask - 1));
|
|
|
|
// Dequantize and check error
|
|
float dequantized_height = block_min_value + (quantized_height + 0.5f) * block_height / float(sample_mask);
|
|
if (abs(dequantized_height - height) <= inMaxError)
|
|
break;
|
|
|
|
// Not accurate enough, increase bits per sample
|
|
bits_per_sample++;
|
|
|
|
// Don't go above 8 bits per sample
|
|
if (bits_per_sample == 8)
|
|
return bits_per_sample;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
return bits_per_sample;
|
|
}
|
|
|
|
void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator)
|
|
{
|
|
// Allocate temporary buffer for normals
|
|
uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3);
|
|
Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size);
|
|
JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); });
|
|
|
|
// Calculate triangle normals and make normals zero for triangles that are missing
|
|
Vec3 *out_normal = normals;
|
|
for (uint y = 0; y < inSizeY; ++y)
|
|
for (uint x = 0; x < inSizeX; ++x)
|
|
{
|
|
// Get height on diagonal
|
|
const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
|
|
float x1y1_h = height_samples[0];
|
|
float x2y2_h = height_samples[inHeightsStride + 1];
|
|
if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue)
|
|
{
|
|
// Calculate normal for lower left triangle (e.g. T1A)
|
|
float x1y2_h = height_samples[inHeightsStride];
|
|
if (x1y2_h != cNoCollisionValue)
|
|
{
|
|
Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
|
|
Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ());
|
|
out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized();
|
|
}
|
|
else
|
|
out_normal[0] = Vec3::sZero();
|
|
|
|
// Calculate normal for upper right triangle (e.g. T1B)
|
|
float x2y1_h = height_samples[1];
|
|
if (x2y1_h != cNoCollisionValue)
|
|
{
|
|
Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0);
|
|
Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ());
|
|
out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized();
|
|
}
|
|
else
|
|
out_normal[1] = Vec3::sZero();
|
|
}
|
|
else
|
|
{
|
|
out_normal[0] = Vec3::sZero();
|
|
out_normal[1] = Vec3::sZero();
|
|
}
|
|
|
|
out_normal += 2;
|
|
}
|
|
|
|
// Calculate active edges
|
|
const Vec3 *in_normal = normals;
|
|
uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX);
|
|
for (uint y = 0; y < inSizeY; ++y)
|
|
{
|
|
for (uint x = 0; x < inSizeX; ++x)
|
|
{
|
|
// Get vertex heights
|
|
const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
|
|
float x1y1_h = height_samples[0];
|
|
float x1y2_h = height_samples[inHeightsStride];
|
|
float x2y2_h = height_samples[inHeightsStride + 1];
|
|
bool x1y1_valid = x1y1_h != cNoCollisionValue;
|
|
bool x1y2_valid = x1y2_h != cNoCollisionValue;
|
|
bool x2y2_valid = x2y2_h != cNoCollisionValue;
|
|
|
|
// Calculate the edge flags (3 bits)
|
|
// See diagram in the next function for the edge numbering
|
|
uint16 edge_mask = 0b111;
|
|
uint16 edge_flags = 0;
|
|
|
|
// Edge 0
|
|
if (x == 0)
|
|
edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge
|
|
else if (x1y1_valid && x1y2_valid)
|
|
{
|
|
Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ());
|
|
if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle))
|
|
edge_flags |= 0b001;
|
|
}
|
|
|
|
// Edge 1
|
|
if (y == inSizeY - 1)
|
|
edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge
|
|
else if (x1y2_valid && x2y2_valid)
|
|
{
|
|
Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
|
|
if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle))
|
|
edge_flags |= 0b010;
|
|
}
|
|
|
|
// Edge 2
|
|
if (x1y1_valid && x2y2_valid)
|
|
{
|
|
Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ());
|
|
if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle))
|
|
edge_flags |= 0b100;
|
|
}
|
|
|
|
// Store the edge flags in the array
|
|
uint byte_pos = global_bit_pos >> 3;
|
|
uint bit_pos = global_bit_pos & 0b111;
|
|
JPH_ASSERT(byte_pos < mActiveEdgesSize);
|
|
uint8 *edge_flags_ptr = &mActiveEdges[byte_pos];
|
|
uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8);
|
|
combined_edge_flags &= ~(edge_mask << bit_pos);
|
|
combined_edge_flags |= edge_flags << bit_pos;
|
|
edge_flags_ptr[0] = uint8(combined_edge_flags);
|
|
edge_flags_ptr[1] = uint8(combined_edge_flags >> 8);
|
|
|
|
in_normal += 2;
|
|
global_bit_pos += 3;
|
|
}
|
|
|
|
global_bit_pos += 3 * (mSampleCount - 1 - inSizeX);
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings)
|
|
{
|
|
/*
|
|
Store active edges. The triangles are organized like this:
|
|
x --->
|
|
|
|
y + +
|
|
| \ T1B | \ T2B
|
|
| e0 e2 | \
|
|
| | T1A \ | T2A \
|
|
V +--e1---+-------+
|
|
| \ T3B | \ T4B
|
|
| \ | \
|
|
| T3A \ | T4A \
|
|
+-------+-------+
|
|
We store active edges e0 .. e2 as bits 0 .. 2.
|
|
We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A).
|
|
The top edge and right edge of the heightfield are always active so we do not need to store them,
|
|
therefore we only need to store (mSampleCount - 1)^2 * 3-bit
|
|
The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles.
|
|
Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary
|
|
*/
|
|
|
|
// Make all edges active (if mSampleCount is bigger than inSettings.mSampleCount we need to fill up the padding,
|
|
// also edges at x = 0 and y = inSettings.mSampleCount - 1 are not updated)
|
|
memset(mActiveEdges, 0xff, mActiveEdgesSize);
|
|
|
|
// Now clear the edges that are not active
|
|
TempAllocatorMalloc allocator;
|
|
CalculateActiveEdges(0, 0, inSettings.mSampleCount - 1, inSettings.mSampleCount - 1, inSettings.mHeightSamples.data(), 0, 0, inSettings.mSampleCount, inSettings.mScale.GetY(), inSettings.mActiveEdgeCosThresholdAngle, allocator);
|
|
}
|
|
|
|
void HeightFieldShape::StoreMaterialIndices(const HeightFieldShapeSettings &inSettings)
|
|
{
|
|
// We need to account for any rounding of the sample count to the nearest block size
|
|
uint in_count_min_1 = inSettings.mSampleCount - 1;
|
|
uint out_count_min_1 = mSampleCount - 1;
|
|
|
|
mNumBitsPerMaterialIndex = 32 - CountLeadingZeros(max((uint32)mMaterials.size(), inSettings.mMaterialsCapacity) - 1);
|
|
mMaterialIndices.resize(((Square(out_count_min_1) * mNumBitsPerMaterialIndex + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16
|
|
|
|
if (mMaterials.size() > 1)
|
|
for (uint y = 0; y < out_count_min_1; ++y)
|
|
for (uint x = 0; x < out_count_min_1; ++x)
|
|
{
|
|
// Read material
|
|
uint16 material_index = x < in_count_min_1 && y < in_count_min_1? uint16(inSettings.mMaterialIndices[x + y * in_count_min_1]) : 0;
|
|
|
|
// Calculate byte and bit position where the material index needs to go
|
|
uint sample_pos = x + y * out_count_min_1;
|
|
uint bit_pos = sample_pos * mNumBitsPerMaterialIndex;
|
|
uint byte_pos = bit_pos >> 3;
|
|
bit_pos &= 0b111;
|
|
|
|
// Write the material index
|
|
material_index <<= bit_pos;
|
|
JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size());
|
|
mMaterialIndices[byte_pos] |= uint8(material_index);
|
|
mMaterialIndices[byte_pos + 1] |= uint8(material_index >> 8);
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::CacheValues()
|
|
{
|
|
mSampleMask = uint8((uint32(1) << mBitsPerSample) - 1);
|
|
}
|
|
|
|
void HeightFieldShape::AllocateBuffers()
|
|
{
|
|
uint num_blocks = GetNumBlocks();
|
|
uint max_stride = (num_blocks + 1) >> 1;
|
|
mRangeBlocksSize = sGridOffsets[sGetMaxLevel(num_blocks) - 1] + Square(max_stride);
|
|
mHeightSamplesSize = (mSampleCount * mSampleCount * mBitsPerSample + 7) / 8 + 1;
|
|
mActiveEdgesSize = (Square(mSampleCount - 1) * 3 + 7) / 8 + 1; // See explanation at HeightFieldShape::CalculateActiveEdges
|
|
|
|
JPH_ASSERT(mRangeBlocks == nullptr && mHeightSamples == nullptr && mActiveEdges == nullptr);
|
|
void *data = AlignedAllocate(mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize, alignof(RangeBlock));
|
|
mRangeBlocks = reinterpret_cast<RangeBlock *>(data);
|
|
mHeightSamples = reinterpret_cast<uint8 *>(mRangeBlocks + mRangeBlocksSize);
|
|
mActiveEdges = mHeightSamples + mHeightSamplesSize;
|
|
}
|
|
|
|
HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult) :
|
|
Shape(EShapeType::HeightField, EShapeSubType::HeightField, inSettings, outResult),
|
|
mOffset(inSettings.mOffset),
|
|
mScale(inSettings.mScale),
|
|
mSampleCount(((inSettings.mSampleCount + inSettings.mBlockSize - 1) / inSettings.mBlockSize) * inSettings.mBlockSize), // Round sample count to nearest block size
|
|
mBlockSize(inSettings.mBlockSize),
|
|
mBitsPerSample(uint8(inSettings.mBitsPerSample))
|
|
{
|
|
CacheValues();
|
|
|
|
// Reserve a bigger materials list if requested
|
|
if (inSettings.mMaterialsCapacity > 0)
|
|
mMaterials.reserve(inSettings.mMaterialsCapacity);
|
|
mMaterials = inSettings.mMaterials;
|
|
|
|
// Check block size
|
|
if (mBlockSize < 2 || mBlockSize > 8)
|
|
{
|
|
outResult.SetError("HeightFieldShape: Block size must be in the range [2, 8]!");
|
|
return;
|
|
}
|
|
|
|
// Check bits per sample
|
|
if (inSettings.mBitsPerSample < 1 || inSettings.mBitsPerSample > 8)
|
|
{
|
|
outResult.SetError("HeightFieldShape: Bits per sample must be in the range [1, 8]!");
|
|
return;
|
|
}
|
|
|
|
// We stop at mBlockSize x mBlockSize height sample blocks
|
|
uint num_blocks = GetNumBlocks();
|
|
|
|
// We want at least 1 grid layer
|
|
if (num_blocks < 2)
|
|
{
|
|
outResult.SetError("HeightFieldShape: Sample count too low!");
|
|
return;
|
|
}
|
|
|
|
// Check that we don't overflow our 32 bit 'properties'
|
|
if (num_blocks > (1 << cNumBitsXY))
|
|
{
|
|
outResult.SetError("HeightFieldShape: Sample count too high!");
|
|
return;
|
|
}
|
|
|
|
// Check if we're not exceeding the amount of sub shape id bits
|
|
if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
|
|
{
|
|
outResult.SetError("HeightFieldShape: Size exceeds the amount of available sub shape ID bits!");
|
|
return;
|
|
}
|
|
|
|
if (!mMaterials.empty())
|
|
{
|
|
// Validate materials
|
|
if (mMaterials.size() > 256)
|
|
{
|
|
outResult.SetError("Supporting max 256 materials per height field");
|
|
return;
|
|
}
|
|
for (uint8 s : inSettings.mMaterialIndices)
|
|
if (s >= mMaterials.size())
|
|
{
|
|
outResult.SetError(StringFormat("Material %u is beyond material list (size: %u)", s, (uint)mMaterials.size()));
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// No materials assigned, validate that no materials have been specified
|
|
if (!inSettings.mMaterialIndices.empty())
|
|
{
|
|
outResult.SetError("No materials present, mMaterialIndices should be empty");
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Determine range
|
|
float min_value, max_value, scale;
|
|
inSettings.DetermineMinAndMaxSample(min_value, max_value, scale);
|
|
if (min_value > max_value)
|
|
{
|
|
// If there is no collision with this heightmap, leave everything empty
|
|
mMaterials.clear();
|
|
outResult.Set(this);
|
|
return;
|
|
}
|
|
|
|
// Allocate space for this shape
|
|
AllocateBuffers();
|
|
|
|
// Quantize to uint16
|
|
Array<uint16> quantized_samples;
|
|
quantized_samples.reserve(mSampleCount * mSampleCount);
|
|
for (uint y = 0; y < inSettings.mSampleCount; ++y)
|
|
{
|
|
for (uint x = 0; x < inSettings.mSampleCount; ++x)
|
|
{
|
|
float h = inSettings.mHeightSamples[x + y * inSettings.mSampleCount];
|
|
if (h == cNoCollisionValue)
|
|
{
|
|
quantized_samples.push_back(cNoCollisionValue16);
|
|
}
|
|
else
|
|
{
|
|
// Floor the quantized height to get a lower bound for the quantized value
|
|
int quantized_height = (int)floor(scale * (h - min_value));
|
|
|
|
// Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value
|
|
quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1));
|
|
|
|
quantized_samples.push_back(uint16(quantized_height));
|
|
}
|
|
}
|
|
// Pad remaining columns with no collision
|
|
for (uint x = inSettings.mSampleCount; x < mSampleCount; ++x)
|
|
quantized_samples.push_back(cNoCollisionValue16);
|
|
}
|
|
// Pad remaining rows with no collision
|
|
for (uint y = inSettings.mSampleCount; y < mSampleCount; ++y)
|
|
for (uint x = 0; x < mSampleCount; ++x)
|
|
quantized_samples.push_back(cNoCollisionValue16);
|
|
|
|
// Update offset and scale to account for the compression to uint16
|
|
if (min_value <= max_value) // Only when there was collision
|
|
{
|
|
// In GetPosition we always add 0.5 to the quantized sample in order to reduce the average error.
|
|
// We want to be able to exactly quantize min_value (this is important in case the heightfield is entirely flat) so we subtract that value from min_value.
|
|
min_value -= 0.5f / (scale * mSampleMask);
|
|
|
|
mOffset.SetY(mOffset.GetY() + mScale.GetY() * min_value);
|
|
}
|
|
mScale.SetY(mScale.GetY() / scale);
|
|
|
|
// Calculate amount of grids
|
|
uint max_level = sGetMaxLevel(num_blocks);
|
|
|
|
// Temporary data structure used during creating of a hierarchy of grids
|
|
struct Range
|
|
{
|
|
uint16 mMin;
|
|
uint16 mMax;
|
|
};
|
|
|
|
// Reserve size for temporary range data + reserve 1 extra for a 1x1 grid that we won't store but use for calculating the bounding box
|
|
Array<Array<Range>> ranges;
|
|
ranges.resize(max_level + 1);
|
|
|
|
// Calculate highest detail grid by combining mBlockSize x mBlockSize height samples
|
|
Array<Range> *cur_range_vector = &ranges.back();
|
|
uint num_blocks_pow2 = GetNextPowerOf2(num_blocks); // We calculate the range blocks as if the heightfield was a power of 2, when we save the range blocks we'll ignore the extra samples (this makes downsampling easier)
|
|
cur_range_vector->resize(num_blocks_pow2 * num_blocks_pow2);
|
|
Range *range_dst = &cur_range_vector->front();
|
|
for (uint y = 0; y < num_blocks_pow2; ++y)
|
|
for (uint x = 0; x < num_blocks_pow2; ++x)
|
|
{
|
|
range_dst->mMin = 0xffff;
|
|
range_dst->mMax = 0;
|
|
uint max_bx = x == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; // for interior blocks take 1 more because the triangles connect to the next block so we must include their height too
|
|
uint max_by = y == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1;
|
|
for (uint by = 0; by < max_by; ++by)
|
|
for (uint bx = 0; bx < max_bx; ++bx)
|
|
{
|
|
uint sx = x * mBlockSize + bx;
|
|
uint sy = y * mBlockSize + by;
|
|
if (sx < mSampleCount && sy < mSampleCount)
|
|
{
|
|
uint16 h = quantized_samples[sy * mSampleCount + sx];
|
|
if (h != cNoCollisionValue16)
|
|
{
|
|
range_dst->mMin = min(range_dst->mMin, h);
|
|
range_dst->mMax = max(range_dst->mMax, uint16(h + 1)); // Add 1 to the max so we know the real value is between mMin and mMax
|
|
}
|
|
}
|
|
}
|
|
++range_dst;
|
|
}
|
|
|
|
// Calculate remaining grids
|
|
for (uint n = num_blocks_pow2 >> 1; n >= 1; n >>= 1)
|
|
{
|
|
// Get source buffer
|
|
const Range *range_src = &cur_range_vector->front();
|
|
|
|
// Previous array element
|
|
--cur_range_vector;
|
|
|
|
// Make space for this grid
|
|
cur_range_vector->resize(n * n);
|
|
|
|
// Get target buffer
|
|
range_dst = &cur_range_vector->front();
|
|
|
|
// Combine the results of 2x2 ranges
|
|
for (uint y = 0; y < n; ++y)
|
|
for (uint x = 0; x < n; ++x)
|
|
{
|
|
range_dst->mMin = 0xffff;
|
|
range_dst->mMax = 0;
|
|
for (uint by = 0; by < 2; ++by)
|
|
for (uint bx = 0; bx < 2; ++bx)
|
|
{
|
|
const Range &r = range_src[(y * 2 + by) * n * 2 + x * 2 + bx];
|
|
range_dst->mMin = min(range_dst->mMin, r.mMin);
|
|
range_dst->mMax = max(range_dst->mMax, r.mMax);
|
|
}
|
|
++range_dst;
|
|
}
|
|
}
|
|
JPH_ASSERT(cur_range_vector == &ranges.front());
|
|
|
|
// Store global range for bounding box calculation
|
|
mMinSample = ranges[0][0].mMin;
|
|
mMaxSample = ranges[0][0].mMax;
|
|
|
|
#ifdef JPH_ENABLE_ASSERTS
|
|
// Validate that we did not lose range along the way
|
|
uint16 minv = 0xffff, maxv = 0;
|
|
for (uint16 v : quantized_samples)
|
|
if (v != cNoCollisionValue16)
|
|
{
|
|
minv = min(minv, v);
|
|
maxv = max(maxv, uint16(v + 1));
|
|
}
|
|
JPH_ASSERT(mMinSample == minv && mMaxSample == maxv);
|
|
#endif
|
|
|
|
// Now erase the first element, we need a 2x2 grid to start with
|
|
ranges.erase(ranges.begin());
|
|
|
|
// Create blocks
|
|
uint max_stride = (num_blocks + 1) >> 1;
|
|
RangeBlock *current_block = mRangeBlocks;
|
|
for (uint level = 0; level < ranges.size(); ++level)
|
|
{
|
|
JPH_ASSERT(uint(current_block - mRangeBlocks) == sGridOffsets[level]);
|
|
|
|
uint in_n = 1 << level;
|
|
uint out_n = min(in_n, max_stride); // At the most detailed level we store a non-power of 2 number of blocks
|
|
|
|
for (uint y = 0; y < out_n; ++y)
|
|
for (uint x = 0; x < out_n; ++x)
|
|
{
|
|
// Convert from 2x2 Range structure to 1 RangeBlock structure
|
|
RangeBlock &rb = *current_block++;
|
|
for (uint by = 0; by < 2; ++by)
|
|
for (uint bx = 0; bx < 2; ++bx)
|
|
{
|
|
uint src_pos = (y * 2 + by) * 2 * in_n + (x * 2 + bx);
|
|
uint dst_pos = by * 2 + bx;
|
|
rb.mMin[dst_pos] = ranges[level][src_pos].mMin;
|
|
rb.mMax[dst_pos] = ranges[level][src_pos].mMax;
|
|
}
|
|
}
|
|
}
|
|
JPH_ASSERT(uint32(current_block - mRangeBlocks) == mRangeBlocksSize);
|
|
|
|
// Quantize height samples
|
|
memset(mHeightSamples, 0, mHeightSamplesSize);
|
|
int sample = 0;
|
|
for (uint y = 0; y < mSampleCount; ++y)
|
|
for (uint x = 0; x < mSampleCount; ++x)
|
|
{
|
|
uint32 output_value;
|
|
|
|
float h = x < inSettings.mSampleCount && y < inSettings.mSampleCount? inSettings.mHeightSamples[x + y * inSettings.mSampleCount] : cNoCollisionValue;
|
|
if (h == cNoCollisionValue)
|
|
{
|
|
// No collision
|
|
output_value = mSampleMask;
|
|
}
|
|
else
|
|
{
|
|
// Get range of block so we know what range to compress to
|
|
uint bx = x / mBlockSize;
|
|
uint by = y / mBlockSize;
|
|
const Range &range = ranges.back()[by * num_blocks_pow2 + bx];
|
|
JPH_ASSERT(range.mMin < range.mMax);
|
|
|
|
// Quantize to mBitsPerSample bits, note that mSampleMask is reserved for indicating that there's no collision.
|
|
// We divide the range into mSampleMask segments and use the mid points of these segments as the quantized values.
|
|
// This results in a lower error than if we had quantized our data using the lowest point of all these segments.
|
|
float h_min = min_value + range.mMin / scale;
|
|
float h_delta = float(range.mMax - range.mMin) / scale;
|
|
float quantized_height = floor((h - h_min) * float(mSampleMask) / h_delta);
|
|
output_value = uint32(Clamp((int)quantized_height, 0, int(mSampleMask) - 1)); // mSampleMask is reserved as 'no collision value'
|
|
}
|
|
|
|
// Store the sample
|
|
uint byte_pos = sample >> 3;
|
|
uint bit_pos = sample & 0b111;
|
|
output_value <<= bit_pos;
|
|
JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
|
|
mHeightSamples[byte_pos] |= uint8(output_value);
|
|
mHeightSamples[byte_pos + 1] |= uint8(output_value >> 8);
|
|
sample += inSettings.mBitsPerSample;
|
|
}
|
|
|
|
// Calculate the active edges
|
|
CalculateActiveEdges(inSettings);
|
|
|
|
// Compress material indices
|
|
if (mMaterials.size() > 1 || inSettings.mMaterialsCapacity > 1)
|
|
StoreMaterialIndices(inSettings);
|
|
|
|
outResult.Set(this);
|
|
}
|
|
|
|
HeightFieldShape::~HeightFieldShape()
|
|
{
|
|
if (mRangeBlocks != nullptr)
|
|
AlignedFree(mRangeBlocks);
|
|
}
|
|
|
|
Ref<HeightFieldShape> HeightFieldShape::Clone() const
|
|
{
|
|
Ref<HeightFieldShape> clone = new HeightFieldShape;
|
|
clone->SetUserData(GetUserData());
|
|
|
|
clone->mOffset = mOffset;
|
|
clone->mScale = mScale;
|
|
clone->mSampleCount = mSampleCount;
|
|
clone->mBlockSize = mBlockSize;
|
|
clone->mBitsPerSample = mBitsPerSample;
|
|
clone->mSampleMask = mSampleMask;
|
|
clone->mMinSample = mMinSample;
|
|
clone->mMaxSample = mMaxSample;
|
|
|
|
clone->AllocateBuffers();
|
|
memcpy(clone->mRangeBlocks, mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); // Copy the entire buffer in 1 go
|
|
|
|
clone->mMaterials.reserve(mMaterials.capacity()); // Ensure we keep the capacity of the original
|
|
clone->mMaterials = mMaterials;
|
|
clone->mMaterialIndices = mMaterialIndices;
|
|
clone->mNumBitsPerMaterialIndex = mNumBitsPerMaterialIndex;
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
clone->mGeometry = mGeometry;
|
|
clone->mCachedUseMaterialColors = mCachedUseMaterialColors;
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
return clone;
|
|
}
|
|
|
|
inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride)
|
|
{
|
|
outRangeBlockOffset = sGridOffsets[inMaxLevel - 1];
|
|
outRangeBlockStride = (inNumBlocks + 1) >> 1;
|
|
}
|
|
|
|
inline void HeightFieldShape::GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock)
|
|
{
|
|
JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
|
|
|
|
// Convert to location of range block
|
|
uint rbx = inBlockX >> 1;
|
|
uint rby = inBlockY >> 1;
|
|
outIndexInBlock = ((inBlockY & 1) << 1) + (inBlockX & 1);
|
|
|
|
uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx;
|
|
JPH_ASSERT(offset < mRangeBlocksSize);
|
|
outBlock = mRangeBlocks + offset;
|
|
}
|
|
|
|
inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const
|
|
{
|
|
JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
|
|
|
|
// Convert to location of range block
|
|
uint rbx = inBlockX >> 1;
|
|
uint rby = inBlockY >> 1;
|
|
uint n = ((inBlockY & 1) << 1) + (inBlockX & 1);
|
|
|
|
// Calculate offset and scale
|
|
uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx;
|
|
JPH_ASSERT(offset < mRangeBlocksSize);
|
|
const RangeBlock &block = mRangeBlocks[offset];
|
|
outBlockOffset = float(block.mMin[n]);
|
|
outBlockScale = float(block.mMax[n] - block.mMin[n]) / float(mSampleMask);
|
|
}
|
|
|
|
inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const
|
|
{
|
|
JPH_ASSERT(inX < mSampleCount);
|
|
JPH_ASSERT(inY < mSampleCount);
|
|
|
|
// Determine bit position of sample
|
|
uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample);
|
|
uint byte_pos = sample >> 3;
|
|
uint bit_pos = sample & 0b111;
|
|
|
|
// Fetch the height sample value
|
|
JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
|
|
const uint8 *height_samples = mHeightSamples + byte_pos;
|
|
uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
|
|
return uint8(height_sample >> bit_pos) & mSampleMask;
|
|
}
|
|
|
|
inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const
|
|
{
|
|
// Get quantized value
|
|
uint8 height_sample = GetHeightSample(inX, inY);
|
|
outNoCollision = height_sample == mSampleMask;
|
|
|
|
// Add 0.5 to the quantized value to minimize the error (see constructor)
|
|
return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY));
|
|
}
|
|
|
|
Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const
|
|
{
|
|
// Test if there are any samples
|
|
if (mHeightSamplesSize == 0)
|
|
return mOffset + mScale * Vec3(float(inX), 0.0f, float(inY));
|
|
|
|
// Get block location
|
|
uint bx = inX / mBlockSize;
|
|
uint by = inY / mBlockSize;
|
|
|
|
// Calculate offset and stride
|
|
uint num_blocks = GetNumBlocks();
|
|
uint range_block_offset, range_block_stride;
|
|
sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride);
|
|
|
|
float offset, scale;
|
|
GetBlockOffsetAndScale(bx, by, range_block_offset, range_block_stride, offset, scale);
|
|
|
|
bool no_collision;
|
|
return GetPosition(inX, inY, offset, scale, no_collision);
|
|
}
|
|
|
|
bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const
|
|
{
|
|
return mHeightSamplesSize == 0 || GetHeightSample(inX, inY) == mSampleMask;
|
|
}
|
|
|
|
bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const
|
|
{
|
|
// Check if we have collision
|
|
if (mHeightSamplesSize == 0)
|
|
return false;
|
|
|
|
// Convert coordinate to integer space
|
|
Vec3 integer_space = (inLocalPosition - mOffset) / mScale;
|
|
|
|
// Get x coordinate and fraction
|
|
float x_frac = integer_space.GetX();
|
|
if (x_frac < 0.0f || x_frac >= mSampleCount - 1)
|
|
return false;
|
|
uint x = (uint)floor(x_frac);
|
|
x_frac -= x;
|
|
|
|
// Get y coordinate and fraction
|
|
float y_frac = integer_space.GetZ();
|
|
if (y_frac < 0.0f || y_frac >= mSampleCount - 1)
|
|
return false;
|
|
uint y = (uint)floor(y_frac);
|
|
y_frac -= y;
|
|
|
|
// If one of the diagonal points doesn't have collision, we don't have a height at this location
|
|
if (IsNoCollision(x, y) || IsNoCollision(x + 1, y + 1))
|
|
return false;
|
|
|
|
if (y_frac >= x_frac)
|
|
{
|
|
// Left bottom triangle, test the 3rd point
|
|
if (IsNoCollision(x, y + 1))
|
|
return false;
|
|
|
|
// Interpolate height value
|
|
Vec3 v1 = GetPosition(x, y);
|
|
Vec3 v2 = GetPosition(x, y + 1);
|
|
Vec3 v3 = GetPosition(x + 1, y + 1);
|
|
outSurfacePosition = v1 + y_frac * (v2 - v1) + x_frac * (v3 - v2);
|
|
SubShapeIDCreator creator;
|
|
outSubShapeID = EncodeSubShapeID(creator, x, y, 0);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// Right top triangle, test the third point
|
|
if (IsNoCollision(x + 1, y))
|
|
return false;
|
|
|
|
// Interpolate height value
|
|
Vec3 v1 = GetPosition(x, y);
|
|
Vec3 v2 = GetPosition(x + 1, y + 1);
|
|
Vec3 v3 = GetPosition(x + 1, y);
|
|
outSurfacePosition = v1 + y_frac * (v2 - v3) + x_frac * (v3 - v1);
|
|
SubShapeIDCreator creator;
|
|
outSubShapeID = EncodeSubShapeID(creator, x, y, 1);
|
|
return true;
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const
|
|
{
|
|
if (inSizeX == 0 || inSizeY == 0)
|
|
return;
|
|
|
|
JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0);
|
|
JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
|
|
JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount);
|
|
|
|
// Test if there are any samples
|
|
if (mHeightSamplesSize == 0)
|
|
{
|
|
// No samples, return the offset
|
|
float offset = mOffset.GetY();
|
|
for (uint y = 0; y < inSizeY; ++y, outHeights += inHeightsStride)
|
|
for (uint x = 0; x < inSizeX; ++x)
|
|
outHeights[x] = offset;
|
|
}
|
|
else
|
|
{
|
|
// Calculate offset and stride
|
|
uint num_blocks = GetNumBlocks();
|
|
uint range_block_offset, range_block_stride;
|
|
sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride);
|
|
|
|
// Loop over blocks
|
|
uint block_start_x = inX / mBlockSize;
|
|
uint block_start_y = inY / mBlockSize;
|
|
uint num_blocks_x = inSizeX / mBlockSize;
|
|
uint num_blocks_y = inSizeY / mBlockSize;
|
|
for (uint block_y = 0; block_y < num_blocks_y; ++block_y)
|
|
for (uint block_x = 0; block_x < num_blocks_x; ++block_x)
|
|
{
|
|
// Get offset and scale for block
|
|
float offset, scale;
|
|
GetBlockOffsetAndScale(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, offset, scale);
|
|
|
|
// Adjust by global offset and scale
|
|
// Note: This is the math applied in GetPosition() written out to reduce calculations in the inner loop
|
|
scale *= mScale.GetY();
|
|
offset = mOffset.GetY() + mScale.GetY() * offset + 0.5f * scale;
|
|
|
|
// Loop over samples in block
|
|
for (uint sample_y = 0; sample_y < mBlockSize; ++sample_y)
|
|
for (uint sample_x = 0; sample_x < mBlockSize; ++sample_x)
|
|
{
|
|
// Calculate output coordinate
|
|
uint output_x = block_x * mBlockSize + sample_x;
|
|
uint output_y = block_y * mBlockSize + sample_y;
|
|
|
|
// Get quantized value
|
|
uint8 height_sample = GetHeightSample(inX + output_x, inY + output_y);
|
|
|
|
// Dequantize
|
|
float h = height_sample != mSampleMask? offset + height_sample * scale : cNoCollisionValue;
|
|
outHeights[output_y * inHeightsStride + output_x] = h;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle)
|
|
{
|
|
if (inSizeX == 0 || inSizeY == 0)
|
|
return;
|
|
|
|
JPH_ASSERT(mHeightSamplesSize > 0);
|
|
JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0);
|
|
JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
|
|
JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount);
|
|
|
|
// If we have a block in negative x/y direction, we will affect its range so we need to take it into account
|
|
bool need_temp_heights = false;
|
|
uint affected_x = inX;
|
|
uint affected_y = inY;
|
|
uint affected_size_x = inSizeX;
|
|
uint affected_size_y = inSizeY;
|
|
if (inX > 0) { affected_x -= mBlockSize; affected_size_x += mBlockSize; need_temp_heights = true; }
|
|
if (inY > 0) { affected_y -= mBlockSize; affected_size_y += mBlockSize; need_temp_heights = true; }
|
|
|
|
// If we have a block in positive x/y direction, our ranges are affected by it so we need to take it into account
|
|
uint heights_size_x = affected_size_x;
|
|
uint heights_size_y = affected_size_y;
|
|
if (inX + inSizeX < mSampleCount) { heights_size_x += mBlockSize; need_temp_heights = true; }
|
|
if (inY + inSizeY < mSampleCount) { heights_size_y += mBlockSize; need_temp_heights = true; }
|
|
|
|
// Get heights for affected area
|
|
const float *heights;
|
|
intptr_t heights_stride;
|
|
float *temp_heights;
|
|
if (need_temp_heights)
|
|
{
|
|
// Fetch the surrounding height data (note we're forced to recompress this data with a potentially different range so there will be some precision loss here)
|
|
temp_heights = (float *)inAllocator.Allocate(heights_size_x * heights_size_y * sizeof(float));
|
|
heights = temp_heights;
|
|
heights_stride = heights_size_x;
|
|
|
|
// We need to fill in the following areas:
|
|
//
|
|
// +-----------------+
|
|
// | 2 |
|
|
// |---+---------+---|
|
|
// | | | |
|
|
// | 3 | 1 | 4 |
|
|
// | | | |
|
|
// |---+---------+---|
|
|
// | 5 |
|
|
// +-----------------+
|
|
//
|
|
// 1. The area that is affected by the new heights (we just copy these)
|
|
// 2-5. These areas are either needed to calculate the range of the affected blocks or they need to be recompressed with a different range
|
|
uint offset_x = inX - affected_x;
|
|
uint offset_y = inY - affected_y;
|
|
|
|
// Area 2
|
|
GetHeights(affected_x, affected_y, heights_size_x, offset_y, temp_heights, heights_size_x);
|
|
float *area3_start = temp_heights + offset_y * heights_size_x;
|
|
|
|
// Area 3
|
|
GetHeights(affected_x, inY, offset_x, inSizeY, area3_start, heights_size_x);
|
|
|
|
// Area 1
|
|
float *area1_start = area3_start + offset_x;
|
|
for (uint y = 0; y < inSizeY; ++y, area1_start += heights_size_x, inHeights += inHeightsStride)
|
|
memcpy(area1_start, inHeights, inSizeX * sizeof(float));
|
|
|
|
// Area 4
|
|
uint area4_x = inX + inSizeX;
|
|
GetHeights(area4_x, inY, affected_x + heights_size_x - area4_x, inSizeY, area3_start + area4_x - affected_x, heights_size_x);
|
|
|
|
// Area 5
|
|
uint area5_y = inY + inSizeY;
|
|
float *area5_start = temp_heights + (area5_y - affected_y) * heights_size_x;
|
|
GetHeights(affected_x, area5_y, heights_size_x, affected_y + heights_size_y - area5_y, area5_start, heights_size_x);
|
|
}
|
|
else
|
|
{
|
|
// We can directly use the input buffer because there are no extra edges to take into account
|
|
heights = inHeights;
|
|
heights_stride = inHeightsStride;
|
|
temp_heights = nullptr;
|
|
}
|
|
|
|
// Calculate offset and stride
|
|
uint num_blocks = GetNumBlocks();
|
|
uint range_block_offset, range_block_stride;
|
|
uint max_level = sGetMaxLevel(num_blocks);
|
|
sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride);
|
|
|
|
// Loop over blocks
|
|
uint block_start_x = affected_x / mBlockSize;
|
|
uint block_start_y = affected_y / mBlockSize;
|
|
uint num_blocks_x = affected_size_x / mBlockSize;
|
|
uint num_blocks_y = affected_size_y / mBlockSize;
|
|
for (uint block_y = 0, sample_start_y = 0; block_y < num_blocks_y; ++block_y, sample_start_y += mBlockSize)
|
|
for (uint block_x = 0, sample_start_x = 0; block_x < num_blocks_x; ++block_x, sample_start_x += mBlockSize)
|
|
{
|
|
// Determine quantized min and max value for block
|
|
// Note that we need to include 1 extra row in the positive x/y direction to account for connecting triangles
|
|
int min_value = 0xffff;
|
|
int max_value = 0;
|
|
uint sample_x_end = min(sample_start_x + mBlockSize + 1, mSampleCount - affected_x);
|
|
uint sample_y_end = min(sample_start_y + mBlockSize + 1, mSampleCount - affected_y);
|
|
for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y)
|
|
for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x)
|
|
{
|
|
float h = heights[sample_y * heights_stride + sample_x];
|
|
if (h != cNoCollisionValue)
|
|
{
|
|
int quantized_height = Clamp((int)floor((h - mOffset.GetY()) / mScale.GetY()), 0, int(cMaxHeightValue16 - 1));
|
|
min_value = min(min_value, quantized_height);
|
|
max_value = max(max_value, quantized_height + 1);
|
|
}
|
|
}
|
|
if (min_value > max_value)
|
|
min_value = max_value = cNoCollisionValue16;
|
|
|
|
// Update range for block
|
|
RangeBlock *range_block;
|
|
uint index_in_block;
|
|
GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, range_block, index_in_block);
|
|
range_block->mMin[index_in_block] = uint16(min_value);
|
|
range_block->mMax[index_in_block] = uint16(max_value);
|
|
|
|
// Get offset and scale for block
|
|
float offset_block = float(min_value);
|
|
float scale_block = float(max_value - min_value) / float(mSampleMask);
|
|
|
|
// Calculate scale and offset using the formula used in GetPosition() solved for the quantized height (excluding 0.5 because we round down while quantizing)
|
|
float scale = scale_block * mScale.GetY();
|
|
float offset = mOffset.GetY() + offset_block * mScale.GetY();
|
|
|
|
// Loop over samples in block
|
|
sample_x_end = sample_start_x + mBlockSize;
|
|
sample_y_end = sample_start_y + mBlockSize;
|
|
for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y)
|
|
for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x)
|
|
{
|
|
// Quantize height
|
|
float h = heights[sample_y * heights_stride + sample_x];
|
|
uint8 quantized_height = h != cNoCollisionValue? uint8(Clamp((int)floor((h - offset) / scale), 0, int(mSampleMask) - 1)) : mSampleMask;
|
|
|
|
// Determine bit position of sample
|
|
uint sample = ((affected_y + sample_y) * mSampleCount + affected_x + sample_x) * uint(mBitsPerSample);
|
|
uint byte_pos = sample >> 3;
|
|
uint bit_pos = sample & 0b111;
|
|
|
|
// Update the height value sample
|
|
JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
|
|
uint8 *height_samples = mHeightSamples + byte_pos;
|
|
uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
|
|
height_sample &= ~(uint16(mSampleMask) << bit_pos);
|
|
height_sample |= uint16(quantized_height) << bit_pos;
|
|
height_samples[0] = uint8(height_sample);
|
|
height_samples[1] = uint8(height_sample >> 8);
|
|
}
|
|
}
|
|
|
|
// Update active edges
|
|
// Note that we must take an extra row on all sides to account for connecting triangles
|
|
uint ae_x = inX > 1? inX - 2 : 0;
|
|
uint ae_y = inY > 1? inY - 2 : 0;
|
|
uint ae_sx = min(inX + inSizeX + 1, mSampleCount - 1) - ae_x;
|
|
uint ae_sy = min(inY + inSizeY + 1, mSampleCount - 1) - ae_y;
|
|
CalculateActiveEdges(ae_x, ae_y, ae_sx, ae_sy, heights, affected_x, affected_y, heights_stride, 1.0f, inActiveEdgeCosThresholdAngle, inAllocator);
|
|
|
|
// Free temporary buffer
|
|
if (temp_heights != nullptr)
|
|
inAllocator.Free(temp_heights, heights_size_x * heights_size_y * sizeof(float));
|
|
|
|
// Update hierarchy of range blocks
|
|
while (max_level > 1)
|
|
{
|
|
// Get offset and stride for destination blocks
|
|
uint dst_range_block_offset, dst_range_block_stride;
|
|
sGetRangeBlockOffsetAndStride(num_blocks >> 1, max_level - 1, dst_range_block_offset, dst_range_block_stride);
|
|
|
|
// We'll be processing 2x2 blocks below so we need the start coordinates to be even and we extend the number of blocks to correct for that
|
|
if (block_start_x & 1) { --block_start_x; ++num_blocks_x; }
|
|
if (block_start_y & 1) { --block_start_y; ++num_blocks_y; }
|
|
|
|
// Loop over all affected blocks
|
|
uint block_end_x = block_start_x + num_blocks_x;
|
|
uint block_end_y = block_start_y + num_blocks_y;
|
|
for (uint block_y = block_start_y; block_y < block_end_y; block_y += 2)
|
|
for (uint block_x = block_start_x; block_x < block_end_x; block_x += 2)
|
|
{
|
|
// Get source range block
|
|
RangeBlock *src_range_block;
|
|
uint index_in_src_block;
|
|
GetRangeBlock(block_x, block_y, range_block_offset, range_block_stride, src_range_block, index_in_src_block);
|
|
|
|
// Determine quantized min and max value for the entire 2x2 block
|
|
uint16 min_value = 0xffff;
|
|
uint16 max_value = 0;
|
|
for (uint i = 0; i < 4; ++i)
|
|
if (src_range_block->mMin[i] != cNoCollisionValue16)
|
|
{
|
|
min_value = min(min_value, src_range_block->mMin[i]);
|
|
max_value = max(max_value, src_range_block->mMax[i]);
|
|
}
|
|
|
|
// Write to destination block
|
|
RangeBlock *dst_range_block;
|
|
uint index_in_dst_block;
|
|
GetRangeBlock(block_x >> 1, block_y >> 1, dst_range_block_offset, dst_range_block_stride, dst_range_block, index_in_dst_block);
|
|
dst_range_block->mMin[index_in_dst_block] = uint16(min_value);
|
|
dst_range_block->mMax[index_in_dst_block] = uint16(max_value);
|
|
}
|
|
|
|
// Go up one level
|
|
--max_level;
|
|
num_blocks >>= 1;
|
|
block_start_x >>= 1;
|
|
block_start_y >>= 1;
|
|
num_blocks_x = min((num_blocks_x + 1) >> 1, num_blocks);
|
|
num_blocks_y = min((num_blocks_y + 1) >> 1, num_blocks);
|
|
|
|
// Update stride and offset for source to old destination
|
|
range_block_offset = dst_range_block_offset;
|
|
range_block_stride = dst_range_block_stride;
|
|
}
|
|
|
|
// Calculate new min and max sample for the entire height field
|
|
mMinSample = 0xffff;
|
|
mMaxSample = 0;
|
|
for (uint i = 0; i < 4; ++i)
|
|
if (mRangeBlocks[0].mMin[i] != cNoCollisionValue16)
|
|
{
|
|
mMinSample = min(mMinSample, mRangeBlocks[0].mMin[i]);
|
|
mMaxSample = max(mMaxSample, mRangeBlocks[0].mMax[i]);
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
// Invalidate temporary rendering data
|
|
mGeometry.clear();
|
|
#endif
|
|
}
|
|
|
|
void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const
|
|
{
|
|
if (inSizeX == 0 || inSizeY == 0)
|
|
return;
|
|
|
|
if (mMaterialIndices.empty())
|
|
{
|
|
// Return all 0's
|
|
for (uint y = 0; y < inSizeY; ++y)
|
|
{
|
|
uint8 *out_indices = outMaterials + y * inMaterialsStride;
|
|
for (uint x = 0; x < inSizeX; ++x)
|
|
*out_indices++ = 0;
|
|
}
|
|
return;
|
|
}
|
|
|
|
JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
|
|
JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount);
|
|
|
|
uint count_min_1 = mSampleCount - 1;
|
|
uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
|
|
|
|
for (uint y = 0; y < inSizeY; ++y)
|
|
{
|
|
// Calculate input position
|
|
uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex;
|
|
const uint8 *in_indices = mMaterialIndices.data() + (bit_pos >> 3);
|
|
bit_pos &= 0b111;
|
|
|
|
// Calculate output position
|
|
uint8 *out_indices = outMaterials + y * inMaterialsStride;
|
|
|
|
for (uint x = 0; x < inSizeX; ++x)
|
|
{
|
|
// Get material index
|
|
uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8);
|
|
material_index >>= bit_pos;
|
|
material_index &= material_index_mask;
|
|
*out_indices = uint8(material_index);
|
|
|
|
// Go to the next index
|
|
bit_pos += mNumBitsPerMaterialIndex;
|
|
in_indices += bit_pos >> 3;
|
|
bit_pos &= 0b111;
|
|
++out_indices;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool HeightFieldShape::SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator)
|
|
{
|
|
if (inSizeX == 0 || inSizeY == 0)
|
|
return true;
|
|
|
|
JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
|
|
JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount);
|
|
|
|
// Remap materials
|
|
uint material_remap_table_size = uint(inMaterialList != nullptr? inMaterialList->size() : mMaterials.size());
|
|
uint8 *material_remap_table = (uint8 *)inAllocator.Allocate(material_remap_table_size);
|
|
JPH_SCOPE_EXIT([&inAllocator, material_remap_table, material_remap_table_size]{ inAllocator.Free(material_remap_table, material_remap_table_size); });
|
|
if (inMaterialList != nullptr)
|
|
{
|
|
// Conservatively reserve more space if the incoming material list is bigger
|
|
if (inMaterialList->size() > mMaterials.size())
|
|
mMaterials.reserve(inMaterialList->size());
|
|
|
|
// Create a remap table
|
|
uint8 *remap_entry = material_remap_table;
|
|
for (const PhysicsMaterial *material : *inMaterialList)
|
|
{
|
|
// Try to find it in the existing list
|
|
PhysicsMaterialList::const_iterator it = std::find(mMaterials.begin(), mMaterials.end(), material);
|
|
if (it != mMaterials.end())
|
|
{
|
|
// Found it, calculate index
|
|
*remap_entry = uint8(it - mMaterials.begin());
|
|
}
|
|
else
|
|
{
|
|
// Not found, add it
|
|
if (mMaterials.size() >= 256)
|
|
{
|
|
// We can't have more than 256 materials since we use uint8 as indices
|
|
return false;
|
|
}
|
|
*remap_entry = uint8(mMaterials.size());
|
|
mMaterials.push_back(material);
|
|
}
|
|
++remap_entry;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// No remapping
|
|
for (uint i = 0; i < material_remap_table_size; ++i)
|
|
material_remap_table[i] = uint8(i);
|
|
}
|
|
|
|
if (mMaterials.size() == 1)
|
|
{
|
|
// Only 1 material, we don't need to store the material indices
|
|
return true;
|
|
}
|
|
|
|
// Check if we need to resize the material indices array
|
|
uint count_min_1 = mSampleCount - 1;
|
|
uint32 new_bits_per_material_index = 32 - CountLeadingZeros((uint32)mMaterials.size() - 1);
|
|
JPH_ASSERT(mNumBitsPerMaterialIndex <= 8 && new_bits_per_material_index <= 8);
|
|
if (new_bits_per_material_index > mNumBitsPerMaterialIndex)
|
|
{
|
|
// Resize the material indices array
|
|
mMaterialIndices.resize(((Square(count_min_1) * new_bits_per_material_index + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16
|
|
|
|
// Calculate old and new mask
|
|
uint16 old_material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
|
|
uint16 new_material_index_mask = uint16((1 << new_bits_per_material_index) - 1);
|
|
|
|
// Loop through the array backwards to avoid overwriting data
|
|
int in_bit_pos = (count_min_1 * count_min_1 - 1) * mNumBitsPerMaterialIndex;
|
|
const uint8 *in_indices = mMaterialIndices.data() + (in_bit_pos >> 3);
|
|
in_bit_pos &= 0b111;
|
|
int out_bit_pos = (count_min_1 * count_min_1 - 1) * new_bits_per_material_index;
|
|
uint8 *out_indices = mMaterialIndices.data() + (out_bit_pos >> 3);
|
|
out_bit_pos &= 0b111;
|
|
|
|
while (out_indices >= mMaterialIndices.data())
|
|
{
|
|
// Read the material index
|
|
uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8);
|
|
material_index >>= in_bit_pos;
|
|
material_index &= old_material_index_mask;
|
|
|
|
// Write the material index
|
|
uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8);
|
|
output_data &= ~(new_material_index_mask << out_bit_pos);
|
|
output_data |= material_index << out_bit_pos;
|
|
out_indices[0] = uint8(output_data);
|
|
out_indices[1] = uint8(output_data >> 8);
|
|
|
|
// Go to the previous index
|
|
in_bit_pos -= int(mNumBitsPerMaterialIndex);
|
|
in_indices += in_bit_pos >> 3;
|
|
in_bit_pos &= 0b111;
|
|
out_bit_pos -= int(new_bits_per_material_index);
|
|
out_indices += out_bit_pos >> 3;
|
|
out_bit_pos &= 0b111;
|
|
}
|
|
|
|
// Accept the new bits per material index
|
|
mNumBitsPerMaterialIndex = new_bits_per_material_index;
|
|
}
|
|
|
|
uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
|
|
for (uint y = 0; y < inSizeY; ++y)
|
|
{
|
|
// Calculate input position
|
|
const uint8 *in_indices = inMaterials + y * inMaterialsStride;
|
|
|
|
// Calculate output position
|
|
uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex;
|
|
uint8 *out_indices = mMaterialIndices.data() + (bit_pos >> 3);
|
|
bit_pos &= 0b111;
|
|
|
|
for (uint x = 0; x < inSizeX; ++x)
|
|
{
|
|
// Update material
|
|
uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8);
|
|
output_data &= ~(material_index_mask << bit_pos);
|
|
output_data |= material_remap_table[*in_indices] << bit_pos;
|
|
out_indices[0] = uint8(output_data);
|
|
out_indices[1] = uint8(output_data >> 8);
|
|
|
|
// Go to the next index
|
|
in_indices++;
|
|
bit_pos += mNumBitsPerMaterialIndex;
|
|
out_indices += bit_pos >> 3;
|
|
bit_pos &= 0b111;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
MassProperties HeightFieldShape::GetMassProperties() const
|
|
{
|
|
// Object should always be static, return default mass properties
|
|
return MassProperties();
|
|
}
|
|
|
|
const PhysicsMaterial *HeightFieldShape::GetMaterial(uint inX, uint inY) const
|
|
{
|
|
if (mMaterials.empty())
|
|
return PhysicsMaterial::sDefault;
|
|
if (mMaterials.size() == 1)
|
|
return mMaterials[0];
|
|
|
|
uint count_min_1 = mSampleCount - 1;
|
|
JPH_ASSERT(inX < count_min_1);
|
|
JPH_ASSERT(inY < count_min_1);
|
|
|
|
// Calculate at which bit the material index starts
|
|
uint bit_pos = (inX + inY * count_min_1) * mNumBitsPerMaterialIndex;
|
|
uint byte_pos = bit_pos >> 3;
|
|
bit_pos &= 0b111;
|
|
|
|
// Read the material index
|
|
JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size());
|
|
const uint8 *material_indices = mMaterialIndices.data() + byte_pos;
|
|
uint16 material_index = uint16(material_indices[0]) + uint16(uint16(material_indices[1]) << 8);
|
|
material_index >>= bit_pos;
|
|
material_index &= (1 << mNumBitsPerMaterialIndex) - 1;
|
|
|
|
// Return the material
|
|
return mMaterials[material_index];
|
|
}
|
|
|
|
uint HeightFieldShape::GetSubShapeIDBits() const
|
|
{
|
|
// Need to store X, Y and 1 extra bit to specify the triangle number in the quad
|
|
return 2 * (32 - CountLeadingZeros(mSampleCount - 1)) + 1;
|
|
}
|
|
|
|
SubShapeID HeightFieldShape::EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const
|
|
{
|
|
return inCreator.PushID((inX + inY * mSampleCount) * 2 + inTriangle, GetSubShapeIDBits()).GetID();
|
|
}
|
|
|
|
void HeightFieldShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const
|
|
{
|
|
// Decode sub shape id
|
|
SubShapeID remainder;
|
|
uint32 id = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
|
|
JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID");
|
|
|
|
// Get triangle index
|
|
outTriangle = id & 1;
|
|
id >>= 1;
|
|
|
|
// Fetch the x and y coordinate
|
|
outX = id % mSampleCount;
|
|
outY = id / mSampleCount;
|
|
}
|
|
|
|
void HeightFieldShape::GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const
|
|
{
|
|
DecodeSubShapeID(inSubShapeID, outX, outY, outTriangleIndex);
|
|
}
|
|
|
|
const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShapeID) const
|
|
{
|
|
// Decode ID
|
|
uint x, y, triangle;
|
|
DecodeSubShapeID(inSubShapeID, x, y, triangle);
|
|
|
|
// Fetch the material
|
|
return GetMaterial(x, y);
|
|
}
|
|
|
|
Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
|
|
{
|
|
// Decode ID
|
|
uint x, y, triangle;
|
|
DecodeSubShapeID(inSubShapeID, x, y, triangle);
|
|
|
|
// Fetch vertices that both triangles share
|
|
Vec3 x1y1 = GetPosition(x, y);
|
|
Vec3 x2y2 = GetPosition(x + 1, y + 1);
|
|
|
|
// Get normal depending on which triangle was selected
|
|
Vec3 normal;
|
|
if (triangle == 0)
|
|
{
|
|
Vec3 x1y2 = GetPosition(x, y + 1);
|
|
normal = (x2y2 - x1y2).Cross(x1y1 - x1y2);
|
|
}
|
|
else
|
|
{
|
|
Vec3 x2y1 = GetPosition(x + 1, y);
|
|
normal = (x1y1 - x2y1).Cross(x2y2 - x2y1);
|
|
}
|
|
|
|
return normal.Normalized();
|
|
}
|
|
|
|
void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
|
|
{
|
|
// Decode ID
|
|
uint x, y, triangle;
|
|
DecodeSubShapeID(inSubShapeID, x, y, triangle);
|
|
|
|
// Fetch the triangle
|
|
outVertices.resize(3);
|
|
outVertices[0] = GetPosition(x, y);
|
|
Vec3 v2 = GetPosition(x + 1, y + 1);
|
|
if (triangle == 0)
|
|
{
|
|
outVertices[1] = GetPosition(x, y + 1);
|
|
outVertices[2] = v2;
|
|
}
|
|
else
|
|
{
|
|
outVertices[1] = v2;
|
|
outVertices[2] = GetPosition(x + 1, y);
|
|
}
|
|
|
|
// Flip triangle if scaled inside out
|
|
if (ScaleHelpers::IsInsideOut(inScale))
|
|
std::swap(outVertices[1], outVertices[2]);
|
|
|
|
// Transform to world space
|
|
Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
|
|
for (Vec3 &v : outVertices)
|
|
v = transform * v;
|
|
}
|
|
|
|
inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle) const
|
|
{
|
|
JPH_ASSERT(inX < mSampleCount - 1 && inY < mSampleCount - 1);
|
|
|
|
if (inTriangle == 0)
|
|
{
|
|
// The edge flags for this triangle are directly stored, find the right 3 bits
|
|
uint bit_pos = 3 * (inX + inY * (mSampleCount - 1));
|
|
uint byte_pos = bit_pos >> 3;
|
|
bit_pos &= 0b111;
|
|
JPH_ASSERT(byte_pos + 1 < mActiveEdgesSize);
|
|
const uint8 *active_edges = mActiveEdges + byte_pos;
|
|
uint16 edge_flags = uint16(active_edges[0]) + uint16(uint16(active_edges[1]) << 8);
|
|
return uint8(edge_flags >> bit_pos) & 0b111;
|
|
}
|
|
else
|
|
{
|
|
// We don't store this triangle directly, we need to look at our three neighbours to construct the edge flags
|
|
uint8 edge0 = (GetEdgeFlags(inX, inY, 0) & 0b100) != 0? 0b001 : 0; // Diagonal edge
|
|
uint8 edge1 = inX == mSampleCount - 2 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge
|
|
uint8 edge2 = inY == 0 || (GetEdgeFlags(inX, inY - 1, 0) & 0b010) != 0? 0b100 : 0; // Horizontal edge
|
|
return edge0 | edge1 | edge2;
|
|
}
|
|
}
|
|
|
|
AABox HeightFieldShape::GetLocalBounds() const
|
|
{
|
|
if (mMinSample == cNoCollisionValue16)
|
|
{
|
|
// This whole height field shape doesn't have any collision, return the center point
|
|
Vec3 center = mOffset + 0.5f * mScale * Vec3(float(mSampleCount - 1), 0.0f, float(mSampleCount - 1));
|
|
return AABox(center, center);
|
|
}
|
|
else
|
|
{
|
|
// Bounding box based on min and max sample height
|
|
Vec3 bmin = mOffset + mScale * Vec3(0.0f, float(mMinSample), 0.0f);
|
|
Vec3 bmax = mOffset + mScale * Vec3(float(mSampleCount - 1), float(mMaxSample), float(mSampleCount - 1));
|
|
return AABox(bmin, bmax);
|
|
}
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
|
|
{
|
|
// Don't draw anything if we don't have any collision
|
|
if (mHeightSamplesSize == 0)
|
|
return;
|
|
|
|
// Reset the batch if we switch coloring mode
|
|
if (mCachedUseMaterialColors != inUseMaterialColors)
|
|
{
|
|
mGeometry.clear();
|
|
mCachedUseMaterialColors = inUseMaterialColors;
|
|
}
|
|
|
|
if (mGeometry.empty())
|
|
{
|
|
// Divide terrain in triangle batches of max 64x64x2 triangles to allow better culling of the terrain
|
|
uint32 block_size = min<uint32>(mSampleCount, 64);
|
|
for (uint32 by = 0; by < mSampleCount; by += block_size)
|
|
for (uint32 bx = 0; bx < mSampleCount; bx += block_size)
|
|
{
|
|
// Create vertices for a block
|
|
Array<DebugRenderer::Triangle> triangles;
|
|
triangles.resize(block_size * block_size * 2);
|
|
DebugRenderer::Triangle *out_tri = &triangles[0];
|
|
for (uint32 y = by, max_y = min(by + block_size, mSampleCount - 1); y < max_y; ++y)
|
|
for (uint32 x = bx, max_x = min(bx + block_size, mSampleCount - 1); x < max_x; ++x)
|
|
if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1))
|
|
{
|
|
Vec3 x1y1 = GetPosition(x, y);
|
|
Vec3 x2y2 = GetPosition(x + 1, y + 1);
|
|
Color color = inUseMaterialColors? GetMaterial(x, y)->GetDebugColor() : Color::sWhite;
|
|
|
|
if (!IsNoCollision(x, y + 1))
|
|
{
|
|
Vec3 x1y2 = GetPosition(x, y + 1);
|
|
|
|
x1y1.StoreFloat3(&out_tri->mV[0].mPosition);
|
|
x1y2.StoreFloat3(&out_tri->mV[1].mPosition);
|
|
x2y2.StoreFloat3(&out_tri->mV[2].mPosition);
|
|
|
|
Vec3 normal = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized();
|
|
for (DebugRenderer::Vertex &v : out_tri->mV)
|
|
{
|
|
v.mColor = color;
|
|
v.mUV = Float2(0, 0);
|
|
normal.StoreFloat3(&v.mNormal);
|
|
}
|
|
|
|
++out_tri;
|
|
}
|
|
|
|
if (!IsNoCollision(x + 1, y))
|
|
{
|
|
Vec3 x2y1 = GetPosition(x + 1, y);
|
|
|
|
x1y1.StoreFloat3(&out_tri->mV[0].mPosition);
|
|
x2y2.StoreFloat3(&out_tri->mV[1].mPosition);
|
|
x2y1.StoreFloat3(&out_tri->mV[2].mPosition);
|
|
|
|
Vec3 normal = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized();
|
|
for (DebugRenderer::Vertex &v : out_tri->mV)
|
|
{
|
|
v.mColor = color;
|
|
v.mUV = Float2(0, 0);
|
|
normal.StoreFloat3(&v.mNormal);
|
|
}
|
|
|
|
++out_tri;
|
|
}
|
|
}
|
|
|
|
// Resize triangles array to actual amount of triangles written
|
|
size_t num_triangles = out_tri - &triangles[0];
|
|
triangles.resize(num_triangles);
|
|
|
|
// Create batch
|
|
if (num_triangles > 0)
|
|
mGeometry.push_back(new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), DebugRenderer::sCalculateBounds(&triangles[0].mV[0], int(3 * num_triangles))));
|
|
}
|
|
}
|
|
|
|
// Get transform including scale
|
|
RMat44 transform = inCenterOfMassTransform.PreScaled(inScale);
|
|
|
|
// Test if the shape is scaled inside out
|
|
DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace;
|
|
|
|
// Determine the draw mode
|
|
DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
|
|
|
|
// Draw the geometry
|
|
for (const DebugRenderer::GeometryRef &b : mGeometry)
|
|
inRenderer->DrawGeometry(transform, inColor, b, cull_mode, DebugRenderer::ECastShadow::On, draw_mode);
|
|
|
|
if (sDrawTriangleOutlines)
|
|
{
|
|
struct Visitor
|
|
{
|
|
JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) :
|
|
mShape(inShape),
|
|
mRenderer(inRenderer),
|
|
mTransform(inTransform)
|
|
{
|
|
}
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
|
|
{
|
|
return true;
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
|
|
{
|
|
UVec4 valid = Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY);
|
|
return CountAndSortTrues(valid, ioProperties);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
|
|
{
|
|
// Determine active edges
|
|
uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle);
|
|
|
|
// Loop through edges
|
|
Vec3 v[] = { inV0, inV1, inV2 };
|
|
for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
|
|
{
|
|
RVec3 v1 = mTransform * v[edge_idx];
|
|
RVec3 v2 = mTransform * v[(edge_idx + 1) % 3];
|
|
|
|
// Draw active edge as a green arrow, other edges as grey
|
|
if (active_edges & (1 << edge_idx))
|
|
mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f);
|
|
else
|
|
mRenderer->DrawLine(v1, v2, Color::sGrey);
|
|
}
|
|
}
|
|
|
|
const HeightFieldShape *mShape;
|
|
DebugRenderer * mRenderer;
|
|
RMat44 mTransform;
|
|
};
|
|
|
|
Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale));
|
|
WalkHeightField(visitor);
|
|
}
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
class HeightFieldShape::DecodingContext
|
|
{
|
|
public:
|
|
JPH_INLINE explicit DecodingContext(const HeightFieldShape *inShape) :
|
|
mShape(inShape)
|
|
{
|
|
static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough");
|
|
|
|
// Construct root stack entry
|
|
mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0
|
|
}
|
|
|
|
template <class Visitor>
|
|
JPH_INLINE void WalkHeightField(Visitor &ioVisitor)
|
|
{
|
|
// Early out if there's no collision
|
|
if (mShape->mHeightSamplesSize == 0)
|
|
return;
|
|
|
|
// Assert that an inside-out bounding box does not collide
|
|
JPH_IF_ENABLE_ASSERTS(UVec4 dummy = UVec4::sReplicate(0);)
|
|
JPH_ASSERT(ioVisitor.VisitRangeBlock(Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), dummy, 0) == 0);
|
|
|
|
// Precalculate values relating to sample count
|
|
uint32 sample_count = mShape->mSampleCount;
|
|
UVec4 sample_count_min_1 = UVec4::sReplicate(sample_count - 1);
|
|
|
|
// Precalculate values relating to block size
|
|
uint32 block_size = mShape->mBlockSize;
|
|
uint32 block_size_plus_1 = block_size + 1;
|
|
uint num_blocks = mShape->GetNumBlocks();
|
|
uint num_blocks_min_1 = num_blocks - 1;
|
|
uint max_level = HeightFieldShape::sGetMaxLevel(num_blocks);
|
|
uint32 max_stride = (num_blocks + 1) >> 1;
|
|
|
|
// Precalculate range block offset and stride for GetBlockOffsetAndScale
|
|
uint range_block_offset, range_block_stride;
|
|
sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride);
|
|
|
|
// Allocate space for vertices and 'no collision' flags
|
|
int array_size = Square(block_size_plus_1);
|
|
Vec3 *vertices = reinterpret_cast<Vec3 *>(JPH_STACK_ALLOC(array_size * sizeof(Vec3)));
|
|
bool *no_collision = reinterpret_cast<bool *>(JPH_STACK_ALLOC(array_size * sizeof(bool)));
|
|
|
|
// Splat offsets
|
|
Vec4 ox = mShape->mOffset.SplatX();
|
|
Vec4 oy = mShape->mOffset.SplatY();
|
|
Vec4 oz = mShape->mOffset.SplatZ();
|
|
|
|
// Splat scales
|
|
Vec4 sx = mShape->mScale.SplatX();
|
|
Vec4 sy = mShape->mScale.SplatY();
|
|
Vec4 sz = mShape->mScale.SplatZ();
|
|
|
|
do
|
|
{
|
|
// Decode properties
|
|
uint32 properties_top = mPropertiesStack[mTop];
|
|
uint32 x = properties_top & cMaskBitsXY;
|
|
uint32 y = (properties_top >> cNumBitsXY) & cMaskBitsXY;
|
|
uint32 level = properties_top >> cLevelShift;
|
|
|
|
if (level >= max_level)
|
|
{
|
|
// Determine actual range of samples (minus one because we eventually want to iterate over the triangles, not the samples)
|
|
uint32 min_x = x * block_size;
|
|
uint32 max_x = min_x + block_size;
|
|
uint32 min_y = y * block_size;
|
|
uint32 max_y = min_y + block_size;
|
|
|
|
// Decompress vertices of block at (x, y)
|
|
Vec3 *dst_vertex = vertices;
|
|
bool *dst_no_collision = no_collision;
|
|
float block_offset, block_scale;
|
|
mShape->GetBlockOffsetAndScale(x, y, range_block_offset, range_block_stride, block_offset, block_scale);
|
|
for (uint32 v_y = min_y; v_y < max_y; ++v_y)
|
|
{
|
|
for (uint32 v_x = min_x; v_x < max_x; ++v_x)
|
|
{
|
|
*dst_vertex = mShape->GetPosition(v_x, v_y, block_offset, block_scale, *dst_no_collision);
|
|
++dst_vertex;
|
|
++dst_no_collision;
|
|
}
|
|
|
|
// Skip last column, these values come from a different block
|
|
++dst_vertex;
|
|
++dst_no_collision;
|
|
}
|
|
|
|
// Decompress block (x + 1, y)
|
|
uint32 max_x_decrement = 0;
|
|
if (x < num_blocks_min_1)
|
|
{
|
|
dst_vertex = vertices + block_size;
|
|
dst_no_collision = no_collision + block_size;
|
|
mShape->GetBlockOffsetAndScale(x + 1, y, range_block_offset, range_block_stride, block_offset, block_scale);
|
|
for (uint32 v_y = min_y; v_y < max_y; ++v_y)
|
|
{
|
|
*dst_vertex = mShape->GetPosition(max_x, v_y, block_offset, block_scale, *dst_no_collision);
|
|
dst_vertex += block_size_plus_1;
|
|
dst_no_collision += block_size_plus_1;
|
|
}
|
|
}
|
|
else
|
|
max_x_decrement = 1; // We don't have a next block, one less triangle to test
|
|
|
|
// Decompress block (x, y + 1)
|
|
if (y < num_blocks_min_1)
|
|
{
|
|
uint start = block_size * block_size_plus_1;
|
|
dst_vertex = vertices + start;
|
|
dst_no_collision = no_collision + start;
|
|
mShape->GetBlockOffsetAndScale(x, y + 1, range_block_offset, range_block_stride, block_offset, block_scale);
|
|
for (uint32 v_x = min_x; v_x < max_x; ++v_x)
|
|
{
|
|
*dst_vertex = mShape->GetPosition(v_x, max_y, block_offset, block_scale, *dst_no_collision);
|
|
++dst_vertex;
|
|
++dst_no_collision;
|
|
}
|
|
|
|
// Decompress single sample of block at (x + 1, y + 1)
|
|
if (x < num_blocks_min_1)
|
|
{
|
|
mShape->GetBlockOffsetAndScale(x + 1, y + 1, range_block_offset, range_block_stride, block_offset, block_scale);
|
|
*dst_vertex = mShape->GetPosition(max_x, max_y, block_offset, block_scale, *dst_no_collision);
|
|
}
|
|
}
|
|
else
|
|
--max_y; // We don't have a next block, one less triangle to test
|
|
|
|
// Update max_x (we've been using it so we couldn't update it earlier)
|
|
max_x -= max_x_decrement;
|
|
|
|
// We're going to divide the vertices in 4 blocks to do one more runtime sub-division, calculate the ranges of those blocks
|
|
struct Range
|
|
{
|
|
uint32 mMinX, mMinY, mNumTrianglesX, mNumTrianglesY;
|
|
};
|
|
uint32 half_block_size = block_size >> 1;
|
|
uint32 block_size_x = max_x - min_x - half_block_size;
|
|
uint32 block_size_y = max_y - min_y - half_block_size;
|
|
Range ranges[] =
|
|
{
|
|
{ 0, 0, half_block_size, half_block_size },
|
|
{ half_block_size, 0, block_size_x, half_block_size },
|
|
{ 0, half_block_size, half_block_size, block_size_y },
|
|
{ half_block_size, half_block_size, block_size_x, block_size_y },
|
|
};
|
|
|
|
// Calculate the min and max of each of the blocks
|
|
Mat44 block_min, block_max;
|
|
for (int block = 0; block < 4; ++block)
|
|
{
|
|
// Get the range for this block
|
|
const Range &range = ranges[block];
|
|
uint32 start = range.mMinX + range.mMinY * block_size_plus_1;
|
|
uint32 size_x_plus_1 = range.mNumTrianglesX + 1;
|
|
uint32 size_y_plus_1 = range.mNumTrianglesY + 1;
|
|
|
|
// Calculate where to start reading
|
|
const Vec3 *src_vertex = vertices + start;
|
|
const bool *src_no_collision = no_collision + start;
|
|
uint32 stride = block_size_plus_1 - size_x_plus_1;
|
|
|
|
// Start range with a very large inside-out box
|
|
Vec3 value_min = Vec3::sReplicate(1.0e30f);
|
|
Vec3 value_max = Vec3::sReplicate(-1.0e30f);
|
|
|
|
// Loop over the samples to determine the min and max of this block
|
|
for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y)
|
|
{
|
|
for (uint32 block_x = 0; block_x < size_x_plus_1; ++block_x)
|
|
{
|
|
if (!*src_no_collision)
|
|
{
|
|
value_min = Vec3::sMin(value_min, *src_vertex);
|
|
value_max = Vec3::sMax(value_max, *src_vertex);
|
|
}
|
|
++src_vertex;
|
|
++src_no_collision;
|
|
}
|
|
src_vertex += stride;
|
|
src_no_collision += stride;
|
|
}
|
|
block_min.SetColumn4(block, Vec4(value_min));
|
|
block_max.SetColumn4(block, Vec4(value_max));
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_HEIGHT_FIELD
|
|
// Draw the bounding boxes of the sub-nodes
|
|
for (int block = 0; block < 4; ++block)
|
|
{
|
|
AABox bounds(block_min.GetColumn3(block), block_max.GetColumn3(block));
|
|
if (bounds.IsValid())
|
|
DebugRenderer::sInstance->DrawWireBox(bounds, Color::sYellow);
|
|
}
|
|
#endif // JPH_DEBUG_HEIGHT_FIELD
|
|
|
|
// Transpose so we have the mins and maxes of each of the blocks in rows instead of columns
|
|
Mat44 transposed_min = block_min.Transposed();
|
|
Mat44 transposed_max = block_max.Transposed();
|
|
|
|
// Check which blocks collide
|
|
// Note: At this point we don't use our own stack but we do allow the visitor to use its own stack
|
|
// to store collision distances so that we can still early out when no closer hits have been found.
|
|
UVec4 colliding_blocks(0, 1, 2, 3);
|
|
int num_results = ioVisitor.VisitRangeBlock(transposed_min.GetColumn4(0), transposed_min.GetColumn4(1), transposed_min.GetColumn4(2), transposed_max.GetColumn4(0), transposed_max.GetColumn4(1), transposed_max.GetColumn4(2), colliding_blocks, mTop);
|
|
|
|
// Loop through the results backwards (closest first)
|
|
int result = num_results - 1;
|
|
while (result >= 0)
|
|
{
|
|
// Calculate the min and max of this block
|
|
uint32 block = colliding_blocks[result];
|
|
const Range &range = ranges[block];
|
|
uint32 block_min_x = min_x + range.mMinX;
|
|
uint32 block_max_x = block_min_x + range.mNumTrianglesX;
|
|
uint32 block_min_y = min_y + range.mMinY;
|
|
uint32 block_max_y = block_min_y + range.mNumTrianglesY;
|
|
|
|
// Loop triangles
|
|
for (uint32 v_y = block_min_y; v_y < block_max_y; ++v_y)
|
|
for (uint32 v_x = block_min_x; v_x < block_max_x; ++v_x)
|
|
{
|
|
// Get first vertex
|
|
const int offset = (v_y - min_y) * block_size_plus_1 + (v_x - min_x);
|
|
const Vec3 *start_vertex = vertices + offset;
|
|
const bool *start_no_collision = no_collision + offset;
|
|
|
|
// Check if vertices shared by both triangles have collision
|
|
if (!start_no_collision[0] && !start_no_collision[block_size_plus_1 + 1])
|
|
{
|
|
// Loop 2 triangles
|
|
for (uint t = 0; t < 2; ++t)
|
|
{
|
|
// Determine triangle vertices
|
|
Vec3 v0, v1, v2;
|
|
if (t == 0)
|
|
{
|
|
// Check third vertex
|
|
if (start_no_collision[block_size_plus_1])
|
|
continue;
|
|
|
|
// Get vertices for triangle
|
|
v0 = start_vertex[0];
|
|
v1 = start_vertex[block_size_plus_1];
|
|
v2 = start_vertex[block_size_plus_1 + 1];
|
|
}
|
|
else
|
|
{
|
|
// Check third vertex
|
|
if (start_no_collision[1])
|
|
continue;
|
|
|
|
// Get vertices for triangle
|
|
v0 = start_vertex[0];
|
|
v1 = start_vertex[block_size_plus_1 + 1];
|
|
v2 = start_vertex[1];
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_HEIGHT_FIELD
|
|
DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite);
|
|
#endif
|
|
|
|
// Call visitor
|
|
ioVisitor.VisitTriangle(v_x, v_y, t, v0, v1, v2);
|
|
|
|
// Check if we're done
|
|
if (ioVisitor.ShouldAbort())
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Fetch next block until we find one that the visitor wants to see
|
|
do
|
|
--result;
|
|
while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Visit child grid
|
|
uint32 stride = min(1U << level, max_stride); // At the most detailed level we store a non-power of 2 number of blocks
|
|
uint32 offset = sGridOffsets[level] + stride * y + x;
|
|
|
|
// Decode min/max height
|
|
JPH_ASSERT(offset < mShape->mRangeBlocksSize);
|
|
UVec4 block = UVec4::sLoadInt4Aligned(reinterpret_cast<const uint32 *>(&mShape->mRangeBlocks[offset]));
|
|
Vec4 bounds_miny = oy + sy * block.Expand4Uint16Lo().ToFloat();
|
|
Vec4 bounds_maxy = oy + sy * block.Expand4Uint16Hi().ToFloat();
|
|
|
|
// Calculate size of one cell at this grid level
|
|
UVec4 internal_cell_size = UVec4::sReplicate(block_size << (max_level - level - 1)); // subtract 1 from level because we have an internal grid of 2x2
|
|
|
|
// Calculate min/max x and z
|
|
UVec4 two_x = UVec4::sReplicate(2 * x); // multiply by two because we have an internal grid of 2x2
|
|
Vec4 bounds_minx = ox + sx * (internal_cell_size * (two_x + UVec4(0, 1, 0, 1))).ToFloat();
|
|
Vec4 bounds_maxx = ox + sx * UVec4::sMin(internal_cell_size * (two_x + UVec4(1, 2, 1, 2)), sample_count_min_1).ToFloat();
|
|
|
|
UVec4 two_y = UVec4::sReplicate(2 * y);
|
|
Vec4 bounds_minz = oz + sz * (internal_cell_size * (two_y + UVec4(0, 0, 1, 1))).ToFloat();
|
|
Vec4 bounds_maxz = oz + sz * UVec4::sMin(internal_cell_size * (two_y + UVec4(1, 1, 2, 2)), sample_count_min_1).ToFloat();
|
|
|
|
// Calculate properties of child blocks
|
|
UVec4 properties = UVec4::sReplicate(((level + 1) << cLevelShift) + (y << (cNumBitsXY + 1)) + (x << 1)) + UVec4(0, 1, 1 << cNumBitsXY, (1 << cNumBitsXY) + 1);
|
|
|
|
#ifdef JPH_DEBUG_HEIGHT_FIELD
|
|
// Draw boxes
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
AABox b(Vec3(bounds_minx[i], bounds_miny[i], bounds_minz[i]), Vec3(bounds_maxx[i], bounds_maxy[i], bounds_maxz[i]));
|
|
if (b.IsValid())
|
|
DebugRenderer::sInstance->DrawWireBox(b, Color::sGreen);
|
|
}
|
|
#endif
|
|
|
|
// Check which sub nodes to visit
|
|
int num_results = ioVisitor.VisitRangeBlock(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop);
|
|
|
|
// Push them onto the stack
|
|
JPH_ASSERT(mTop + 4 < cStackSize);
|
|
properties.StoreInt4(&mPropertiesStack[mTop]);
|
|
mTop += num_results;
|
|
}
|
|
|
|
// Check if we're done
|
|
if (ioVisitor.ShouldAbort())
|
|
return;
|
|
|
|
// Fetch next node until we find one that the visitor wants to see
|
|
do
|
|
--mTop;
|
|
while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop));
|
|
}
|
|
while (mTop >= 0);
|
|
}
|
|
|
|
// This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkHeightField() again)
|
|
JPH_INLINE bool IsDoneWalking() const
|
|
{
|
|
return mTop < 0;
|
|
}
|
|
|
|
private:
|
|
const HeightFieldShape * mShape;
|
|
int mTop = 0;
|
|
uint32 mPropertiesStack[cStackSize];
|
|
};
|
|
|
|
template <class Visitor>
|
|
void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const
|
|
{
|
|
DecodingContext ctx(this);
|
|
ctx.WalkHeightField(ioVisitor);
|
|
}
|
|
|
|
bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
struct Visitor
|
|
{
|
|
JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) :
|
|
mHit(ioHit),
|
|
mRayOrigin(inRay.mOrigin),
|
|
mRayDirection(inRay.mDirection),
|
|
mRayInvDirection(inRay.mDirection),
|
|
mShape(inShape),
|
|
mSubShapeIDCreator(inSubShapeIDCreator)
|
|
{
|
|
}
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return mHit.mFraction <= 0.0f;
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const
|
|
{
|
|
return mDistanceStack[inStackTop] < mHit.mFraction;
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
|
|
{
|
|
// Test bounds of 4 children
|
|
Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
|
|
|
|
// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
|
|
return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
|
|
if (fraction < mHit.mFraction)
|
|
{
|
|
// It's a closer hit
|
|
mHit.mFraction = fraction;
|
|
mHit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle);
|
|
mReturnValue = true;
|
|
}
|
|
}
|
|
|
|
RayCastResult & mHit;
|
|
Vec3 mRayOrigin;
|
|
Vec3 mRayDirection;
|
|
RayInvDirection mRayInvDirection;
|
|
const HeightFieldShape *mShape;
|
|
SubShapeIDCreator mSubShapeIDCreator;
|
|
bool mReturnValue = false;
|
|
float mDistanceStack[cStackSize];
|
|
};
|
|
|
|
Visitor visitor(this, inRay, inSubShapeIDCreator, ioHit);
|
|
WalkHeightField(visitor);
|
|
|
|
return visitor.mReturnValue;
|
|
}
|
|
|
|
void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
// Test shape filter
|
|
if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
|
|
return;
|
|
|
|
struct Visitor
|
|
{
|
|
JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) :
|
|
mCollector(ioCollector),
|
|
mRayOrigin(inRay.mOrigin),
|
|
mRayDirection(inRay.mDirection),
|
|
mRayInvDirection(inRay.mDirection),
|
|
mBackFaceMode(inRayCastSettings.mBackFaceModeTriangles),
|
|
mShape(inShape),
|
|
mSubShapeIDCreator(inSubShapeIDCreator)
|
|
{
|
|
}
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return mCollector.ShouldEarlyOut();
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const
|
|
{
|
|
return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
|
|
{
|
|
// Test bounds of 4 children
|
|
Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
|
|
|
|
// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
|
|
return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
|
|
{
|
|
// Back facing check
|
|
if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0)
|
|
return;
|
|
|
|
// Check the triangle
|
|
float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
|
|
if (fraction < mCollector.GetEarlyOutFraction())
|
|
{
|
|
RayCastResult hit;
|
|
hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext());
|
|
hit.mFraction = fraction;
|
|
hit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle);
|
|
mCollector.AddHit(hit);
|
|
}
|
|
}
|
|
|
|
CastRayCollector & mCollector;
|
|
Vec3 mRayOrigin;
|
|
Vec3 mRayDirection;
|
|
RayInvDirection mRayInvDirection;
|
|
EBackFaceMode mBackFaceMode;
|
|
const HeightFieldShape *mShape;
|
|
SubShapeIDCreator mSubShapeIDCreator;
|
|
float mDistanceStack[cStackSize];
|
|
};
|
|
|
|
Visitor visitor(this, inRay, inRayCastSettings, inSubShapeIDCreator, ioCollector);
|
|
WalkHeightField(visitor);
|
|
}
|
|
|
|
void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
|
|
{
|
|
// A height field doesn't have volume, so we can't test insideness
|
|
}
|
|
|
|
void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
struct Visitor : public CollideSoftBodyVerticesVsTriangles
|
|
{
|
|
using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles;
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return false;
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
|
|
{
|
|
return mDistanceStack[inStackTop] < mClosestDistanceSq;
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
|
|
{
|
|
// Get distance to vertex
|
|
Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
|
|
|
|
// Clear distance for invalid bounds
|
|
dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
|
|
|
|
// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
|
|
return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
ProcessTriangle(inV0, inV1, inV2);
|
|
}
|
|
|
|
float mDistanceStack[cStackSize];
|
|
};
|
|
|
|
Visitor visitor(inCenterOfMassTransform, inScale);
|
|
|
|
for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
|
|
if (v.GetInvMass() > 0.0f)
|
|
{
|
|
visitor.StartVertex(v);
|
|
WalkHeightField(visitor);
|
|
visitor.FinishVertex(v, inCollidingShapeIndex);
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
struct Visitor : public CastConvexVsTriangles
|
|
{
|
|
using CastConvexVsTriangles::CastConvexVsTriangles;
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return mCollector.ShouldEarlyOut();
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const
|
|
{
|
|
return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
|
|
{
|
|
// Scale the bounding boxes of this node
|
|
Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
|
|
AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Enlarge them by the casted shape's box extents
|
|
AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Test bounds of 4 children
|
|
Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Clear distance for invalid bounds
|
|
distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
|
|
|
|
// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
|
|
return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
// Create sub shape id for this part
|
|
SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
|
|
|
|
// Determine active edges
|
|
uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
|
|
|
|
Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
|
|
}
|
|
|
|
const HeightFieldShape * mShape2;
|
|
RayInvDirection mInvDirection;
|
|
Vec3 mBoxCenter;
|
|
Vec3 mBoxExtent;
|
|
SubShapeIDCreator mSubShapeIDCreator2;
|
|
float mDistanceStack[cStackSize];
|
|
};
|
|
|
|
JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
|
|
const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
|
|
|
|
Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
|
|
visitor.mShape2 = shape;
|
|
visitor.mInvDirection.Set(inShapeCast.mDirection);
|
|
visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter();
|
|
visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent();
|
|
visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
|
|
shape->WalkHeightField(visitor);
|
|
}
|
|
|
|
void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
struct Visitor : public CastSphereVsTriangles
|
|
{
|
|
using CastSphereVsTriangles::CastSphereVsTriangles;
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return mCollector.ShouldEarlyOut();
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const
|
|
{
|
|
return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
|
|
{
|
|
// Scale the bounding boxes of this node
|
|
Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
|
|
AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Enlarge them by the radius of the sphere
|
|
AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Test bounds of 4 children
|
|
Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Clear distance for invalid bounds
|
|
distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
|
|
|
|
// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
|
|
return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
// Create sub shape id for this part
|
|
SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
|
|
|
|
// Determine active edges
|
|
uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
|
|
|
|
Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
|
|
}
|
|
|
|
const HeightFieldShape * mShape2;
|
|
RayInvDirection mInvDirection;
|
|
SubShapeIDCreator mSubShapeIDCreator2;
|
|
float mDistanceStack[cStackSize];
|
|
};
|
|
|
|
JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
|
|
const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
|
|
|
|
Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
|
|
visitor.mShape2 = shape;
|
|
visitor.mInvDirection.Set(inShapeCast.mDirection);
|
|
visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
|
|
shape->WalkHeightField(visitor);
|
|
}
|
|
|
|
struct HeightFieldShape::HSGetTrianglesContext
|
|
{
|
|
HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
|
|
mDecodeCtx(inShape),
|
|
mShape(inShape),
|
|
mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
|
|
mHeightFieldScale(inScale),
|
|
mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
|
|
mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
|
|
{
|
|
}
|
|
|
|
bool ShouldAbort() const
|
|
{
|
|
return mShouldAbort;
|
|
}
|
|
|
|
bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
|
|
{
|
|
return true;
|
|
}
|
|
|
|
int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
|
|
{
|
|
// Scale the bounding boxes of this node
|
|
Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
|
|
AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Test which nodes collide
|
|
UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Filter out invalid bounding boxes
|
|
collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
|
|
|
|
return CountAndSortTrues(collides, ioProperties);
|
|
}
|
|
|
|
void VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
// When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here.
|
|
if (mNumTrianglesFound + 1 > mMaxTrianglesRequested)
|
|
{
|
|
mShouldAbort = true;
|
|
return;
|
|
}
|
|
|
|
// Store vertices as Float3
|
|
if (mIsInsideOut)
|
|
{
|
|
// Reverse vertices
|
|
(mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++);
|
|
(mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++);
|
|
(mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++);
|
|
}
|
|
else
|
|
{
|
|
// Normal scale
|
|
(mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++);
|
|
(mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++);
|
|
(mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++);
|
|
}
|
|
|
|
// Decode material
|
|
if (mMaterials != nullptr)
|
|
*mMaterials++ = mShape->GetMaterial(inX, inY);
|
|
|
|
// Accumulate triangles found
|
|
mNumTrianglesFound++;
|
|
}
|
|
|
|
DecodingContext mDecodeCtx;
|
|
const HeightFieldShape * mShape;
|
|
OrientedBox mLocalBox;
|
|
Vec3 mHeightFieldScale;
|
|
Mat44 mLocalToWorld;
|
|
int mMaxTrianglesRequested;
|
|
Float3 * mTriangleVertices;
|
|
int mNumTrianglesFound;
|
|
const PhysicsMaterial ** mMaterials;
|
|
bool mShouldAbort;
|
|
bool mIsInsideOut;
|
|
};
|
|
|
|
void HeightFieldShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
|
|
{
|
|
static_assert(sizeof(HSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
|
|
JPH_ASSERT(IsAligned(&ioContext, alignof(HSGetTrianglesContext)));
|
|
|
|
new (&ioContext) HSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale);
|
|
}
|
|
|
|
int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
|
|
{
|
|
static_assert(cGetTrianglesMinTrianglesRequested >= 1, "cGetTrianglesMinTrianglesRequested is too small");
|
|
JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
|
|
|
|
// Check if we're done
|
|
HSGetTrianglesContext &context = (HSGetTrianglesContext &)ioContext;
|
|
if (context.mDecodeCtx.IsDoneWalking())
|
|
return 0;
|
|
|
|
// Store parameters on context
|
|
context.mMaxTrianglesRequested = inMaxTrianglesRequested;
|
|
context.mTriangleVertices = outTriangleVertices;
|
|
context.mMaterials = outMaterials;
|
|
context.mShouldAbort = false; // Reset the abort flag
|
|
context.mNumTrianglesFound = 0;
|
|
|
|
// Continue (or start) walking the height field
|
|
context.mDecodeCtx.WalkHeightField(context);
|
|
return context.mNumTrianglesFound;
|
|
}
|
|
|
|
void HeightFieldShape::sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
// Get the shapes
|
|
JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
|
|
JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField);
|
|
const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
|
|
const HeightFieldShape *shape2 = static_cast<const HeightFieldShape *>(inShape2);
|
|
|
|
struct Visitor : public CollideConvexVsTriangles
|
|
{
|
|
using CollideConvexVsTriangles::CollideConvexVsTriangles;
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return mCollector.ShouldEarlyOut();
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
|
|
{
|
|
return true;
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
|
|
{
|
|
// Scale the bounding boxes of this node
|
|
Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
|
|
AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Test which nodes collide
|
|
UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Filter out invalid bounding boxes
|
|
collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
|
|
|
|
return CountAndSortTrues(collides, ioProperties);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
// Create ID for triangle
|
|
SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
|
|
|
|
// Determine active edges
|
|
uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
|
|
|
|
Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
|
|
}
|
|
|
|
const HeightFieldShape * mShape2;
|
|
SubShapeIDCreator mSubShapeIDCreator2;
|
|
};
|
|
|
|
Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
|
|
visitor.mShape2 = shape2;
|
|
visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
|
|
shape2->WalkHeightField(visitor);
|
|
}
|
|
|
|
void HeightFieldShape::sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
|
|
{
|
|
JPH_PROFILE_FUNCTION();
|
|
|
|
// Get the shapes
|
|
JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
|
|
JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField);
|
|
const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
|
|
const HeightFieldShape *shape2 = static_cast<const HeightFieldShape *>(inShape2);
|
|
|
|
struct Visitor : public CollideSphereVsTriangles
|
|
{
|
|
using CollideSphereVsTriangles::CollideSphereVsTriangles;
|
|
|
|
JPH_INLINE bool ShouldAbort() const
|
|
{
|
|
return mCollector.ShouldEarlyOut();
|
|
}
|
|
|
|
JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
|
|
{
|
|
return true;
|
|
}
|
|
|
|
JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
|
|
{
|
|
// Scale the bounding boxes of this node
|
|
Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
|
|
AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Test which nodes collide
|
|
UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
|
|
|
|
// Filter out invalid bounding boxes
|
|
collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
|
|
|
|
return CountAndSortTrues(collides, ioProperties);
|
|
}
|
|
|
|
JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
|
|
{
|
|
// Create ID for triangle
|
|
SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
|
|
|
|
// Determine active edges
|
|
uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
|
|
|
|
Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
|
|
}
|
|
|
|
const HeightFieldShape * mShape2;
|
|
SubShapeIDCreator mSubShapeIDCreator2;
|
|
};
|
|
|
|
Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
|
|
visitor.mShape2 = shape2;
|
|
visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
|
|
shape2->WalkHeightField(visitor);
|
|
}
|
|
|
|
void HeightFieldShape::SaveBinaryState(StreamOut &inStream) const
|
|
{
|
|
Shape::SaveBinaryState(inStream);
|
|
|
|
inStream.Write(mOffset);
|
|
inStream.Write(mScale);
|
|
inStream.Write(mSampleCount);
|
|
inStream.Write(mBlockSize);
|
|
inStream.Write(mBitsPerSample);
|
|
inStream.Write(mMinSample);
|
|
inStream.Write(mMaxSample);
|
|
inStream.Write(mMaterialIndices);
|
|
inStream.Write(mNumBitsPerMaterialIndex);
|
|
|
|
if (mRangeBlocks != nullptr)
|
|
{
|
|
inStream.Write(true);
|
|
inStream.WriteBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize);
|
|
}
|
|
else
|
|
{
|
|
inStream.Write(false);
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::RestoreBinaryState(StreamIn &inStream)
|
|
{
|
|
Shape::RestoreBinaryState(inStream);
|
|
|
|
inStream.Read(mOffset);
|
|
inStream.Read(mScale);
|
|
inStream.Read(mSampleCount);
|
|
inStream.Read(mBlockSize);
|
|
inStream.Read(mBitsPerSample);
|
|
inStream.Read(mMinSample);
|
|
inStream.Read(mMaxSample);
|
|
inStream.Read(mMaterialIndices);
|
|
inStream.Read(mNumBitsPerMaterialIndex);
|
|
|
|
// We don't have the exact number of reserved materials anymore, but ensure that our array is big enough
|
|
// TODO: Next time when we bump the binary serialization format of this class we should store the capacity and allocate the right amount, for now we accept a little bit of waste
|
|
mMaterials.reserve(PhysicsMaterialList::size_type(1) << mNumBitsPerMaterialIndex);
|
|
|
|
CacheValues();
|
|
|
|
bool has_heights = false;
|
|
inStream.Read(has_heights);
|
|
if (has_heights)
|
|
{
|
|
AllocateBuffers();
|
|
inStream.ReadBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize);
|
|
}
|
|
}
|
|
|
|
void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
|
|
{
|
|
outMaterials = mMaterials;
|
|
}
|
|
|
|
void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
|
|
{
|
|
mMaterials.assign(inMaterials, inMaterials + inNumMaterials);
|
|
}
|
|
|
|
Shape::Stats HeightFieldShape::GetStats() const
|
|
{
|
|
return Stats(
|
|
sizeof(*this)
|
|
+ mMaterials.size() * sizeof(Ref<PhysicsMaterial>)
|
|
+ mRangeBlocksSize * sizeof(RangeBlock)
|
|
+ mHeightSamplesSize * sizeof(uint8)
|
|
+ mActiveEdgesSize * sizeof(uint8)
|
|
+ mMaterialIndices.size() * sizeof(uint8),
|
|
mHeightSamplesSize == 0? 0 : Square(mSampleCount - 1) * 2);
|
|
}
|
|
|
|
void HeightFieldShape::sRegister()
|
|
{
|
|
ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::HeightField);
|
|
f.mConstruct = []() -> Shape * { return new HeightFieldShape; };
|
|
f.mColor = Color::sPurple;
|
|
|
|
for (EShapeSubType s : sConvexSubShapeTypes)
|
|
{
|
|
CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField);
|
|
CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField);
|
|
|
|
CollisionDispatch::sRegisterCastShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCastShape);
|
|
CollisionDispatch::sRegisterCollideShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCollideShape);
|
|
}
|
|
|
|
// Specialized collision functions
|
|
CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCollideSphereVsHeightField);
|
|
CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCastSphereVsHeightField);
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|