void UPhysicsConstraintComponent::UpdateConstraintFrames()
{
FTransform A1Transform = GetBodyTransform(EConstraintFrame::Frame1);
A1Transform.RemoveScaling();
FTransform A2Transform = GetBodyTransform(EConstraintFrame::Frame2);
A2Transform.RemoveScaling();
// World ref frame
const FVector WPos = GetComponentLocation();
const FVector WPri = ComponentToWorld.GetUnitAxis( EAxis::X );
const FVector WOrth = ComponentToWorld.GetUnitAxis( EAxis::Y );
ConstraintInstance.Pos1 = A1Transform.InverseTransformPosition(WPos);
ConstraintInstance.PriAxis1 = A1Transform.InverseTransformVectorNoScale(WPri);
ConstraintInstance.SecAxis1 = A1Transform.InverseTransformVectorNoScale(WOrth);
const FVector RotatedX = ConstraintInstance.AngularRotationOffset.RotateVector(FVector(1,0,0));
const FVector RotatedY = ConstraintInstance.AngularRotationOffset.RotateVector(FVector(0,1,0));
const FVector WPri2 = ComponentToWorld.TransformVectorNoScale(RotatedX);
const FVector WOrth2 = ComponentToWorld.TransformVectorNoScale(RotatedY);
ConstraintInstance.Pos2 = A2Transform.InverseTransformPosition(WPos);
ConstraintInstance.PriAxis2 = A2Transform.InverseTransformVectorNoScale(WPri2);
ConstraintInstance.SecAxis2 = A2Transform.InverseTransformVectorNoScale(WOrth2);
//Constraint instance is given our reference frame scale and uses it to scale position.
//Note that the scale passed in is also used for limits, so we first undo the position scale so that it's consistent.
//Note that in the case where there is no body instance, the position is given in world space and there is no scaling.
const float RefScale = FMath::Max(GetConstraintScale(), 0.01f);
if(GetBodyInstance(EConstraintFrame::Frame1))
{
ConstraintInstance.Pos1 /= RefScale;
}
if (GetBodyInstance(EConstraintFrame::Frame2))
{
ConstraintInstance.Pos2 /= RefScale;
}
}
void TSRODERigidBody::GetWorldTransform( TSRMatrix4& _worldTransform )
{
TSRMatrix4 m;
GetBodyTransform( m );
_worldTransform = m_BodyToWorldTransform * m;
}
