117 lines
3.5 KiB
C++
117 lines
3.5 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/Constraints/PathConstraintPath.h>
|
|
#include <Jolt/Core/StreamIn.h>
|
|
#include <Jolt/Core/StreamOut.h>
|
|
#include <Jolt/Core/Factory.h>
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
#include <Jolt/Renderer/DebugRenderer.h>
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath)
|
|
{
|
|
JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject)
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
// Helper function to transform the results of GetPointOnPath to world space
|
|
static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal)
|
|
{
|
|
outPosition = inTransform * inPosition;
|
|
ioNormal = inTransform.Multiply3x3(ioNormal);
|
|
ioBinormal = inTransform.Multiply3x3(ioBinormal);
|
|
}
|
|
|
|
// Helper function to draw a path segment
|
|
static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal)
|
|
{
|
|
inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite);
|
|
inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f);
|
|
inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f);
|
|
}
|
|
|
|
void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const
|
|
{
|
|
// Calculate first point
|
|
Vec3 lfirst_pos, first_tangent, first_normal, first_binormal;
|
|
GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal);
|
|
RVec3 first_pos;
|
|
sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal);
|
|
|
|
float t_max = GetPathMaxFraction();
|
|
|
|
// Draw the segments
|
|
RVec3 prev_pos = first_pos;
|
|
for (float t = 0.1f; t < t_max; t += 0.1f)
|
|
{
|
|
Vec3 lpos, tangent, normal, binormal;
|
|
GetPointOnPath(t, lpos, tangent, normal, binormal);
|
|
RVec3 pos;
|
|
sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
|
|
sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
|
|
prev_pos = pos;
|
|
}
|
|
|
|
// Draw last point
|
|
Vec3 lpos, tangent, normal, binormal;
|
|
GetPointOnPath(t_max, lpos, tangent, normal, binormal);
|
|
RVec3 pos;
|
|
sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
|
|
sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
|
|
}
|
|
#endif // JPH_DEBUG_RENDERER
|
|
|
|
void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const
|
|
{
|
|
inStream.Write(GetRTTI()->GetHash());
|
|
inStream.Write(mIsLooping);
|
|
}
|
|
|
|
void PathConstraintPath::RestoreBinaryState(StreamIn &inStream)
|
|
{
|
|
// Type hash read by sRestoreFromBinaryState
|
|
inStream.Read(mIsLooping);
|
|
}
|
|
|
|
PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream)
|
|
{
|
|
PathResult result;
|
|
|
|
// Read the type of the constraint
|
|
uint32 hash;
|
|
inStream.Read(hash);
|
|
if (inStream.IsEOF() || inStream.IsFailed())
|
|
{
|
|
result.SetError("Failed to read type id");
|
|
return result;
|
|
}
|
|
|
|
// Get the RTTI for the shape
|
|
const RTTI *rtti = Factory::sInstance->Find(hash);
|
|
if (rtti == nullptr)
|
|
{
|
|
result.SetError("Failed to resolve type. Type not registered in factory?");
|
|
return result;
|
|
}
|
|
|
|
// Construct and read the data of the shape
|
|
Ref<PathConstraintPath> path = reinterpret_cast<PathConstraintPath *>(rtti->CreateObject());
|
|
path->RestoreBinaryState(inStream);
|
|
if (inStream.IsEOF() || inStream.IsFailed())
|
|
{
|
|
result.SetError("Failed to restore constraint");
|
|
return result;
|
|
}
|
|
|
|
result.Set(path);
|
|
return result;
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|