FORCEINLINE void TVoxelData::getNormal(int x, int y, int z, FVector& normal) const {
if (normal_data.size() == 0) {
normal.Set(0, 0, 0);
}
if (x < voxel_num && y < voxel_num && z < voxel_num) {
const int index = x * voxel_num * voxel_num + y * voxel_num + z;
FVector tmp = normal_data[index];
normal.Set(tmp.X, tmp.Y, tmp.Z);
}
else {
normal.Set(0, 0, 0);
}
}
void UAnimGraphNode_SkeletalControlBase::GetDefaultValue(const FString& UpdateDefaultValueName, FVector& OutVec)
{
for (UEdGraphPin* Pin : Pins)
{
if (Pin->PinName == UpdateDefaultValueName)
{
if (GetSchema()->IsCurrentPinDefaultValid(Pin).IsEmpty())
{
FString DefaultString = Pin->GetDefaultAsString();
TArray<FString> ResultString;
//Parse string to split its contents separated by ','
DefaultString.Trim();
DefaultString.TrimTrailing();
DefaultString.ParseIntoArray(&ResultString, TEXT(","), true);
check(ResultString.Num() == 3);
OutVec.Set(
FCString::Atof(*ResultString[0]),
FCString::Atof(*ResultString[1]),
FCString::Atof(*ResultString[2])
);
return;
}
}
}
OutVec = FVector::ZeroVector;
}
void UAURSmoothingFilterKalman::Measurement(FTransform const & MeasuredTransform)
{
try
{
FVector translation = MeasuredTransform.GetTranslation();
FString msg = "In: " + translation.ToString();
//FQuat rotation = MeasuredTransform.GetRotation();
TransformAsMat.at<float>(0) = translation.X;
TransformAsMat.at<float>(1) = translation.Y;
TransformAsMat.at<float>(2) = translation.Z;
//TransformAsMat.at<float>(3) = rotation.X * rotation.W;
//TransformAsMat.at<float>(4) = rotation.Y * rotation.W;
//TransformAsMat.at<float>(5) = rotation.Z * rotation.W;
cv::Mat FilterResult = Filter.predict();
Filter.correct(TransformAsMat);
translation.Set(FilterResult.at<float>(0), FilterResult.at<float>(1), FilterResult.at<float>(2));
msg += "\nOut: " + translation.ToString();
UE_LOG(LogAUR, Log, TEXT("%s"), *msg);
//FVector rot_axis(FilterResult.at<float>(3), FilterResult.at<float>(4), FilterResult.at<float>(5));
//float rot_magnitude = rot_axis.Size();
//rot_axis.Normalize();
//rotation.X = rot_axis.X;
//rotation.Y = rot_axis.Y;
//rotation.Z = rot_axis.Z;
//rotation.W = rot_magnitude;
CurrentTransform.SetComponents(MeasuredTransform.GetRotation(), translation, FVector(1, 1, 1));
}
catch (cv::Exception& e)
{
FString exception_msg(e.what());
UE_LOG(LogAUR, Error, TEXT("Kalman: %s"), *exception_msg)
CurrentTransform = MeasuredTransform;
}
}
/*
* Creates a Box above this Vessel that listens for Available Vessels that enter its perimeter
* Generates an event which will subsequently inform the Available Vessel that is overlapping this
*/
void AVessel::CreateTriggerBox()
{
if (TriggerBox == NULL)
{
TriggerBox = NewObject<UBoxComponent>(this, TEXT("TriggerBox"));
TriggerBox->OnComponentBeginOverlap.AddDynamic(this, &AVessel::OnBeginOverlap);
TriggerBox->OnComponentEndOverlap.AddDynamic(this, &AVessel::OnEndOverlap);
TriggerBox->RegisterComponent();
TriggerBox->AttachTo(RootComponent);
}
// Adjust the size of the Trigger Box
FVector BoundingBox = ThreeDeeModel->Bounds.BoxExtent; // copy
BoundingBox *= BoundingBoxBufferScale;
float larger = FMath::Max(BoundingBox.X, BoundingBox.Y); // Make it a cube
const float MAX_PLACEMENT_CAMERA_HEIGHT = 2000.f; // TODO: Place this somewhere more appropriate
BoundingBox.Set(larger, larger, MAX_PLACEMENT_CAMERA_HEIGHT);
TriggerBox->SetRelativeLocation(FVector(0, 0, 0));
TriggerBox->SetBoxExtent(BoundingBox);
}
void USkeleton::ConvertAnims(UAnimSequence4* Seq)
{
guard(USkeleton::ConvertAnims);
CAnimSet* AnimSet = ConvertedAnim;
if (!AnimSet)
{
AnimSet = new CAnimSet(this);
ConvertedAnim = AnimSet;
// Copy bone names
AnimSet->TrackBoneNames.Empty(ReferenceSkeleton.RefBoneInfo.Num());
for (int i = 0; i < ReferenceSkeleton.RefBoneInfo.Num(); i++)
{
AnimSet->TrackBoneNames.Add(ReferenceSkeleton.RefBoneInfo[i].Name);
}
//TODO: verify if UE4 has AnimRotationOnly stuff
AnimSet->AnimRotationOnly = false;
}
if (!Seq) return; // allow calling ConvertAnims(NULL) to create empty AnimSet
// DBG("----------- Skeleton %s: %d seq, %d bones -----------\n", Name, Anims.Num(), ReferenceSkeleton.RefBoneInfo.Num());
int NumTracks = Seq->GetNumTracks();
#if DEBUG_DECOMPRESS
appPrintf("Sequence %s: %d bones, %d offsets (%g per bone), %d frames, %d compressed data\n"
" trans %s, rot %s, scale %s, key %s\n",
Seq->Name, NumTracks, Seq->CompressedTrackOffsets.Num(), Seq->CompressedTrackOffsets.Num() / (float)NumTracks,
Seq->NumFrames, Seq->CompressedByteStream.Num(),
EnumToName(Seq->TranslationCompressionFormat),
EnumToName(Seq->RotationCompressionFormat),
EnumToName(Seq->ScaleCompressionFormat),
EnumToName(Seq->KeyEncodingFormat)
);
for (int i2 = 0; i2 < Seq->CompressedTrackOffsets.Num(); /*empty*/)
{
if (Seq->KeyEncodingFormat != AKF_PerTrackCompression)
{
FName BoneName = ReferenceSkeleton.RefBoneInfo[Seq->GetTrackBoneIndex(i2/4)].Name;
int TransOffset = Seq->CompressedTrackOffsets[i2 ];
int TransKeys = Seq->CompressedTrackOffsets[i2+1];
int RotOffset = Seq->CompressedTrackOffsets[i2+2];
int RotKeys = Seq->CompressedTrackOffsets[i2+3];
appPrintf(" [%d] = trans %d[%d] rot %d[%d] - %s\n", i2/4, TransOffset, TransKeys, RotOffset, RotKeys, *BoneName);
i2 += 4;
}
else
{
FName BoneName = ReferenceSkeleton.RefBoneInfo[Seq->GetTrackBoneIndex(i2/2)].Name;
int TransOffset = Seq->CompressedTrackOffsets[i2 ];
int RotOffset = Seq->CompressedTrackOffsets[i2+1];
appPrintf(" [%d] = trans %d rot %d - %s\n", i2/2, TransOffset, RotOffset, *BoneName);
i2 += 2;
}
}
#endif // DEBUG_DECOMPRESS
// some checks
int offsetsPerBone = 4;
if (Seq->KeyEncodingFormat == AKF_PerTrackCompression)
offsetsPerBone = 2;
if (Seq->CompressedTrackOffsets.Num() != NumTracks * offsetsPerBone && !Seq->RawAnimationData.Num())
{
appNotify("AnimSequence %s has wrong CompressedTrackOffsets size (has %d, expected %d), removing track",
Seq->Name, Seq->CompressedTrackOffsets.Num(), NumTracks * offsetsPerBone);
return;
}
// create CAnimSequence
CAnimSequence *Dst = new CAnimSequence;
AnimSet->Sequences.Add(Dst);
Dst->Name = Seq->Name;
Dst->NumFrames = Seq->NumFrames;
Dst->Rate = Seq->NumFrames / Seq->SequenceLength * Seq->RateScale;
// bone tracks ...
Dst->Tracks.Empty(NumTracks);
FMemReader Reader(Seq->CompressedByteStream.GetData(), Seq->CompressedByteStream.Num());
Reader.SetupFrom(*Package);
bool HasTimeTracks = (Seq->KeyEncodingFormat == AKF_VariableKeyLerp);
for (int BoneIndex = 0; BoneIndex < ReferenceSkeleton.RefBoneInfo.Num(); BoneIndex++)
{
CAnimTrack *A = new (Dst->Tracks) CAnimTrack;
int TrackIndex = Seq->FindTrackForBoneIndex(BoneIndex);
if (TrackIndex < 0)
{
// this track has no animation, use static pose from ReferenceSkeleton
const FTransform& RefPose = ReferenceSkeleton.RefBonePose[BoneIndex];
A->KeyPos.Add(CVT(RefPose.Translation));
A->KeyQuat.Add(CVT(RefPose.Rotation));
//!! RefPose.Scale3D
continue;
}
int k;
if (!Seq->CompressedTrackOffsets.Num()) //?? or if RawAnimData.Num() != 0
{
// using RawAnimData array
assert(Seq->RawAnimationData.Num() == NumTracks);
CopyArray(A->KeyPos, CVT(Seq->RawAnimationData[TrackIndex].PosKeys));
CopyArray(A->KeyQuat, CVT(Seq->RawAnimationData[TrackIndex].RotKeys));
CopyArray(A->KeyTime, Seq->RawAnimationData[TrackIndex].KeyTimes); // may be empty
for (int k = 0; k < A->KeyTime.Num(); k++)
A->KeyTime[k] *= Dst->Rate;
continue;
}
FVector Mins, Ranges; // common ...
static const CVec3 nullVec = { 0, 0, 0 };
static const CQuat nullQuat = { 0, 0, 0, 1 };
int offsetIndex = TrackIndex * offsetsPerBone;
// PARAGON has invalid data inside some animation tracks. Not sure if game engine ignores them
// or trying to process (this game has holes in data due to wrong pointers in CompressedTrackOffsets).
// This causes garbage data to appear instead of real animation track header, with wrong compression
// method etc. We're going to skip such tracks with displaying a warning message.
if (0) // this is just a placeholder for error handler - it should be located somewhere
{
track_error:
AnimSet->Sequences.RemoveSingle(Dst);
delete Dst;
return;
}
//----------------------------------------------
// decode AKF_PerTrackCompression data
//----------------------------------------------
if (Seq->KeyEncodingFormat == AKF_PerTrackCompression)
{
// this format uses different key storage
guard(PerTrackCompression);
assert(Seq->TranslationCompressionFormat == ACF_Identity);
assert(Seq->RotationCompressionFormat == ACF_Identity);
int TransOffset = Seq->CompressedTrackOffsets[offsetIndex ];
int RotOffset = Seq->CompressedTrackOffsets[offsetIndex+1];
uint32 PackedInfo;
AnimationCompressionFormat KeyFormat;
int ComponentMask;
int NumKeys;
#define DECODE_PER_TRACK_INFO(info) \
KeyFormat = (AnimationCompressionFormat)(info >> 28); \
ComponentMask = (info >> 24) & 0xF; \
NumKeys = info & 0xFFFFFF; \
HasTimeTracks = (ComponentMask & 8) != 0;
guard(TransKeys);
// read translation keys
if (TransOffset == -1)
{
A->KeyPos.Add(nullVec);
DBG(" [%d] no translation data\n", TrackIndex);
}
else
{
Reader.Seek(TransOffset);
Reader << PackedInfo;
DECODE_PER_TRACK_INFO(PackedInfo);
A->KeyPos.Empty(NumKeys);
DBG(" [%d] trans: fmt=%d (%s), %d keys, mask %d\n", TrackIndex,
KeyFormat, EnumToName(KeyFormat), NumKeys, ComponentMask
);
if (KeyFormat == ACF_IntervalFixed32NoW)
{
// read mins/maxs
Mins.Set(0, 0, 0);
Ranges.Set(0, 0, 0);
if (ComponentMask & 1) Reader << Mins.X << Ranges.X;
if (ComponentMask & 2) Reader << Mins.Y << Ranges.Y;
if (ComponentMask & 4) Reader << Mins.Z << Ranges.Z;
}
for (k = 0; k < NumKeys; k++)
{
switch (KeyFormat)
{
// case ACF_None:
case ACF_Float96NoW:
{
FVector v;
if (ComponentMask & 7)
{
v.Set(0, 0, 0);
if (ComponentMask & 1) Reader << v.X;
if (ComponentMask & 2) Reader << v.Y;
if (ComponentMask & 4) Reader << v.Z;
}
else
{
// ACF_Float96NoW has a special case for ((ComponentMask & 7) == 0)
Reader << v;
}
A->KeyPos.Add(CVT(v));
}
break;
TPR(ACF_IntervalFixed32NoW, FVectorIntervalFixed32)
case ACF_Fixed48NoW:
{
uint16 X, Y, Z;
CVec3 v;
v.Set(0, 0, 0);
if (ComponentMask & 1)
{
Reader << X; v[0] = DecodeFixed48_PerTrackComponent<7>(X);
}
if (ComponentMask & 2)
{
Reader << Y; v[1] = DecodeFixed48_PerTrackComponent<7>(Y);
}
if (ComponentMask & 4)
{
Reader << Z; v[2] = DecodeFixed48_PerTrackComponent<7>(Z);
}
A->KeyPos.Add(v);
}
break;
case ACF_Identity:
A->KeyPos.Add(nullVec);
break;
default:
{
char buf[1024];
Seq->GetFullName(buf, 1024);
appNotify("%s: unknown translation compression method: %d (%s) - dropping track", buf, KeyFormat, EnumToName(KeyFormat));
goto track_error;
}
}
}
// align to 4 bytes
Reader.Seek(Align(Reader.Tell(), 4));
if (HasTimeTracks)
ReadTimeArray(Reader, NumKeys, A->KeyPosTime, Seq->NumFrames);
}
unguard;
guard(RotKeys);
// read rotation keys
if (RotOffset == -1)
{
A->KeyQuat.Add(nullQuat);
DBG(" [%d] no rotation data\n", TrackIndex);
}
else
{
Reader.Seek(RotOffset);
Reader << PackedInfo;
DECODE_PER_TRACK_INFO(PackedInfo);
A->KeyQuat.Empty(NumKeys);
DBG(" [%d] rot : fmt=%d (%s), %d keys, mask %d\n", TrackIndex,
KeyFormat, EnumToName(KeyFormat), NumKeys, ComponentMask
);
if (KeyFormat == ACF_IntervalFixed32NoW)
{
// read mins/maxs
Mins.Set(0, 0, 0);
Ranges.Set(0, 0, 0);
if (ComponentMask & 1) Reader << Mins.X << Ranges.X;
if (ComponentMask & 2) Reader << Mins.Y << Ranges.Y;
if (ComponentMask & 4) Reader << Mins.Z << Ranges.Z;
}
for (k = 0; k < NumKeys; k++)
{
switch (KeyFormat)
{
// TR (ACF_None, FQuat)
case ACF_Float96NoW:
{
FQuatFloat96NoW q;
Reader << q;
FQuat q2 = q; // convert
A->KeyQuat.Add(CVT(q2));
}
break;
case ACF_Fixed48NoW:
{
FQuatFixed48NoW q;
q.X = q.Y = q.Z = 32767; // corresponds to 0
if (ComponentMask & 1) Reader << q.X;
if (ComponentMask & 2) Reader << q.Y;
if (ComponentMask & 4) Reader << q.Z;
FQuat q2 = q; // convert
A->KeyQuat.Add(CVT(q2));
}
break;
TR (ACF_Fixed32NoW, FQuatFixed32NoW)
TRR(ACF_IntervalFixed32NoW, FQuatIntervalFixed32NoW)
TR (ACF_Float32NoW, FQuatFloat32NoW)
case ACF_Identity:
A->KeyQuat.Add(nullQuat);
break;
default:
{
char buf[1024];
Seq->GetFullName(buf, 1024);
appNotify("%s: unknown rotation compression method: %d (%s) - dropping track", buf, KeyFormat, EnumToName(KeyFormat));
goto track_error;
}
}
}
// align to 4 bytes
Reader.Seek(Align(Reader.Tell(), 4));
if (HasTimeTracks)
ReadTimeArray(Reader, NumKeys, A->KeyQuatTime, Seq->NumFrames);
}
unguard;
unguard;
continue;
// end of AKF_PerTrackCompression block ...
}
//----------------------------------------------
// end of AKF_PerTrackCompression decoder
//----------------------------------------------
// read animations
int TransOffset = Seq->CompressedTrackOffsets[offsetIndex ];
int TransKeys = Seq->CompressedTrackOffsets[offsetIndex+1];
int RotOffset = Seq->CompressedTrackOffsets[offsetIndex+2];
int RotKeys = Seq->CompressedTrackOffsets[offsetIndex+3];
// appPrintf("[%d:%d:%d] : %d[%d] %d[%d] %d[%d]\n", j, Seq->RotationCompressionFormat, Seq->TranslationCompressionFormat, TransOffset, TransKeys, RotOffset, RotKeys, ScaleOffset, ScaleKeys);
A->KeyPos.Empty(TransKeys);
A->KeyQuat.Empty(RotKeys);
// read translation keys
if (TransKeys)
{
Reader.Seek(TransOffset);
AnimationCompressionFormat TranslationCompressionFormat = Seq->TranslationCompressionFormat;
if (TransKeys == 1)
TranslationCompressionFormat = ACF_None; // single key is stored without compression
// read mins/ranges
if (TranslationCompressionFormat == ACF_IntervalFixed32NoW)
{
Reader << Mins << Ranges;
}
for (k = 0; k < TransKeys; k++)
{
switch (TranslationCompressionFormat)
{
TP (ACF_None, FVector)
TP (ACF_Float96NoW, FVector)
TPR(ACF_IntervalFixed32NoW, FVectorIntervalFixed32)
TP (ACF_Fixed48NoW, FVectorFixed48)
case ACF_Identity:
A->KeyPos.Add(nullVec);
break;
default:
appError("Unknown translation compression method: %d (%s)", TranslationCompressionFormat, EnumToName(TranslationCompressionFormat));
}
}
// align to 4 bytes
Reader.Seek(Align(Reader.Tell(), 4));
if (HasTimeTracks)
ReadTimeArray(Reader, TransKeys, A->KeyPosTime, Seq->NumFrames);
}
else
{
// A->KeyPos.Add(nullVec);
// appNotify("No translation keys!");
}
