// NB: ElemTM is assumed to have no scaling in it!
void FKSphylElem::DrawElemWire(FPrimitiveDrawInterface* PDI, const FTransform& ElemTM, float Scale, const FColor Color)
{
FVector Origin = ElemTM.GetLocation();
FVector XAxis = ElemTM.GetScaledAxis( EAxis::X );
FVector YAxis = ElemTM.GetScaledAxis( EAxis::Y );
FVector ZAxis = ElemTM.GetScaledAxis( EAxis::Z );
// Draw top and bottom circles
FVector TopEnd = Origin + Scale*0.5f*Length*ZAxis;
FVector BottomEnd = Origin - Scale*0.5f*Length*ZAxis;
DrawCircle(PDI,TopEnd, XAxis, YAxis, Color, Scale*Radius, DrawCollisionSides, SDPG_World);
DrawCircle(PDI,BottomEnd, XAxis, YAxis, Color, Scale*Radius, DrawCollisionSides, SDPG_World);
// Draw domed caps
DrawHalfCircle(PDI, TopEnd, YAxis, ZAxis, Color,Scale* Radius);
DrawHalfCircle(PDI, TopEnd, XAxis, ZAxis, Color, Scale*Radius);
FVector NegZAxis = -ZAxis;
DrawHalfCircle(PDI, BottomEnd, YAxis, NegZAxis, Color, Scale*Radius);
DrawHalfCircle(PDI, BottomEnd, XAxis, NegZAxis, Color, Scale*Radius);
// Draw connecty lines
PDI->DrawLine(TopEnd + Scale*Radius*XAxis, BottomEnd + Scale*Radius*XAxis, Color, SDPG_World);
PDI->DrawLine(TopEnd - Scale*Radius*XAxis, BottomEnd - Scale*Radius*XAxis, Color, SDPG_World);
PDI->DrawLine(TopEnd + Scale*Radius*YAxis, BottomEnd + Scale*Radius*YAxis, Color, SDPG_World);
PDI->DrawLine(TopEnd - Scale*Radius*YAxis, BottomEnd - Scale*Radius*YAxis, Color, SDPG_World);
}
// NB: ElemTM is assumed to have no scaling in it!
void FKSphereElem::DrawElemWire(FPrimitiveDrawInterface* PDI, const FTransform& ElemTM, float Scale, const FColor Color)
{
FVector ElemCenter = ElemTM.GetLocation();
FVector X = ElemTM.GetScaledAxis( EAxis::X );
FVector Y = ElemTM.GetScaledAxis( EAxis::Y );
FVector Z = ElemTM.GetScaledAxis( EAxis::Z );
DrawCircle(PDI,ElemCenter, X, Y, Color, Scale*Radius, DrawCollisionSides, SDPG_World);
DrawCircle(PDI,ElemCenter, X, Z, Color, Scale*Radius, DrawCollisionSides, SDPG_World);
DrawCircle(PDI,ElemCenter, Y, Z, Color, Scale*Radius, DrawCollisionSides, SDPG_World);
}
FBox FKSphylElem::CalcAABB(const FTransform& BoneTM, float Scale) const
{
FTransform ElemTM = GetTransform();
ElemTM.ScaleTranslation( FVector(Scale) );
ElemTM *= BoneTM;
const FVector SphylCenter = ElemTM.GetLocation();
// Get sphyl axis direction
const FVector Axis = ElemTM.GetScaledAxis( EAxis::Z );
// Get abs of that vector
const FVector AbsAxis(FMath::Abs(Axis.X), FMath::Abs(Axis.Y), FMath::Abs(Axis.Z));
// Scale by length of sphyl
const FVector AbsDist = (Scale * 0.5f * Length) * AbsAxis;
const FVector MaxPos = SphylCenter + AbsDist;
const FVector MinPos = SphylCenter - AbsDist;
const FVector Extent(Scale * Radius);
FBox Result(MinPos - Extent, MaxPos + Extent);
return Result;
}
FQuat FAnimNode_RotationMultiplier::ExtractAngle(const FTransform& RefPoseTransform, const FTransform& LocalBoneTransform, const EBoneAxis Axis)
{
// local bone transform with reference rotation
FTransform ReferenceBoneTransform = RefPoseTransform;
ReferenceBoneTransform.SetTranslation(LocalBoneTransform.GetTranslation());
// find delta angle between the two quaternions X Axis.
const FVector RotationAxis = GetAxisVector(Axis);
const FVector LocalRotationVector = LocalBoneTransform.GetRotation().RotateVector(RotationAxis);
const FVector ReferenceRotationVector = ReferenceBoneTransform.GetRotation().RotateVector(RotationAxis);
const FQuat LocalToRefQuat = FQuat::FindBetween(LocalRotationVector, ReferenceRotationVector);
checkSlow( LocalToRefQuat.IsNormalized() );
// Rotate parent bone atom from position in local space to reference skeleton
// Since our rotation rotates both vectors with shortest arc
// we're essentially left with a quaternion that has angle difference with reference skeleton version
const FQuat BoneQuatAligned = LocalToRefQuat* LocalBoneTransform.GetRotation();
checkSlow( BoneQuatAligned.IsNormalized() );
// Find that delta angle
const FQuat DeltaQuat = (ReferenceBoneTransform.GetRotation().Inverse()) * BoneQuatAligned;
checkSlow( DeltaQuat.IsNormalized() );
#if 0 //DEBUG_TWISTBONECONTROLLER
UE_LOG(LogSkeletalControl, Log, TEXT("\t ExtractAngle, Bone: %s"),
*SourceBone.BoneName.ToString());
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t Bone Quat: %s, Rot: %s, AxisX: %s"), *LocalBoneTransform.GetRotation().ToString(), *LocalBoneTransform.GetRotation().Rotator().ToString(), *LocalRotationVector.ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t BoneRef Quat: %s, Rot: %s, AxisX: %s"), *ReferenceBoneTransform.GetRotation().ToString(), *ReferenceBoneTransform.GetRotation().Rotator().ToString(), *ReferenceRotationVector.ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t LocalToRefQuat Quat: %s, Rot: %s"), *LocalToRefQuat.ToString(), *LocalToRefQuat.Rotator().ToString() );
const FVector BoneQuatAlignedX = LocalBoneTransform.GetRotation().RotateVector(RotationAxis);
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t BoneQuatAligned Quat: %s, Rot: %s, AxisX: %s"), *BoneQuatAligned.ToString(), *BoneQuatAligned.Rotator().ToString(), *BoneQuatAlignedX.ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t DeltaQuat Quat: %s, Rot: %s"), *DeltaQuat.ToString(), *DeltaQuat.Rotator().ToString() );
FTransform BoneAtomAligned(BoneQuatAligned, ReferenceBoneTransform.GetTranslation());
const FQuat DeltaQuatAligned = FQuat::FindBetween(BoneAtomAligned.GetScaledAxis( EAxis::X ), ReferenceBoneTransform.GetScaledAxis( EAxis::X ));
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t DeltaQuatAligned Quat: %s, Rot: %s"), *DeltaQuatAligned.ToString(), *DeltaQuatAligned.Rotator().ToString() );
FVector DeltaAxis;
float DeltaAngle;
DeltaQuat.ToAxisAndAngle(DeltaAxis, DeltaAngle);
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t DeltaAxis: %s, DeltaAngle: %f"), *DeltaAxis.ToString(), DeltaAngle );
#endif
return DeltaQuat;
}
