9#include "Math/Matrix.h"
10#include "Components/CapsuleComponent.h"
11#include "DrawDebugHelpers.h"
12#include "Kismet/KismetMathLibrary.h"
15#include <unordered_set>
24#define SPRINGVEGETATIONLOGS 0
26#define COLLISIONLOGS 0
27#define ACCUMULATIONLOGS 0
28#define FICTITIOUSFORCELOGS 0
31#if SOLVERLOGS && SPRINGVEGETATIONLOGS
32#define SOLVER_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
34#define SOLVER_LOG(...)
36#if COLLISIONLOGS && SPRINGVEGETATIONLOGS
37#define COLLISION_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
39#define COLLISION_LOG(...)
41#if ACCUMULATIONLOGS && SPRINGVEGETATIONLOGS
42#define ACC_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
46#if FICTITIOUSFORCELOGS && SPRINGVEGETATIONLOGS
47#define FICT_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
51#if OTHERLOGS && SPRINGVEGETATIONLOGS
52#define OTHER_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
60 float HalfCircle = 180.f;
61 float FullCircle = 360.f;
62 auto GetDeltaAngle = [&](
float Angle1,
float Angle2) ->
float
66 Angle1 = FullCircle + Angle1;
70 Angle2 = FullCircle + Angle2;
72 float Diff = fmod( Angle1 - Angle2 + HalfCircle , FullCircle) - HalfCircle;
73 return Diff < -HalfCircle ? Diff + FullCircle : Diff;
77 GetDeltaAngle(Rotator1.Pitch, Rotator2.Pitch),
78 GetDeltaAngle(Rotator1.Yaw, Rotator2.Yaw),
79 GetDeltaAngle(Rotator1.Roll, Rotator2.Roll)
86 return n < 0.0f ? -1.0f : 1.0f;
93 return carla::rpc::ToFString(ss.str());
95static Eigen::Matrix3d
OuterProduct(
const Eigen::Vector3d& V1,
const Eigen::Vector3d& V2)
97 return V1 * V2.transpose();
101 return V1 * V1.transpose();
106 return Eigen::Vector3d(V1.X, V1.Y, -V1.Z);
110 return FVector(V1(0), V1(1), -V1(2));
115 Eigen::Matrix3d EigenMatrix;
116 EigenMatrix << Matrix.M[0][0], Matrix.M[0][1], -Matrix.M[0][2],
117 Matrix.M[1][0], Matrix.M[1][1], -Matrix.M[1][2],
118 Matrix.M[2][0], Matrix.M[2][1], -Matrix.M[2][2];
119 return EigenMatrix.transpose();
123 FMatrix Matrix = Transform.ToMatrixNoScale();
129 return Eigen::Vector3d(
130 FMath::DegreesToRadians(Rotator.Roll),
131 FMath::DegreesToRadians(Rotator.Pitch),
132 -FMath::DegreesToRadians(Rotator.Yaw));
137 FMath::RadiansToDegrees(Vector(1)),
138 -FMath::RadiansToDegrees(Vector(2)),
139 FMath::RadiansToDegrees(Vector(0)));
152 for (
int i = 1; i <
Joints.Num(); i++)
160 for (
int i = 0; i <
Joints.Num(); i++)
163 FString ChildrenIds =
"";
166 ChildrenIds += FString::FromInt(ChildrenId) +
", ";
168 OTHER_LOG(Warning,
"Joint %d, Children: %s.", i, *ChildrenIds);
176 for (
int i = 0; i <
Joints.Num(); i++)
187 std::vector<int> JointsToVisit;
188 JointsToVisit.emplace_back(0);
190 while (JointsToVisit.size())
193 JointsToVisit.pop_back();
198 JointsToVisit.emplace_back(ChildrenId);
204 FString IdOrder =
"";
205 FString NameOrder =
"";
210 IdOrder += FString::FromInt(Id) +
" ";
211 NameOrder +=
Joints[Id].JointName +
" ";
214 OTHER_LOG(Warning,
"Tree order: %s", *IdOrder);
215 OTHER_LOG(Warning,
"Tree order (names): %s", *NameOrder);
224 if(Joint.JointName == BoneName)
226 Joint.ExternalForces += Force;
235 Joint.ExternalForces = FVector(0,0,0);
239USpringBasedVegetationComponent::USpringBasedVegetationComponent(
const FObjectInitializer& ObjectInitializer)
240 : Super(ObjectInitializer)
242 PrimaryComponentTick.bCanEverTick =
true;
243 PrimaryComponentTick.bStartWithTickEnabled =
true;
246void USpringBasedVegetationComponent::GenerateSkeletonHierarchy()
248 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::GenerateSkeletonHierarchy);
249 OTHER_LOG(Warning,
"Get skeleton hierarchy");
253 UActorComponent* Component = GetOwner()->GetComponentByClass(USkeletalMeshComponent::StaticClass());
254 SkeletalMesh = Cast<USkeletalMeshComponent>(Component);
256 TArray<FName> BoneNames;
257 SkeletalMesh->GetBoneNames(BoneNames);
259 for (int32 i = 0; i < BoneNames.Num(); i++)
261 FName& BoneName = BoneNames[i];
262 FString BoneNameString = BoneName.ToString();
263 bool bIsFixedJoint = FixedJointsList.Contains(BoneNameString) ||
264 (DebugJointsToSimulate.Num() && !DebugJointsToSimulate.Contains(BoneNameString));
265 FName ParentBoneName = SkeletalMesh->GetParentBone(BoneName);
266 int32 ParentIndex = SkeletalMesh->GetBoneIndex(ParentBoneName);
267 Skeleton.Joints.Add(
FSkeletonJoint{i, ParentIndex, BoneNameString, bIsFixedJoint});
268 OTHER_LOG(Log,
"Added bone %s with id %d and parent %d", *BoneNameString, i, ParentIndex);
271 Skeleton.ComputeChildrenJointsAndBones();
272 Skeleton.ComputeEndJoints();
275 auto *AnimInst = SkeletalMesh->GetAnimInstance();
278 OTHER_LOG(Error,
"Could not get animation instance.");
281 UWalkerAnim *WalkerAnim = Cast<UWalkerAnim>(AnimInst);
284 OTHER_LOG(Error,
"Could not get UWalkerAnim.");
289 FPoseSnapshot TempSnapshot;
290 SkeletalMesh->SnapshotPose(TempSnapshot);
293 WalkerAnim->
Snap = TempSnapshot;
295 for (
int i=0; i<Skeleton.Joints.Num(); ++i)
298 FTransform JointTransform = SkeletalMesh->GetSocketTransform(FName(*Joint.
JointName), ERelativeTransformSpace::RTS_ParentBoneSpace);
305 FVector BoneCOM = Joint.
Transform.GetLocation()*0.5f;
306 float BoneLength = Joint.
Transform.GetLocation().Size();
307 ParentJoint.
Bones.Add({10, BoneLength, BoneCOM});
311 UpdateGlobalTransform();
314void USpringBasedVegetationComponent::BeginPlay()
316 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::BeginPlay);
318 OTHER_LOG(Warning,
"USpringBasedVegetationComponent::BeginPlay");
319 OTHER_LOG(Warning,
"Params: BaseSpringStrength %f, Num joints: %d, CollisionForceParameter %f", BaseSpringStrength, Skeleton.Joints.Num(), CollisionForceParameter);
322 UActorComponent* Component = GetOwner()->GetComponentByClass(USkeletalMeshComponent::StaticClass());
323 SkeletalMesh = Cast<USkeletalMeshComponent>(Component);
327 SetComponentTickEnabled(
false);
328 OTHER_LOG(Error,
"Could not find skeletal mesh component.");
339 SkeletalMesh->OnComponentHit.AddDynamic(
this, &USpringBasedVegetationComponent::OnCollisionEvent);
341 if (!Skeleton.Joints.Num())
343 GenerateSkeletonHierarchy();
346 UpdateGlobalTransform();
347 GenerateCollisionCapsules();
348 if(bAutoComputeStrength)
350 ComputeSpringStrengthForBranches();
353 JointCollisionList.resize(Skeleton.Joints.Num());
356void USpringBasedVegetationComponent::ResetComponent()
358 Skeleton.ClearExternalForces();
359 SkeletalMesh->ResetAllBodiesSimulatePhysics();
362void USpringBasedVegetationComponent::GenerateCollisionCapsules()
368 if (Bone.Length < MinBoneLength)
372 UCapsuleComponent* Capsule = NewObject<UCapsuleComponent>(GetOwner());
373 Capsule->AttachToComponent(SkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform, FName(*Joint.
JointName));
374 Capsule->RegisterComponent();
376 FRotator BoneRotation = UKismetMathLibrary::MakeRotFromZ(Bone.CenterOfMass.GetSafeNormal());
377 FTransform CapsuleTransform(BoneRotation, Bone.CenterOfMass, FVector(1,1,1));
378 Capsule->SetRelativeTransform(CapsuleTransform);
379 Capsule->SetCapsuleHalfHeight(Bone.Length*0.5f);
380 Capsule->SetCapsuleRadius(CapsuleRadius);
383 Capsule->SetGenerateOverlapEvents(
false);
384 Capsule->SetCollisionProfileName(
"BlockAll");
385 Capsule->OnComponentHit.AddDynamic(
this, &USpringBasedVegetationComponent::OnCollisionEvent);
389 Capsule->SetGenerateOverlapEvents(
true);
390 Capsule->SetCollisionProfileName(
"OverlapAll");
391 Capsule->OnComponentBeginOverlap.AddDynamic(
this, &USpringBasedVegetationComponent::OnBeginOverlapEvent);
392 Capsule->OnComponentEndOverlap.AddDynamic(
this, &USpringBasedVegetationComponent::OnEndOverlapEvent);
395 BoneCapsules.Add(Capsule);
396 CapsuleToJointId.Add(Capsule, Joint.
JointId);
401void USpringBasedVegetationComponent::ComputeSpringStrengthForBranches()
403 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeSpringStrengthForBranches);
404 FTransform RootTransform = Skeleton.GetRootJoint().GlobalTransform;
405 FVector RootLocation = RootTransform.GetLocation();
407 FVector TreeAxis = FVector(0,0,1);
411 FVector ClosestPoint;
412 float HorizontalDistance = FMath::PointDistToLine(JointLocation, TreeAxis, RootLocation, ClosestPoint);
413 float VerticalDistance = FVector::Distance(ClosestPoint, RootLocation);
416 BaseSpringStrength - HorizontalFallof*HorizontalDistance - VerticalFallof*VerticalDistance,
423void USpringBasedVegetationComponent::EndPlay(
const EEndPlayReason::Type EndPlayReason)
427 Capsule->DestroyComponent();
429 BoneCapsules.Empty();
433void USpringBasedVegetationComponent::ComputePerJointProperties(
434 std::vector<FJointProperties>& JointLocalPropertiesList,
435 std::vector<FJointProperties>& JointPropertiesList)
437 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputePerJointProperties);
444 if (!Joint.
Bones.Num())
449 Properties.
Mass += Bone.Mass;
450 FVector GlobalCenterOfMass = Joint.
GlobalTransform.TransformPositionNoScale(Bone.CenterOfMass);
465 float CylinderRadius = 0.1f;
466 float CylinderHeight = Bone.Length/100.f;
467 Eigen::Matrix3d LocalCylinderInertia;
468 LocalCylinderInertia <<
469 0.5 * Bone.Mass*CylinderRadius*CylinderRadius, 0.f, 0.f,
470 0.f, 1.f/12.f * Bone.Mass*(3*CylinderRadius*CylinderRadius + CylinderHeight*CylinderHeight), 0.f,
471 0.f, 0.f, 1.f/12.f * Bone.Mass*(3*CylinderRadius*CylinderRadius + CylinderHeight*CylinderHeight);
472 Eigen::Vector3d BoneVector =
ToEigenVector(Bone.CenterOfMass)/100.f;
473 Eigen::Vector3d LocalV1 = BoneVector.normalized();
474 Eigen::Vector3d LocalV2 = LocalV1.cross(Eigen::Vector3d(0,1,0));
475 if (LocalV2.norm() == 0)
476 LocalV2 = LocalV1.cross(Eigen::Vector3d(0,0,1));
478 Eigen::Vector3d LocalV3 = LocalV1.cross(LocalV2);
479 Eigen::Matrix3d LocalToJointMatrix;
480 LocalToJointMatrix << LocalV1, LocalV2, LocalV3;
481 Eigen::Matrix3d JointSpaceInertia = LocalToJointMatrix*LocalCylinderInertia*(LocalToJointMatrix.transpose());
491void USpringBasedVegetationComponent::ComputeCompositeBodyContribution(
492 std::vector<FJointProperties>& JointLocalPropertiesList,
493 std::vector<FJointProperties>& JointPropertiesList)
495 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeCompositeBodyContribution);
496 for (
int Id : Skeleton.EndToRootOrder)
507 Eigen::Vector3d CenterOfMass = JointLocalProperties.
Mass*JointLocalProperties.
CenterOfMass;
509 JointProperties.
Mass = JointLocalProperties.
Mass;
510 for(
int ChildrenId : Joint.ChildrenIds)
515 JointProperties.
Mass += ChildrenProperties.
Mass;
520 JointProperties.
Force = JointLocalProperties.
Force;
521 for(
int ChildrenId : Joint.ChildrenIds)
525 JointProperties.
Force += ChildrenProperties.
Force;
530 for(
int ChildrenId : Joint.ChildrenIds)
536 JointProperties.
Torque += ChildrenProperties.
Torque + (ChildrenJointGlobalPosition - ParentJointGlobalPosition).cross(ChildrenProperties.
Force);
541 for(
int ChildrenId : Joint.ChildrenIds)
553void USpringBasedVegetationComponent::ComputeFictitiousForces(
554 std::vector<FJointProperties>& JointLocalPropertiesList,
555 std::vector<FJointProperties>& JointPropertiesList)
557 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeFictitiousForces);
561 for (
int i = 1; i < Skeleton.RootToEndOrder.Num(); i++)
563 int Id = Skeleton.RootToEndOrder[i];
570 Eigen::Matrix3d GlobalToJointMatrix = JointToGlobalMatrix.transpose();
572 Eigen::Matrix3d GlobalToParentJointMatrix = ParentJointToGlobalMatrix.transpose();
579 + ParentProperties.
AngularVelocity.cross(JointGlobalPosition - ParentJointGlobalPosition);
584 + ParentProperties.
LinearAcceleration.cross(JointGlobalPosition - ParentJointGlobalPosition)
587 Eigen::Vector3d CompositeCenterOfMass = JointProperties.
CenterOfMass;
588 Eigen::Vector3d CompositeLinearVelocity = JointProperties.
LinearVelocity
589 + JointProperties.
AngularVelocity.cross(CompositeCenterOfMass - JointGlobalPosition);
595 Eigen::Vector3d TotalTorque = (CompositeCenterOfMass - JointGlobalPosition).cross(TotalForces);
602void USpringBasedVegetationComponent::ResolveContactsAndCollisions(
603 std::vector<FJointProperties>& JointLocalPropertiesList,
604 std::vector<FJointProperties>& JointPropertiesList)
606 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ResolveContactsAndCollisions);
609 for (
auto &Joint : JointCollisionList)
611 Joint.CanRest =
true;
614 for (
auto& ActorCapsules : OverlappingActors)
616 TRACE_CPUPROFILER_EVENT_SCOPE(ActorLoop);
617 AActor* CollidingActor = ActorCapsules.Key;
620 UPrimitiveComponent* Primitive = Cast<UPrimitiveComponent>(CollidingActor->GetRootComponent());
624 const FVector PrimitiveVelocity = Primitive->GetComponentVelocity();
625 const Eigen::Vector3d ColliderVelocity =
ToEigenVector(PrimitiveVelocity) / 100.f;
626 const FVector Impulse = (Primitive->GetMass() * PrimitiveVelocity);
627 const Eigen::Vector3d CollisionImpulse = 0*
ToEigenVector(Impulse) / 100.f;
628 TArray<UPrimitiveComponent*>& CollidingCapsules = ActorCapsules.Value;
631 TRACE_CPUPROFILER_EVENT_SCOPE(CapsuleLoop);
634 const FVector CapsuleLocation = Capsule->GetComponentLocation();
635 FVector PrimitiveLocation = Primitive->GetComponentLocation();
636 PrimitiveLocation = PrimitiveLocation + Primitive->GetUpVector()*VehicleCenterZOffset;
637 FVector ClosestPointOnCapsule;
638 float DistanceOnCapsule;
639 FHitResult HitResult;
641 FVector ClosestPointOnCollider;
642 float DistanceToCollider;
644 TRACE_CPUPROFILER_EVENT_SCOPE(ColliderPenetrationDistance);
645 DistanceOnCapsule = Capsule->GetClosestPointOnCollision(PrimitiveLocation, ClosestPointOnCapsule);
646 FVector LineDirection = (ClosestPointOnCapsule - PrimitiveLocation).GetSafeNormal();
647 FVector LineTraceStart = ClosestPointOnCapsule + LineTraceMaxDistance*LineDirection;
648 FVector LineTraceEnd = ClosestPointOnCapsule;
649 HitFound = Primitive->LineTraceComponent(HitResult,
650 LineTraceStart, LineTraceEnd, FCollisionQueryParams());
651 ClosestPointOnCollider = HitResult.Location;
652 DistanceToCollider = (ClosestPointOnCollider - ClosestPointOnCapsule).Size();
653 if (DebugEnableVisualization)
655 DrawDebugLine(GetWorld(), LineTraceStart, LineTraceEnd, FColor::Orange,
false, 0.1f, 0.0f, 1.f);
663 if (DebugEnableVisualization)
665 TRACE_CPUPROFILER_EVENT_SCOPE(DebugEnableVisualization);
666 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, ClosestPointOnCollider, FColor::Green,
false, 0.1f, 0.0f, 1.f);
667 static constexpr float DEBUG_SPHERE_SIZE = 5.0f;
668 DrawDebugSphere(GetWorld(), ClosestPointOnCapsule, DEBUG_SPHERE_SIZE, 64, FColor(255, 0, 255, 255));
669 DrawDebugSphere(GetWorld(), ClosestPointOnCollider, DEBUG_SPHERE_SIZE, 64, FColor(255, 0, 255, 255));
670 DrawDebugSphere(GetWorld(), CapsuleLocation, DEBUG_SPHERE_SIZE, 64, FColor(255, 255, 255, 255));
671 DrawDebugSphere(GetWorld(), PrimitiveLocation, DEBUG_SPHERE_SIZE, 64, FColor(255, 255, 255, 255));
674 const int JointId = CapsuleToJointId[Capsule];
679 const Eigen::Vector3d CapsulePosition =
ToEigenVector(CapsuleLocation) / 100.f;
680 const Eigen::Vector3d PointOnCapsulePosition =
ToEigenVector(ClosestPointOnCapsule) / 100.f;
681 const Eigen::Vector3d ColliderPosition =
ToEigenVector(ClosestPointOnCollider) / 100.f;
682 Eigen::Vector3d CollisionTorque = Eigen::Vector3d::Zero();
685 const FRotator CurrRotator = Joint.
Transform.Rotator();
687 const FRotator DeltaRotator =
690 const Eigen::Vector3d JointCapsuleVector = JointGlobalPosition - CapsulePosition;
691 const Eigen::Vector3d RepulsionForce = SpringTorque.cross(JointCapsuleVector) * JointCapsuleVector.squaredNorm();
693 FVector RepulsionForceUE = -
ToUnrealVector(RepulsionForce) * 100.f;
694 Primitive->AddForceAtLocation(RepulsionForceUE, ClosestPointOnCollider);
697 float ForceFactor = 1.f;
702 float ProportionalConstant = 1.f/(FMath::Pow(ForceMaxDistance, ForceDistanceFalloffExponent));
703 ForceFactor = ProportionalConstant * FMath::Pow(DistanceToCollider, ForceDistanceFalloffExponent);
704 ForceFactor = FMath::Clamp(ForceFactor, MinForceFactor, 1.f);
708 float Factor = 1.0f - ((JointCollision.
Iteration / 100.0f) * RestFactor);
709 const Eigen::Vector3d OverlappingForces = (ColliderPosition - PointOnCapsulePosition).normalized() * CollisionForceParameter * ForceFactor * Factor * Joint.
CollisionForceProportionalFactor;
710 Primitive->AddForceAtLocation(-
ToUnrealVector(OverlappingForces) * 100.f, ClosestPointOnCollider);
711 CollisionTorque += (PointOnCapsulePosition - JointGlobalPosition).cross(CollisionImpulse + OverlappingForces);
712 JointProperties.
Torque += CollisionTorque;
717 int TempId = JointId;
721 TempJointCollision.
CanRest =
false;
726 while (TempId != -1);
728 if (DebugEnableVisualization)
730 static constexpr float DEBUG_SPHERE_SIZE = 5.0f;
732 const FVector Start = Capsule->GetComponentLocation();
733 const FVector
End = Primitive->GetComponentLocation();
734 const FColor LineColor(FColor::Green);
735 DrawDebugLine(GetWorld(), Start, End, LineColor,
false, 0.1f, 0.0f, 1.f);
736 DrawDebugLine(GetWorld(), ClosestPointOnCollider, ClosestPointOnCollider+RepulsionForceUE.GetSafeNormal()*20.f, FColor::Red,
false, 0.1f, 0.0f, 1.f);
737 FVector UEOverlapForces =
ToUnrealVector(OverlappingForces)*100.f;
738 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, ClosestPointOnCapsule+UEOverlapForces.GetSafeNormal()*20.f, FColor::Turquoise,
false, 0.1f, 0.0f, 1.f);
740 DrawDebugSphere(GetWorld(), UECOM, DEBUG_SPHERE_SIZE, 64, FColor::Emerald);
742 DrawDebugSphere(GetWorld(), UEJointPos, DEBUG_SPHERE_SIZE, 64, FColor::Purple);
743 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, UEJointPos, FColor::Cyan,
false, 0.1f, 0.0f, 1.f);
749 for (
auto &Joint : JointCollisionList)
753 if (Joint.Iteration > 1)
760 if (Joint.Iteration <= 95)
762 Joint.Iteration += 5;
768void USpringBasedVegetationComponent::SolveEquationOfMotion(
769 std::vector<FJointProperties>& JointPropertiesList,
772 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::SolveEquationOfMotion);
784 if (DebugEnableVisualization)
789 FVector
End = Skeleton.Joints[Joint.
ParentId].GlobalTransform.GetLocation();
792 DrawDebugLine(GetWorld(), Start, End, FColor::Red,
false, 0.3f, 0.0f, 1.f);
796 DrawDebugLine(GetWorld(), Start, End, FColor::Blue,
false, 0.1f, 0.0f, 1.f);
801 float Mass = JointProperties.
Mass;
804 Eigen::Matrix3d JointSpaceIntertiaTensor = JointProperties.
Mass*
OuterProduct(CenterToJoint, CenterToJoint)
805 + GlobalToJointMatrix*JointProperties.
InertiaTensor*GlobalToJointMatrix.transpose();
808 Eigen::Matrix3d I = JointSpaceIntertiaTensor;
813 K << SpringStrength*SpringStrengthMulFactor.X,0.f,0.f,
814 0.f,SpringStrength*SpringStrengthMulFactor.Y,0.f,
815 0.f,0.f,SpringStrength*SpringStrengthMulFactor.Z;
816 Eigen::LLT<Eigen::Matrix3d> lltofI (I);
817 Eigen::Matrix3d L = lltofI.matrixL();
818 Eigen::Matrix3d Linv = L.inverse();
819 Eigen::EigenSolver<Eigen::Matrix3d> EigenDecomposition(Linv*K*Linv.transpose());
820 Eigen::Matrix3d Lambda = EigenDecomposition.eigenvalues().real().asDiagonal();
821 Eigen::Matrix3d X = EigenDecomposition.eigenvectors().real();
823 Eigen::Matrix3d U = Linv.transpose()*X;
825 Eigen::Matrix3d Uinv = X.transpose()*L.transpose();
826 Eigen::Vector3d Coeffsb = Eigen::Vector3d(alpha,alpha,alpha) + beta*Lambda*Eigen::Vector3d(1,1,1);
827 Eigen::Vector3d Coeffsk = Lambda*Eigen::Vector3d(1,1,1);
828 Eigen::Vector3d Coeffsf = U.transpose()*Torque;
833 FString StringGlobalToJoint =
EigenToFString(GlobalToJointMatrix);
835 SOLVER_LOG(Log,
"Bone %s: \n I: \n %s \n Tau: \n %s \n U: \n %s \n Lambda: \n %s \n X: \n %s \n GlobalToJoint \n %s \n CenterToJoint \n %s", *Joint.
JointName, *StringI, *StringTorque, *StringU, *StringLambda, *
EigenToFString(X), *StringGlobalToJoint, *StringCenterToJoint);
837 FRotator CurrRotator = Joint.
Transform.Rotator();
840 FRotator DeltaRotator =
844 float Factor = 1.0f - ((JointCollision.
Iteration / 100.0f) * RestFactor);
846 AngularVelocity *= Factor;
852 Eigen::Vector3d NewTheta (0.f,0.f,0.f);
853 Eigen::Vector3d NewThetaVelocity (0.f,0.f,0.f);
854 Eigen::Vector3d NewThetaAccel (0.f,0.f,0.f);
855 Eigen::Vector3d Discriminant = Coeffsb.cwiseProduct(Coeffsb) - 4*Coeffsk;
857 for (
int i = 0; i < 3; i++)
859 double b = Coeffsb(i);
860 double k = Coeffsk(i);
861 double f = Coeffsf(i);
862 double discriminant = Discriminant(i);
863 double theta0 = InitialTheta(i);
865 SOLVER_LOG(Log,
"Bone %s, component %d, b %f, k %f, f %f, discriminant %f, theta0 %f", *Joint.
JointName, i, b, k, f, discriminant, theta0);
868 NewTheta(i) = theta0;
871 double dtheta0 = InitialThetaVelocity(i);
872 double deltatheta = 0;
873 double angularvelocity = 0;
874 float angularaccel = 0;
875 if (discriminant > 0)
877 double r1 = (-b + FMath::Sqrt(discriminant))*0.5f;
878 double r2 = (-b - FMath::Sqrt(discriminant))*0.5f;
879 double c1 = (r2*(theta0 - f/k) - dtheta0)/(r2-r1);
880 double c2 = (dtheta0 - c1*r1)/r2;
881 deltatheta = c1*std::exp(r1*DeltaTime) + c2*std::exp(r2*DeltaTime) + f/k;
882 angularvelocity = c1*r1*std::exp(r1*DeltaTime) + c2*r2*std::exp(r2*DeltaTime);
883 SOLVER_LOG(Log,
"r1 %f, r2 %f, c1 %f, c2 %f, deltatheta %f", r1, r2, c1, c2, deltatheta);
885 else if (discriminant == 0)
888 double c1 = theta0 - f/k;
889 double c2 = dtheta0 - c1*r;
890 deltatheta = (c1 + c2*DeltaTime)*std::exp(r*DeltaTime) + f/k;
891 angularvelocity = (c1*r + c2 + c2*r*DeltaTime)*std::exp(r*DeltaTime);
892 SOLVER_LOG(Log,
"r %f, c1 %f, c2 %f, deltatheta %f", r, c1, c2, deltatheta);
896 double gamma = -b/2.f;
897 double mu = FMath::Sqrt(FMath::Abs(discriminant))/2.f;
898 double c1 = theta0 - f/k;
899 double c2 = (c1*gamma - dtheta0)/mu;
901 c1*std::exp(gamma*DeltaTime)*std::cos(mu*DeltaTime) +
902 c2*std::exp(gamma*DeltaTime)*std::sin(mu*DeltaTime) + f/k;
904 c1*std::exp(gamma*DeltaTime)*(gamma*std::cos(mu*DeltaTime) + mu*std::sin(mu*DeltaTime)) +
905 c2*std::exp(gamma*DeltaTime)*(gamma*std::sin(mu*DeltaTime) - mu*std::cos(mu*DeltaTime));
906 SOLVER_LOG(Log,
"gamma %f, mu %f, c1 %f, c2 %f, deltatheta %f", gamma, mu, c1, c2, deltatheta);
908 angularaccel = f - b*angularvelocity - k*deltatheta;
909 if (!FMath::IsNaN(deltatheta))
911 NewTheta(i) = deltatheta;
912 if (angularvelocity > 1e-4)
914 NewThetaVelocity(i) = angularvelocity;
916 if (angularaccel > 1e-2)
918 NewThetaAccel(i) = angularaccel;
922 Eigen::Vector3d FinalNewTheta = U*NewTheta;
923 Eigen::Vector3d FinalNewThetaVelocity = U*NewThetaVelocity;
924 Eigen::Vector3d FinalNewThetaAccel = U*NewThetaAccel;
926 auto NewPitch = FMath::RadiansToDegrees(FinalNewTheta(1));
927 auto NewYaw = FMath::RadiansToDegrees(FinalNewTheta(2));
928 auto NewRoll = FMath::RadiansToDegrees(FinalNewTheta(0));
934 RestRotator.Pitch + NewPitch,
935 RestRotator.Yaw - NewYaw,
936 RestRotator.Roll + NewRoll);
937 SOLVER_LOG(Log,
"FinalNewTheta \n %s \n FinalNewThetaVelocity \n %s \n FinalNewThetaAccel \n %s",
939 SOLVER_LOG(Log,
"New angle %s, new angular velocity %s, new angular accel %s",
940 *NewAngle.ToString(), *NewAngularVelocity.ToString(), *NewAngularAccel.ToString());
941 Joint.
Transform.SetRotation(NewAngle.Quaternion());
947void USpringBasedVegetationComponent::TickComponent(
949 enum ELevelTick TickType,
950 FActorComponentTickFunction * ThisTickFunction)
952 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::TickComponent);
953 Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
955 float DeltaTimeFinal = DeltaTime;
956 if (DeltaTimeOverride > 0)
957 DeltaTimeFinal = DeltaTimeOverride;
959 std::vector<FJointProperties> JointPropertiesList;
960 JointPropertiesList.resize(Skeleton.Joints.Num());
961 std::vector<FJointProperties> JointLocalPropertiesList;
962 JointLocalPropertiesList.resize(Skeleton.Joints.Num());
964 ComputePerJointProperties(JointLocalPropertiesList, JointPropertiesList);
966 ResolveContactsAndCollisions(JointLocalPropertiesList, JointPropertiesList);
968 ComputeCompositeBodyContribution(JointLocalPropertiesList, JointPropertiesList);
970 ComputeFictitiousForces(JointLocalPropertiesList, JointPropertiesList);
972 SolveEquationOfMotion(JointPropertiesList, DeltaTimeFinal);
974 UpdateSkeletalMesh();
975 UpdateGlobalTransform();
976 Skeleton.ClearExternalForces();
979void USpringBasedVegetationComponent::OnCollisionEvent(
983 FVector NormalImpulse,
984 const FHitResult& Hit)
987 if (OtherActor == GetOwner())
990 if(OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) !=
nullptr)
993 if (DebugEnableAllCollisions)
1000 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1004 COLLISION_LOG(LogCarla, Log, TEXT(
"Collision with bone %s, with impulse %s"), *Hit.MyBoneName.ToString(), *NormalImpulse.ToString());
1005 Skeleton.AddForce(Hit.MyBoneName.ToString(), NormalImpulse);
1008void USpringBasedVegetationComponent::OnBeginOverlapEvent(
1012 int32 OtherBodyIndex,
1014 const FHitResult& SweepResult)
1017 if (OtherActor == GetOwner())
1020 if(OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) !=
nullptr)
1023 if (DebugEnableAllCollisions)
1030 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1035 if (!OverlappingActors.Contains(OtherActor))
1037 OverlappingActors.Add(OtherActor);
1039 TArray<UPrimitiveComponent*>& OverlappingCapsules = OverlappingActors.FindOrAdd(OtherActor);
1040 if (!OverlappingCapsules.Contains(OverlapComponent))
1042 OverlappingCapsules.Add(OverlapComponent);
1046void USpringBasedVegetationComponent::OnEndOverlapEvent(
1050 int32 OtherBodyIndex)
1053 if (OtherActor == GetOwner())
1056 if(OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) !=
nullptr)
1059 if (DebugEnableAllCollisions)
1066 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1071 if (!OverlappingActors.Contains(OtherActor))
1073 TArray<UPrimitiveComponent*>& OverlappingCapsules = OverlappingActors.FindOrAdd(OtherActor);
1074 if (OverlappingCapsules.Contains(OverlapComponent))
1076 OverlappingCapsules.RemoveSingle(OverlapComponent);
1077 if (OverlappingCapsules.Num() == 0)
1079 OverlappingActors.Remove(OtherActor);
1084void USpringBasedVegetationComponent::UpdateSkeletalMesh()
1086 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::UpdateSkeletalMesh);
1088 auto *AnimInst = SkeletalMesh->GetAnimInstance();
1089 if (!AnimInst)
return;
1090 UWalkerAnim *WalkerAnim = Cast<UWalkerAnim>(AnimInst);
1091 if (!WalkerAnim)
return;
1094 if (WalkerAnim->
Snap.BoneNames.Num() == 0)
1097 SkeletalMesh->SnapshotPose(WalkerAnim->
Snap);
1100 TMap<FName, FTransform> JointsMap;
1101 for (
int i=0; i<Skeleton.Joints.Num(); ++i)
1104 FName JointName = FName(*Joint.
JointName);
1105 JointsMap.Add(JointName, Joint.
Transform);
1109 for (
int i=0; i<WalkerAnim->
Snap.BoneNames.Num(); ++i)
1111 FTransform *Trans = JointsMap.Find(WalkerAnim->
Snap.BoneNames[i]);
1114 WalkerAnim->
Snap.LocalTransforms[i] = *Trans;
1119void USpringBasedVegetationComponent::UpdateGlobalTransform()
1121 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::UpdateGlobalTransform);
1122 FTransform InitialTransform = SkeletalMesh->GetOwner()->GetActorTransform();
1126 for (
int i = 1; i < Skeleton.RootToEndOrder.Num(); i++)
1128 int Id = Skeleton.RootToEndOrder[i];
static Eigen::Vector3d ToEigenVector(const FVector &V1)
static FVector ToUnrealVector(const Eigen::Vector3d &V1)
static Eigen::Matrix3d OuterProduct(const Eigen::Vector3d &V1, const Eigen::Vector3d &V2)
#define COLLISION_LOG(...)
static FRotator EigenVectorToRotator(const Eigen::Vector3d &Vector)
FRotator GetDeltaRotator(const FRotator &Rotator1, const FRotator &Rotator2)
static Eigen::Matrix3d ToEigenMatrix(const FMatrix &Matrix)
static FString EigenToFString(T &t)
static Eigen::Vector3d RotatorToEigenVector(const FRotator &Rotator)
static bool IsValid(const ACarlaWheeledVehicle *Vehicle)
void SetParametersToComponent()
Base class for CARLA wheeled vehicles.
geom::Transform Transform
Eigen::Matrix3d InertiaTensor
Eigen::Vector3d LinearVelocity
Eigen::Vector3d AngularVelocity
Eigen::Matrix3d JointToGlobalMatrix
Eigen::Vector3d LinearAcceleration
Eigen::Vector3d FictitiousTorque
Eigen::Vector3d AngularAcceleration
Eigen::Vector3d CenterOfMass
TArray< int > RootToEndOrder
void ClearExternalForces()
void AddForce(const FString &BoneName, const FVector &Force)
TArray< FSkeletonJoint > Joints
TArray< int > EndToRootOrder
void ComputeChildrenJointsAndBones()
TArray< FSkeletonBone > Bones
FRotator AngularAcceleration
FTransform GlobalTransform
float CollisionForceProportionalFactor
FTransform GolbalInverseTransform
TArray< int > ChildrenIds