CARLA
Loading...
Searching...
No Matches
SpringBasedVegetationComponent.cpp
Go to the documentation of this file.
1// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
2// de Barcelona (UAB).
3//
4// This work is licensed under the terms of the MIT license.
5// For a copy, see <https://opensource.org/licenses/MIT>.
6
7#include "Carla/Vegetation/SpringBasedVegetationComponent.h"
8#include "Carla/Walker/WalkerAnim.h"
9
10#include "BaseVegetationActor.h"
11#include "Vehicle/CarlaWheeledVehicle.h"
12
13#include <util/disable-ue4-macros.h>
14#include <carla/rpc/String.h>
15#include <util/enable-ue4-macros.h>
16
17#include <util/ue-header-guard-begin.h>
18#include "Math/Matrix.h"
19#include "Components/CapsuleComponent.h"
20#include "DrawDebugHelpers.h"
21#include "Kismet/KismetMathLibrary.h"
22#include <util/ue-header-guard-end.h>
23
24#include <unordered_set>
25#include <vector>
26#include <cmath>
27#include <sstream>
28
29// disable warnings for eigen 3.1.0
30#if defined(__clang__)
31# pragma clang diagnostic push
32# pragma clang diagnostic ignored "-Wdeprecated-register"
33# pragma clang diagnostic ignored "-Wmisleading-indentation"
34# pragma clang diagnostic ignored "-Wint-in-bool-context"
35# pragma clang diagnostic ignored "-Wdeprecated-declarations"
36# pragma clang diagnostic ignored "-Wshadow"
37#endif
38#include <Eigen/Dense>
39#include <Eigen/Cholesky>
40#include <Eigen/Eigenvalues>
41#if defined(__clang__)
42# pragma clang diagnostic pop
43#endif
44
45#define SPRINGVEGETATIONLOGS 0
46#define SOLVERLOGS 0
47#define COLLISIONLOGS 0
48#define ACCUMULATIONLOGS 0
49#define FICTITIOUSFORCELOGS 0
50#define OTHERLOGS 0
51
52#if SOLVERLOGS && SPRINGVEGETATIONLOGS
53#define SOLVER_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
54#else
55#define SOLVER_LOG(...)
56#endif
57#if COLLISIONLOGS && SPRINGVEGETATIONLOGS
58#define COLLISION_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
59#else
60#define COLLISION_LOG(...)
61#endif
62#if ACCUMULATIONLOGS && SPRINGVEGETATIONLOGS
63#define ACC_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
64#else
65#define ACC_LOG(...)
66#endif
67#if FICTITIOUSFORCELOGS && SPRINGVEGETATIONLOGS
68#define FICT_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
69#else
70#define FICT_LOG(...)
71#endif
72#if OTHERLOGS && SPRINGVEGETATIONLOGS
73#define OTHER_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
74#else
75#define OTHER_LOG(...)
76#endif
77
78
79
80struct FJointProperties
81{
82 float Mass = 0.0;
83 Eigen::Matrix3d InertiaTensor = Eigen::Matrix3d::Zero();
84 Eigen::Vector3d Force = Eigen::Vector3d::Zero();
85 Eigen::Vector3d Torque = Eigen::Vector3d::Zero();
86 Eigen::Vector3d FictitiousTorque = Eigen::Vector3d::Zero();
87 Eigen::Vector3d CenterOfMass = Eigen::Vector3d::Zero();
88 Eigen::Matrix3d JointToGlobalMatrix = Eigen::Matrix3d::Zero();
89 Eigen::Vector3d AngularVelocity = Eigen::Vector3d::Zero();
90 Eigen::Vector3d LinearVelocity = Eigen::Vector3d::Zero();
91 Eigen::Vector3d AngularAcceleration = Eigen::Vector3d::Zero();
92 Eigen::Vector3d LinearAcceleration = Eigen::Vector3d::Zero();
93 Eigen::Vector3d LocalAngularAcceleration = Eigen::Vector3d::Zero();
94};
95
96FRotator GetDeltaRotator(const FRotator& Rotator1, const FRotator& Rotator2)
97{
98 float HalfCircle = 180.f;
99 float FullCircle = 360.f;
100 auto GetDeltaAngle = [&](float Angle1, float Angle2) -> float
101 {
102 if (Angle1 < 0)
103 {
104 Angle1 = FullCircle + Angle1;
105 }
106 if (Angle2 < 0)
107 {
108 Angle2 = FullCircle + Angle2;
109 }
110 float Diff = fmod(Angle1 - Angle2 + HalfCircle, FullCircle) - HalfCircle;
111 return Diff < -HalfCircle ? Diff + FullCircle : Diff;
112 };
113
114 return FRotator(
115 GetDeltaAngle(Rotator1.Pitch, Rotator2.Pitch),
116 GetDeltaAngle(Rotator1.Yaw, Rotator2.Yaw),
117 GetDeltaAngle(Rotator1.Roll, Rotator2.Roll)
118 );
119}
120
121template <class T>
122static T GetSign(T n)
123{
124 return n < 0.0f ? -1.0f : 1.0f;
125}
126template <class T>
127static FString EigenToFString(T& t)
128{
129 std::stringstream ss;
130 ss << t;
131 return carla::rpc::ToFString(ss.str());
132}
133static Eigen::Matrix3d OuterProduct(const Eigen::Vector3d& V1, const Eigen::Vector3d& V2)
134{
135 return V1 * V2.transpose();
136}
137static Eigen::Matrix3d OuterProduct(const Eigen::Vector3d& V1)
138{
139 return V1 * V1.transpose();
140}
141// convert to right handed frame by flipping z coordinate
142static Eigen::Vector3d ToEigenVector(const FVector& V1)
143{
144 return Eigen::Vector3d(V1.X, V1.Y, -V1.Z);
145}
146static FVector ToUnrealVector(const Eigen::Vector3d& V1)
147{
148 return FVector(V1(0), V1(1), -V1(2));
149}
150static Eigen::Matrix3d ToEigenMatrix(const FMatrix& Matrix) // Matrix[row][column]
151{
152 Eigen::Matrix3d EigenMatrix;
153 EigenMatrix << Matrix.M[0][0], Matrix.M[0][1], -Matrix.M[0][2],
154 Matrix.M[1][0], Matrix.M[1][1], -Matrix.M[1][2],
155 Matrix.M[2][0], Matrix.M[2][1], -Matrix.M[2][2];
156 return EigenMatrix.transpose();
157}
158static Eigen::Matrix3d ToEigenMatrix(const FTransform& Transform)
159{
160 FMatrix Matrix = Transform.ToMatrixNoScale(); // Matrix[row][column]
161 return ToEigenMatrix(Matrix);
162}
163static Eigen::Vector3d RotatorToEigenVector(const FRotator& Rotator)
164{
165 return Eigen::Vector3d(
166 FMath::DegreesToRadians(Rotator.Roll),
167 FMath::DegreesToRadians(Rotator.Pitch),
168 -FMath::DegreesToRadians(Rotator.Yaw));
169}
170static FRotator EigenVectorToRotator(const Eigen::Vector3d& Vector)
171{
172 return FRotator(
173 FMath::RadiansToDegrees(Vector(1)),
174 -FMath::RadiansToDegrees(Vector(2)),
175 FMath::RadiansToDegrees(Vector(0)));
176}
177
178void FSkeletonHierarchy::Clear()
179{
180 Joints.Empty();
181 EndJoints.Empty();
182 EndToRootOrder.Empty();
183 RootToEndOrder.Empty();
184}
185void FSkeletonHierarchy::ComputeChildrenJointsAndBones()
186{
187 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::ComputeChildrenJointsAndBones);
188 for (int i = 1; i < Joints.Num(); i++)
189 {
190 FSkeletonJoint& Joint = Joints[i];
191 FSkeletonJoint& ParentJoint = Joints[Joint.ParentId];
192 ParentJoint.ChildrenIds.Add(i);
193 }
194
195 // debug
196 for (int i = 0; i < Joints.Num(); i++)
197 {
198 FSkeletonJoint& Joint = Joints[i];
199 FString ChildrenIds = "";
200 for (int ChildrenId : Joint.ChildrenIds)
201 {
202 ChildrenIds += FString::FromInt(ChildrenId) + ", ";
203 }
204 OTHER_LOG(Warning, "Joint %d, Children: %s.", i, *ChildrenIds);
205 }
206}
207
208void FSkeletonHierarchy::ComputeEndJoints()
209{
210 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::ComputeEndJoints);
211 EndJoints.Empty();
212 for (int i = 0; i < Joints.Num(); i++)
213 {
214 FSkeletonJoint& Joint = Joints[i];
215 if (!Joint.ChildrenIds.Num())
216 {
217 EndJoints.Add(i);
218 }
219 }
220
221 // build traversal order so that parent nodes are visited before any children
222 RootToEndOrder.Empty();
223 std::vector<int> JointsToVisit;
224 JointsToVisit.emplace_back(0); // root element in the hierarchy
225 RootToEndOrder.Add(0);
226 while (JointsToVisit.size())
227 {
228 FSkeletonJoint& Joint = Joints[JointsToVisit.back()];
229 JointsToVisit.pop_back();
230
231 for (int ChildrenId : Joint.ChildrenIds)
232 {
233 RootToEndOrder.Add(ChildrenId);
234 JointsToVisit.emplace_back(ChildrenId);
235 }
236 }
237
238 // build the order in reverse, children visited before parents
239 EndToRootOrder.Empty();
240 FString IdOrder = "";
241 FString NameOrder = "";
242 for (int i = RootToEndOrder.Num() - 1; i >= 0; i--)
243 {
244 int Id = RootToEndOrder[i];
245 EndToRootOrder.Add(Id);
246 IdOrder += FString::FromInt(Id) + " ";
247 NameOrder += Joints[Id].JointName + " ";
248 }
249 //debug
250 OTHER_LOG(Warning, "Tree order: %s", *IdOrder);
251 OTHER_LOG(Warning, "Tree order (names): %s", *NameOrder);
252 OTHER_LOG(Warning, "Num elements: %d", EndToRootOrder.Num());
253}
254
255void FSkeletonHierarchy::AddForce(const FString& BoneName, const FVector& Force)
256{
257 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::AddForce);
258 for (FSkeletonJoint& Joint : Joints)
259 {
260 if (Joint.JointName == BoneName)
261 {
262 Joint.ExternalForces += Force;
263 }
264 }
265}
266void FSkeletonHierarchy::ClearExternalForces()
267{
268 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::ClearExternalForces);
269 for (FSkeletonJoint& Joint : Joints)
270 {
271 Joint.ExternalForces = FVector(0, 0, 0);
272 }
273}
274
275USpringBasedVegetationComponent::USpringBasedVegetationComponent(const FObjectInitializer& ObjectInitializer)
276 : Super(ObjectInitializer)
277{
278 PrimaryComponentTick.bCanEverTick = true;
279 PrimaryComponentTick.bStartWithTickEnabled = true;
280}
281
282void USpringBasedVegetationComponent::GenerateSkeletonHierarchy()
283{
284 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::GenerateSkeletonHierarchy);
285 OTHER_LOG(Warning, "Get skeleton hierarchy");
286 // Get skeleton hierarchy
287 if (!SkeletalMesh)
288 {
289 UActorComponent* Component = GetOwner()->GetComponentByClass(USkeletalMeshComponent::StaticClass());
290 SkeletalMesh = Cast<USkeletalMeshComponent>(Component);
291 }
292 TArray<FName> BoneNames;
293 SkeletalMesh->GetBoneNames(BoneNames);
294 Skeleton.Clear();
295 for (int32 i = 0; i < BoneNames.Num(); i++)
296 {
297 FName& BoneName = BoneNames[i];
298 FString BoneNameString = BoneName.ToString();
299 bool bIsFixedJoint = FixedJointsList.Contains(BoneNameString) ||
300 (DebugJointsToSimulate.Num() && !DebugJointsToSimulate.Contains(BoneNameString));
301 FName ParentBoneName = SkeletalMesh->GetParentBone(BoneName);
302 int32 ParentIndex = SkeletalMesh->GetBoneIndex(ParentBoneName);
303 Skeleton.Joints.Add(FSkeletonJoint{ i, ParentIndex, BoneNameString, bIsFixedJoint });
304 OTHER_LOG(Log, "Added bone %s with id %d and parent %d", *BoneNameString, i, ParentIndex);
305 }
306
307 Skeleton.ComputeChildrenJointsAndBones();
308 Skeleton.ComputeEndJoints();
309
310 // Get resting pose for bones
311 auto* AnimInst = SkeletalMesh->GetAnimInstance();
312 if (!AnimInst)
313 {
314 OTHER_LOG(Error, "Could not get animation instance.");
315 return;
316 }
317 UWalkerAnim* WalkerAnim = Cast<UWalkerAnim>(AnimInst);
318 if (!WalkerAnim)
319 {
320 OTHER_LOG(Error, "Could not get UWalkerAnim.");
321 return;
322 }
323
324 // get current pose
325 FPoseSnapshot TempSnapshot;
326 SkeletalMesh->SnapshotPose(TempSnapshot);
327
328 // copy pose
329 WalkerAnim->Snap = TempSnapshot;
330
331 for (int i = 0; i < Skeleton.Joints.Num(); ++i)
332 {
333 FSkeletonJoint& Joint = Skeleton.Joints[i];
334 FTransform JointTransform = SkeletalMesh->GetSocketTransform(FName(*Joint.JointName), ERelativeTransformSpace::RTS_ParentBoneSpace);
335 Joint.Transform = JointTransform;
336 Joint.RestingAngles = JointTransform.Rotator();
337 OTHER_LOG(Log, "Getting info for bone %s, %f, %f, %f, %f", *Joint.JointName, Joint.RestingAngles.Pitch, Joint.RestingAngles.Yaw, Joint.RestingAngles.Roll);
338 if (i > 0)
339 {
340 FSkeletonJoint& ParentJoint = Skeleton.Joints[Joint.ParentId];
341 FVector BoneCOM = Joint.Transform.GetLocation() * 0.5f;
342 float BoneLength = Joint.Transform.GetLocation().Size();
343 ParentJoint.Bones.Add({ 10, BoneLength, BoneCOM });
344 }
345 }
346
347 UpdateGlobalTransform();
348}
349
350void USpringBasedVegetationComponent::BeginPlay()
351{
352 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::BeginPlay);
353 Super::BeginPlay();
354 OTHER_LOG(Warning, "USpringBasedVegetationComponent::BeginPlay");
355 OTHER_LOG(Warning, "Params: BaseSpringStrength %f, Num joints: %d, CollisionForceParameter %f", BaseSpringStrength, Skeleton.Joints.Num(), CollisionForceParameter);
356 if (!SkeletalMesh)
357 {
358 UActorComponent* Component = GetOwner()->GetComponentByClass(USkeletalMeshComponent::StaticClass());
359 SkeletalMesh = Cast<USkeletalMeshComponent>(Component);
360 }
361 if (!SkeletalMesh)
362 {
363 SetComponentTickEnabled(false);
364 OTHER_LOG(Error, "Could not find skeletal mesh component.");
365 return;
366 }
367
368 ABaseVegetationActor* BaseVegetation = Cast<ABaseVegetationActor>(GetOwner());
369 if (BaseVegetation)
370 {
371 BaseVegetation->SetParametersToComponent();
372 }
373
374 // set callbacks
375 SkeletalMesh->OnComponentHit.AddDynamic(this, &USpringBasedVegetationComponent::OnCollisionEvent);
376
377 if (!Skeleton.Joints.Num())
378 {
379 GenerateSkeletonHierarchy();
380 }
381
382 UpdateGlobalTransform();
383 GenerateCollisionCapsules();
384 if (bAutoComputeStrength)
385 {
386 ComputeSpringStrengthForBranches();
387 }
388
389 JointCollisionList.resize(Skeleton.Joints.Num());
390}
391
392void USpringBasedVegetationComponent::ResetComponent()
393{
394 Skeleton.ClearExternalForces();
395 SkeletalMesh->ResetAllBodiesSimulatePhysics();
396}
397
398void USpringBasedVegetationComponent::GenerateCollisionCapsules()
399{
400 for (FSkeletonJoint& Joint : Skeleton.Joints)
401 {
402 for (FSkeletonBone& Bone : Joint.Bones)
403 {
404 if (Bone.Length < MinBoneLength)
405 {
406 continue;
407 }
408 UCapsuleComponent* Capsule = NewObject<UCapsuleComponent>(GetOwner());
409 Capsule->AttachToComponent(SkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform, FName(*Joint.JointName));
410 Capsule->RegisterComponent();
411 // create rotation from z direction to align the capsule
412 FRotator BoneRotation = UKismetMathLibrary::MakeRotFromZ(Bone.CenterOfMass.GetSafeNormal());
413 FTransform CapsuleTransform(BoneRotation, Bone.CenterOfMass, FVector(1, 1, 1));
414 Capsule->SetRelativeTransform(CapsuleTransform);
415 Capsule->SetCapsuleHalfHeight(Bone.Length * 0.5f);
416 Capsule->SetCapsuleRadius(CapsuleRadius);
417 if (Joint.bIsStatic)
418 {
419 Capsule->SetGenerateOverlapEvents(false);
420 Capsule->SetCollisionProfileName("BlockAll");
421 Capsule->OnComponentHit.AddDynamic(this, &USpringBasedVegetationComponent::OnCollisionEvent);
422 }
423 else
424 {
425 Capsule->SetGenerateOverlapEvents(true);
426 Capsule->SetCollisionProfileName("OverlapAll");
427 Capsule->OnComponentBeginOverlap.AddDynamic(this, &USpringBasedVegetationComponent::OnBeginOverlapEvent);
428 Capsule->OnComponentEndOverlap.AddDynamic(this, &USpringBasedVegetationComponent::OnEndOverlapEvent);
429 }
430
431 BoneCapsules.Add(Capsule);
432 CapsuleToJointId.Add(Capsule, Joint.JointId);
433 }
434 }
435}
436
437void USpringBasedVegetationComponent::ComputeSpringStrengthForBranches()
438{
439 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeSpringStrengthForBranches);
440 FTransform RootTransform = Skeleton.GetRootJoint().GlobalTransform;
441 FVector RootLocation = RootTransform.GetLocation();
442 // FVector TreeAxis = RootTransform.GetRotation().GetUpVector();
443 FVector TreeAxis = FVector(0, 0, 1);
444 for (FSkeletonJoint& Joint : Skeleton.Joints)
445 {
446 FVector JointLocation = Joint.GlobalTransform.GetLocation();
447 FVector ClosestPoint;
448 float HorizontalDistance = FMath::PointDistToLine(JointLocation, TreeAxis, RootLocation, ClosestPoint);
449 float VerticalDistance = FVector::Distance(ClosestPoint, RootLocation);
450
451 Joint.SpringStrength = FMath::Max(
452 BaseSpringStrength - HorizontalFallof * HorizontalDistance - VerticalFallof * VerticalDistance,
453 MinSpringStrength);
454
455 OTHER_LOG(Log, "Joint: %s, location %s, Strength %f", *Joint.JointName, *JointLocation.ToString(), Joint.SpringStrength);
456 }
457}
458
459void USpringBasedVegetationComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
460{
461 for (UPrimitiveComponent* Capsule : BoneCapsules)
462 {
463 Capsule->DestroyComponent();
464 }
465 BoneCapsules.Empty();
466}
467
468// Compute a single joint properties (Center of Mass, Inertia, Forces and Torque)
469void USpringBasedVegetationComponent::ComputePerJointProperties(
470 std::vector<FJointProperties>& JointLocalPropertiesList,
471 std::vector<FJointProperties>& JointPropertiesList)
472{
473 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputePerJointProperties);
474 for (FSkeletonJoint& Joint : Skeleton.Joints)
475 {
476 FJointProperties& Properties = JointLocalPropertiesList[Joint.JointId];
477 Eigen::Vector3d JointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation()) / 100.f;
478 Properties.JointToGlobalMatrix = ToEigenMatrix(Joint.GlobalTransform);
479 JointPropertiesList[Joint.JointId].JointToGlobalMatrix = Properties.JointToGlobalMatrix;
480 if (!Joint.Bones.Num())
481 continue;
482 // COM and mass
483 for (FSkeletonBone& Bone : Joint.Bones)
484 {
485 Properties.Mass += Bone.Mass;
486 FVector GlobalCenterOfMass = Joint.GlobalTransform.TransformPositionNoScale(Bone.CenterOfMass);
487 Properties.CenterOfMass += Bone.Mass * ToEigenVector(GlobalCenterOfMass) / 100.f;
488 }
489 Properties.CenterOfMass = Properties.CenterOfMass / Properties.Mass;
490
491 // force
492 Eigen::Vector3d GravityForce = ToEigenVector(Gravity) / 100.f; // world space gravity
493 Properties.Force = Properties.Mass * GravityForce + ToEigenVector(Joint.ExternalForces) / 100.f;
494 // torque
495 Properties.Torque = (Properties.CenterOfMass - JointGlobalPosition).cross(Properties.Force);
496 // inertia tensor
497 for (FSkeletonBone& Bone : Joint.Bones)
498 {
499 if (Bone.Length < 1)
500 continue;
501 float CylinderRadius = 0.1f;
502 float CylinderHeight = Bone.Length / 100.f;
503 Eigen::Matrix3d LocalCylinderInertia;
504 LocalCylinderInertia <<
505 0.5 * Bone.Mass * CylinderRadius * CylinderRadius, 0.f, 0.f,
506 0.f, 1.f / 12.f * Bone.Mass * (3 * CylinderRadius * CylinderRadius + CylinderHeight * CylinderHeight), 0.f,
507 0.f, 0.f, 1.f / 12.f * Bone.Mass * (3 * CylinderRadius * CylinderRadius + CylinderHeight * CylinderHeight);
508 Eigen::Vector3d BoneVector = ToEigenVector(Bone.CenterOfMass) / 100.f;
509 Eigen::Vector3d LocalV1 = BoneVector.normalized();
510 Eigen::Vector3d LocalV2 = LocalV1.cross(Eigen::Vector3d(0, 1, 0));
511 if (LocalV2.norm() == 0)
512 LocalV2 = LocalV1.cross(Eigen::Vector3d(0, 0, 1));
513 LocalV2.normalize();
514 Eigen::Vector3d LocalV3 = LocalV1.cross(LocalV2);
515 Eigen::Matrix3d LocalToJointMatrix;
516 LocalToJointMatrix << LocalV1, LocalV2, LocalV3;
517 Eigen::Matrix3d JointSpaceInertia = LocalToJointMatrix * LocalCylinderInertia * (LocalToJointMatrix.transpose());
518 Properties.InertiaTensor += Properties.JointToGlobalMatrix * JointSpaceInertia * Properties.JointToGlobalMatrix.transpose();
519 Eigen::Vector3d BoneCenterWorldSpace = ToEigenVector(Joint.GlobalTransform.TransformPositionNoScale(Bone.CenterOfMass)) / 100.0;
520 Properties.InertiaTensor += Properties.Mass * OuterProduct(Properties.CenterOfMass - BoneCenterWorldSpace, Properties.CenterOfMass - BoneCenterWorldSpace);
521 }
522 ACC_LOG(Log, "Local Joint: %s \n Inertia \n %s \n Force \n %s \n Torque \n %s \n COM: \n %s \n Mass %f", *Joint.JointName, *EigenToFString(Properties.InertiaTensor), *EigenToFString(Properties.Force), *EigenToFString(Properties.Torque), *EigenToFString(Properties.CenterOfMass), Properties.Mass);
523 }
524}
525
526// Compute accumulated properties (Center of Mass, Inertia, Forces and Torque)
527void USpringBasedVegetationComponent::ComputeCompositeBodyContribution(
528 std::vector<FJointProperties>& JointLocalPropertiesList,
529 std::vector<FJointProperties>& JointPropertiesList)
530{
531 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeCompositeBodyContribution);
532 for (int Id : Skeleton.EndToRootOrder)
533 {
534 FSkeletonJoint& Joint = Skeleton.Joints[Id];
535 FJointProperties& JointProperties = JointPropertiesList[Joint.JointId];
536 FJointProperties& JointLocalProperties = JointLocalPropertiesList[Joint.JointId];
537
538 if (!Joint.ChildrenIds.Num())
539 {
540 continue;
541 }
542
543 Eigen::Vector3d CenterOfMass = JointLocalProperties.Mass * JointLocalProperties.CenterOfMass;
544 // compute COM and accumulate mass
545 JointProperties.Mass = JointLocalProperties.Mass;
546 for (int ChildrenId : Joint.ChildrenIds)
547 {
548 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
549 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
550 CenterOfMass += ChildrenProperties.Mass * ChildrenProperties.CenterOfMass;
551 JointProperties.Mass += ChildrenProperties.Mass;
552 }
553 JointProperties.CenterOfMass = CenterOfMass / JointProperties.Mass;
554
555 // compute forces
556 JointProperties.Force = JointLocalProperties.Force;
557 for (int ChildrenId : Joint.ChildrenIds)
558 {
559 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
560 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
561 JointProperties.Force += ChildrenProperties.Force;
562 }
563
564 // compute torque
565 JointProperties.Torque = JointLocalProperties.Torque;
566 for (int ChildrenId : Joint.ChildrenIds)
567 {
568 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
569 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
570 Eigen::Vector3d ChildrenJointGlobalPosition = ToEigenVector(ChildrenJoint.GlobalTransform.GetLocation()) / 100.f;
571 Eigen::Vector3d ParentJointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation()) / 100.f;
572 JointProperties.Torque += ChildrenProperties.Torque + (ChildrenJointGlobalPosition - ParentJointGlobalPosition).cross(ChildrenProperties.Force);
573 }
574
575 // compute inertia tensor
576 JointProperties.InertiaTensor = JointLocalProperties.InertiaTensor + JointLocalProperties.Mass * OuterProduct(JointProperties.CenterOfMass - JointLocalProperties.CenterOfMass, JointProperties.CenterOfMass - JointLocalProperties.CenterOfMass);
577 for (int ChildrenId : Joint.ChildrenIds)
578 {
579 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
580 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
581 Eigen::Vector3d ChildrenToCenterOfMass = JointProperties.CenterOfMass - ChildrenProperties.CenterOfMass;
582
583 JointProperties.InertiaTensor += ChildrenProperties.InertiaTensor + ChildrenProperties.Mass * OuterProduct(ChildrenToCenterOfMass, ChildrenToCenterOfMass);
584 }
585 ACC_LOG(Log, "Accumulated Joint: %s \n Inertia \n %s \n Force \n %s \n Torque \n %s \n COM: \n %s \n Mass %f", *Joint.JointName, *EigenToFString(JointProperties.InertiaTensor), *EigenToFString(JointProperties.Force), *EigenToFString(JointProperties.Torque), *EigenToFString(JointProperties.CenterOfMass), JointProperties.Mass);
586 }
587}
588
589void USpringBasedVegetationComponent::ComputeFictitiousForces(
590 std::vector<FJointProperties>& JointLocalPropertiesList,
591 std::vector<FJointProperties>& JointPropertiesList)
592{
593 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeFictitiousForces);
594 // fictitious forces
595 FSkeletonJoint& RootJoint = Skeleton.Joints[0];
596 FJointProperties& RootProperties = JointPropertiesList[0];
597 for (int i = 1; i < Skeleton.RootToEndOrder.Num(); i++)
598 {
599 int Id = Skeleton.RootToEndOrder[i];
600 FSkeletonJoint& Joint = Skeleton.Joints[Id];
601 FJointProperties& JointProperties = JointPropertiesList[Joint.JointId];
602 FSkeletonJoint& ParentJoint = Skeleton.Joints[Joint.ParentId];
603 FJointProperties& ParentProperties = JointPropertiesList[Joint.ParentId];
604
605 Eigen::Matrix3d JointToGlobalMatrix = JointProperties.JointToGlobalMatrix;
606 Eigen::Matrix3d GlobalToJointMatrix = JointToGlobalMatrix.transpose();
607 Eigen::Matrix3d ParentJointToGlobalMatrix = ParentProperties.JointToGlobalMatrix;
608 Eigen::Matrix3d GlobalToParentJointMatrix = ParentJointToGlobalMatrix.transpose();
609 Eigen::Vector3d JointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation()) / 100.f;
610 Eigen::Vector3d ParentJointGlobalPosition = ToEigenVector(ParentJoint.GlobalTransform.GetLocation()) / 100.f;
611
612 JointProperties.AngularVelocity = ParentProperties.AngularVelocity
613 + ParentJointToGlobalMatrix * RotatorToEigenVector(ParentJoint.AngularVelocity);
614 JointProperties.LinearVelocity = ParentProperties.LinearVelocity
615 + ParentProperties.AngularVelocity.cross(JointGlobalPosition - ParentJointGlobalPosition);
616 JointProperties.AngularAcceleration = ParentProperties.AngularAcceleration
617 + ParentJointToGlobalMatrix * RotatorToEigenVector(ParentJoint.AngularAcceleration)
618 + ParentProperties.AngularVelocity.cross(JointProperties.AngularVelocity);
619 JointProperties.LinearAcceleration = ParentProperties.LinearAcceleration
620 + ParentProperties.LinearAcceleration.cross(JointGlobalPosition - ParentJointGlobalPosition)
621 + ParentProperties.AngularVelocity.cross(JointProperties.AngularVelocity - ParentProperties.AngularVelocity);
622
623 Eigen::Vector3d CompositeCenterOfMass = JointProperties.CenterOfMass;
624 Eigen::Vector3d CompositeLinearVelocity = JointProperties.LinearVelocity
625 + JointProperties.AngularVelocity.cross(CompositeCenterOfMass - JointGlobalPosition);
626 Eigen::Vector3d CompositeLinearAcceleration = JointProperties.LinearAcceleration
627 + JointProperties.AngularAcceleration.cross(CompositeCenterOfMass - JointGlobalPosition)
628 + JointProperties.AngularVelocity.cross(CompositeLinearVelocity - JointProperties.LinearVelocity);
629
630 Eigen::Vector3d TotalForces = -Joint.Mass * JointProperties.LinearAcceleration;
631 Eigen::Vector3d TotalTorque = (CompositeCenterOfMass - JointGlobalPosition).cross(TotalForces);
632 JointProperties.FictitiousTorque = GlobalToJointMatrix * TotalTorque;
633
634 FICT_LOG(Log, "Joint: %s \n Position \n %s \n Velocity \n %s \n Acceleration \n %s \n Fictitious forces: \n %s", *Joint.JointName, *EigenToFString(CompositeCenterOfMass), *EigenToFString(CompositeLinearVelocity), *EigenToFString(CompositeLinearAcceleration), *EigenToFString(JointProperties.FictitiousTorque));
635 }
636}
637
638void USpringBasedVegetationComponent::ResolveContactsAndCollisions(
639 std::vector<FJointProperties>& JointLocalPropertiesList,
640 std::vector<FJointProperties>& JointPropertiesList)
641{
642 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ResolveContactsAndCollisions);
643
644 // set all joints that can rest
645 for (auto& Joint : JointCollisionList)
646 {
647 Joint.CanRest = true;
648 }
649
650 for (auto& ActorCapsules : OverlappingActors)
651 {
652 TRACE_CPUPROFILER_EVENT_SCOPE(ActorLoop);
653 AActor* CollidingActor = ActorCapsules.Key;
654 if (!IsValid(CollidingActor))
655 continue;
656 UPrimitiveComponent* Primitive = Cast<UPrimitiveComponent>(CollidingActor->GetRootComponent());
657 if (!IsValid(Primitive))
658 continue;
659 // force transferring momentum (for the initial collision frame)
660 const FVector PrimitiveVelocity = Primitive->GetComponentVelocity();
661 const Eigen::Vector3d ColliderVelocity = ToEigenVector(PrimitiveVelocity) / 100.f;
662 const FVector Impulse = (Primitive->GetMass() * PrimitiveVelocity);
663 const Eigen::Vector3d CollisionImpulse = 0 * ToEigenVector(Impulse) / 100.f;
664 TArray<UPrimitiveComponent*>& CollidingCapsules = ActorCapsules.Value;
665 for (UPrimitiveComponent* Capsule : CollidingCapsules)
666 {
667 TRACE_CPUPROFILER_EVENT_SCOPE(CapsuleLoop);
668 if (!IsValid(Capsule))
669 continue;
670 const FVector CapsuleLocation = Capsule->GetComponentLocation();
671 FVector PrimitiveLocation = Primitive->GetComponentLocation();
672 PrimitiveLocation = PrimitiveLocation + Primitive->GetUpVector() * VehicleCenterZOffset;
673 FVector ClosestPointOnCapsule;
674 float DistanceOnCapsule;
675 FHitResult HitResult;
676 bool HitFound;
677 FVector ClosestPointOnCollider;
678 float DistanceToCollider;
679 {
680 TRACE_CPUPROFILER_EVENT_SCOPE(ColliderPenetrationDistance);
681 DistanceOnCapsule = Capsule->GetClosestPointOnCollision(PrimitiveLocation, ClosestPointOnCapsule);
682 FVector LineDirection = (ClosestPointOnCapsule - PrimitiveLocation).GetSafeNormal();
683 FVector LineTraceStart = ClosestPointOnCapsule + LineTraceMaxDistance * LineDirection;
684 FVector LineTraceEnd = ClosestPointOnCapsule;
685 HitFound = Primitive->LineTraceComponent(HitResult,
686 LineTraceStart, LineTraceEnd, FCollisionQueryParams());
687 ClosestPointOnCollider = HitResult.Location;
688 DistanceToCollider = (ClosestPointOnCollider - ClosestPointOnCapsule).Size();
689 if (DebugEnableVisualization)
690 {
691 DrawDebugLine(GetWorld(), LineTraceStart, LineTraceEnd, FColor::Orange, false, 0.1f, 0.0f, 1.f);
692 }
693 }
694 if (!HitFound)
695 {
696 continue;
697 }
698
699 if (DebugEnableVisualization)
700 {
701 TRACE_CPUPROFILER_EVENT_SCOPE(DebugEnableVisualization);
702 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, ClosestPointOnCollider, FColor::Green, false, 0.1f, 0.0f, 1.f);
703 static constexpr float DEBUG_SPHERE_SIZE = 5.0f;
704 DrawDebugSphere(GetWorld(), ClosestPointOnCapsule, DEBUG_SPHERE_SIZE, 64, FColor(255, 0, 255, 255));
705 DrawDebugSphere(GetWorld(), ClosestPointOnCollider, DEBUG_SPHERE_SIZE, 64, FColor(255, 0, 255, 255));
706 DrawDebugSphere(GetWorld(), CapsuleLocation, DEBUG_SPHERE_SIZE, 64, FColor(255, 255, 255, 255));
707 DrawDebugSphere(GetWorld(), PrimitiveLocation, DEBUG_SPHERE_SIZE, 64, FColor(255, 255, 255, 255));
708 }
709
710 const int JointId = CapsuleToJointId[Capsule];
711 const FSkeletonJoint& Joint = Skeleton.Joints[JointId];
712 FJointProperties& JointProperties = JointLocalPropertiesList[Joint.JointId];
713 FJointCollision& JointCollision = JointCollisionList[Joint.JointId];
714 const Eigen::Vector3d JointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation()) / 100.f;
715 const Eigen::Vector3d CapsulePosition = ToEigenVector(CapsuleLocation) / 100.f;
716 const Eigen::Vector3d PointOnCapsulePosition = ToEigenVector(ClosestPointOnCapsule) / 100.f;
717 const Eigen::Vector3d ColliderPosition = ToEigenVector(ClosestPointOnCollider) / 100.f;
718 Eigen::Vector3d CollisionTorque = Eigen::Vector3d::Zero();
719
720 // Contact forces due to spring strength
721 const FRotator CurrRotator = Joint.Transform.Rotator();
722 const FRotator RestRotator = Joint.RestingAngles;
723 const FRotator DeltaRotator =
724 GetDeltaRotator(CurrRotator, RestRotator);
725 const Eigen::Vector3d SpringTorque = Joint.SpringStrength * RotatorToEigenVector(DeltaRotator);
726 const Eigen::Vector3d JointCapsuleVector = JointGlobalPosition - CapsulePosition;
727 const Eigen::Vector3d RepulsionForce = SpringTorque.cross(JointCapsuleVector) * JointCapsuleVector.squaredNorm();
728
729 FVector RepulsionForceUE = -ToUnrealVector(RepulsionForce) * 100.f;
730 Primitive->AddForceAtLocation(RepulsionForceUE, ClosestPointOnCollider);
731
732 // force to repel geometry overlapping
733 float ForceFactor = 1.f;
734 // from eq f = 1 - a*d^p, f: ForceFactor, a: ProportionalConstant, p: ForceDistanceFalloffExponent, d: DistanceOnCapsule
735 // float ProportionalConstant = 1.f/(FMath::Pow(ForceMaxDistance, ForceDistanceFalloffExponent));
736 // ForceFactor = 1.f - ProportionalConstant * FMath::Pow(DistanceOnCapsule, ForceDistanceFalloffExponent);
737 // from eq f = a*d^p, f: ForceFactor, a: ProportionalConstant, p: ForceDistanceFalloffExponent, d: DistanceToCollider
738 float ProportionalConstant = 1.f / (FMath::Pow(ForceMaxDistance, ForceDistanceFalloffExponent));
739 ForceFactor = ProportionalConstant * FMath::Pow(DistanceToCollider, ForceDistanceFalloffExponent);
740 ForceFactor = FMath::Clamp(ForceFactor, MinForceFactor, 1.f);
741
742 // const Eigen::Vector3d OverlappingForces = (ColliderPosition - CapsulePosition).normalized() * CollisionForceParameter * ForceFactor;
743 // const Eigen::Vector3d OverlappingForces = (ColliderPosition - PointOnCapsulePosition).normalized() * CollisionForceParameter * ForceFactor;
744 float Factor = 1.0f - ((JointCollision.Iteration / 100.0f) * RestFactor);
745 const Eigen::Vector3d OverlappingForces = (ColliderPosition - PointOnCapsulePosition).normalized() * CollisionForceParameter * ForceFactor * Factor * Joint.CollisionForceProportionalFactor;
746 Primitive->AddForceAtLocation(-ToUnrealVector(OverlappingForces) * 100.f, ClosestPointOnCollider);
747 CollisionTorque += (PointOnCapsulePosition - JointGlobalPosition).cross(CollisionImpulse + OverlappingForces);
748 JointProperties.Torque += CollisionTorque;
749 // COLLISION_LOG(Log, "Joint: %s \n ProjectedSpeed %f, ProportionalFactor %f \n RepulsionForce %s \n", *Joint.JointName,ProjectedSpeed,ProportionalFactor,*EigenToFString(RepulsionForce),*EigenToFString(CollisionTorque));
750 //UE_LOG(LogCarla, Display, TEXT("DistanceToCollider: %f, ForceFactor: %f"), DistanceToCollider, ForceFactor);
751
752 // block forces to go to rest angles
753 int TempId = JointId;
754 do
755 {
756 FJointCollision& TempJointCollision = JointCollisionList[TempId];
757 TempJointCollision.CanRest = false;
758
759 FSkeletonJoint& TempJoint = Skeleton.Joints[TempId];
760 TempId = TempJoint.ParentId;
761 } while (TempId != -1);
762
763 if (DebugEnableVisualization)
764 {
765 static constexpr float DEBUG_SPHERE_SIZE = 5.0f;
766 // drawing
767 const FVector Start = Capsule->GetComponentLocation();
768 const FVector End = Primitive->GetComponentLocation();
769 const FColor LineColor(FColor::Green);
770 DrawDebugLine(GetWorld(), Start, End, LineColor, false, 0.1f, 0.0f, 1.f);
771 DrawDebugLine(GetWorld(), ClosestPointOnCollider, ClosestPointOnCollider + RepulsionForceUE.GetSafeNormal() * 20.f, FColor::Red, false, 0.1f, 0.0f, 1.f);
772 FVector UEOverlapForces = ToUnrealVector(OverlappingForces) * 100.f;
773 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, ClosestPointOnCapsule + UEOverlapForces.GetSafeNormal() * 20.f, FColor::Turquoise, false, 0.1f, 0.0f, 1.f);
774 FVector UECOM = ToUnrealVector(JointProperties.CenterOfMass) * 100.f;
775 DrawDebugSphere(GetWorld(), UECOM, DEBUG_SPHERE_SIZE, 64, FColor::Emerald);
776 FVector UEJointPos = ToUnrealVector(JointGlobalPosition) * 100.f;
777 DrawDebugSphere(GetWorld(), UEJointPos, DEBUG_SPHERE_SIZE, 64, FColor::Purple);
778 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, UEJointPos, FColor::Cyan, false, 0.1f, 0.0f, 1.f);
779 }
780 }
781 }
782
783 // reset iteration
784 for (auto& Joint : JointCollisionList)
785 {
786 if (Joint.CanRest)
787 {
788 if (Joint.Iteration > 1)
789 {
790 --Joint.Iteration;
791 }
792 }
793 else
794 {
795 if (Joint.Iteration <= 95)
796 {
797 Joint.Iteration += 5;
798 }
799 }
800 }
801}
802
803void USpringBasedVegetationComponent::SolveEquationOfMotion(
804 std::vector<FJointProperties>& JointPropertiesList,
805 float DeltaTime)
806{
807 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::SolveEquationOfMotion);
808 // solver
809 for (FSkeletonJoint& Joint : Skeleton.Joints)
810 {
811 if (!Joint.Bones.Num() || Joint.bIsStatic)
812 {
813 continue;
814 }
815 FJointProperties& JointProperties = JointPropertiesList[Joint.JointId];
816 FJointCollision& JointCollision = JointCollisionList[Joint.JointId];
817
818 // debug drawing
819 if (DebugEnableVisualization)
820 {
821 if (Joint.ParentId != -1)
822 {
823 FVector Start = Joint.GlobalTransform.GetLocation();
824 FVector End = Skeleton.Joints[Joint.ParentId].GlobalTransform.GetLocation();
825 if (!JointCollision.CanRest)
826 {
827 DrawDebugLine(GetWorld(), Start, End, FColor::Red, false, 0.3f, 0.0f, 1.f);
828 }
829 else
830 {
831 DrawDebugLine(GetWorld(), Start, End, FColor::Blue, false, 0.1f, 0.0f, 1.f);
832 }
833 }
834 }
835
836 float Mass = JointProperties.Mass;
837 Eigen::Vector3d CenterToJoint = JointProperties.CenterOfMass - ToEigenVector(Joint.GlobalTransform.GetLocation()) / 100.f;
838 Eigen::Matrix3d GlobalToJointMatrix = JointProperties.JointToGlobalMatrix.transpose();
839 Eigen::Matrix3d JointSpaceIntertiaTensor = JointProperties.Mass * OuterProduct(CenterToJoint, CenterToJoint)
840 + GlobalToJointMatrix * JointProperties.InertiaTensor * GlobalToJointMatrix.transpose();
841 Eigen::Vector3d Torque = GlobalToJointMatrix * JointProperties.Torque + JointProperties.FictitiousTorque;
842 // build differential equation
843 Eigen::Matrix3d I = JointSpaceIntertiaTensor;
844 float SpringStrength = Joint.SpringStrength;
845 float beta = Beta;
846 float alpha = Alpha;
847 Eigen::Matrix3d K;
848 K << SpringStrength * SpringStrengthMulFactor.X, 0.f, 0.f,
849 0.f, SpringStrength* SpringStrengthMulFactor.Y, 0.f,
850 0.f, 0.f, SpringStrength* SpringStrengthMulFactor.Z;
851 Eigen::LLT<Eigen::Matrix3d> lltofI(I);
852 Eigen::Matrix3d L = lltofI.matrixL();
853 Eigen::Matrix3d Linv = L.inverse();
854 Eigen::EigenSolver<Eigen::Matrix3d> EigenDecomposition(Linv * K * Linv.transpose());
855 Eigen::Matrix3d Lambda = EigenDecomposition.eigenvalues().real().asDiagonal();
856 Eigen::Matrix3d X = EigenDecomposition.eigenvectors().real();
857 // Eigen::Matrix3d Lambda = U.transpose()*K*U;
858 Eigen::Matrix3d U = Linv.transpose() * X;
859 // Eigen::Matrix3d Uinv = U.inverse();
860 Eigen::Matrix3d Uinv = X.transpose() * L.transpose();
861 Eigen::Vector3d Coeffsb = Eigen::Vector3d(alpha, alpha, alpha) + beta * Lambda * Eigen::Vector3d(1, 1, 1);
862 Eigen::Vector3d Coeffsk = Lambda * Eigen::Vector3d(1, 1, 1);
863 Eigen::Vector3d Coeffsf = U.transpose() * Torque;
864 FString StringI = EigenToFString(I);
865 FString StringTorque = EigenToFString(Torque);
866 FString StringU = EigenToFString(U);
867 FString StringLambda = EigenToFString(Lambda);
868 FString StringGlobalToJoint = EigenToFString(GlobalToJointMatrix);
869 FString StringCenterToJoint = EigenToFString(CenterToJoint);
870 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);
871
872 FRotator CurrRotator = Joint.Transform.Rotator();
873 FRotator RestRotator = Joint.RestingAngles;
874 FRotator AngularVelocity = Joint.AngularVelocity;
875 FRotator DeltaRotator =
876 GetDeltaRotator(CurrRotator, RestRotator);
877 if (!JointCollision.CanRest)
878 {
879 float Factor = 1.0f - ((JointCollision.Iteration / 100.0f) * RestFactor);
880 // DeltaRotator *= Factor;
881 AngularVelocity *= Factor;
882 }
883 Eigen::Vector3d InitialTheta = Uinv * RotatorToEigenVector(DeltaRotator);
884 Eigen::Vector3d InitialThetaVelocity = Uinv * RotatorToEigenVector(AngularVelocity);
885 SOLVER_LOG(Log, "Old angle for joint %s, %s", *Joint.JointName, *CurrRotator.ToString());
886 SOLVER_LOG(Log, "InitialTheta \n %s", *EigenToFString(InitialTheta));
887 Eigen::Vector3d NewTheta(0.f, 0.f, 0.f);
888 Eigen::Vector3d NewThetaVelocity(0.f, 0.f, 0.f);
889 Eigen::Vector3d NewThetaAccel(0.f, 0.f, 0.f);
890 Eigen::Vector3d Discriminant = Coeffsb.cwiseProduct(Coeffsb) - 4 * Coeffsk;
891 // 1st row
892 for (int i = 0; i < 3; i++)
893 {
894 double b = Coeffsb(i);
895 double k = Coeffsk(i);
896 double f = Coeffsf(i);
897 double discriminant = Discriminant(i);
898 double theta0 = InitialTheta(i);
899
900 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);
901 if (k == 0)
902 {
903 NewTheta(i) = theta0;
904 SOLVER_LOG(Log, "In Bone %s, K is 0 in component %d", *Joint.JointName, i);
905 }
906 double dtheta0 = InitialThetaVelocity(i); // compute
907 double deltatheta = 0;
908 double angularvelocity = 0;
909 float angularaccel = 0;
910 if (discriminant > 0)
911 {
912 double r1 = (-b + FMath::Sqrt(discriminant)) * 0.5f;
913 double r2 = (-b - FMath::Sqrt(discriminant)) * 0.5f;
914 double c1 = (r2 * (theta0 - f / k) - dtheta0) / (r2 - r1);
915 double c2 = (dtheta0 - c1 * r1) / r2;
916 deltatheta = c1 * std::exp(r1 * DeltaTime) + c2 * std::exp(r2 * DeltaTime) + f / k;
917 angularvelocity = c1 * r1 * std::exp(r1 * DeltaTime) + c2 * r2 * std::exp(r2 * DeltaTime);
918 SOLVER_LOG(Log, "r1 %f, r2 %f, c1 %f, c2 %f, deltatheta %f", r1, r2, c1, c2, deltatheta);
919 }
920 else if (discriminant == 0)
921 {
922 double r = -b / 2.f;
923 double c1 = theta0 - f / k;
924 double c2 = dtheta0 - c1 * r;
925 deltatheta = (c1 + c2 * DeltaTime) * std::exp(r * DeltaTime) + f / k;
926 angularvelocity = (c1 * r + c2 + c2 * r * DeltaTime) * std::exp(r * DeltaTime);
927 SOLVER_LOG(Log, "r %f, c1 %f, c2 %f, deltatheta %f", r, c1, c2, deltatheta);
928 }
929 else
930 {
931 double gamma = -b / 2.f;
932 double mu = FMath::Sqrt(FMath::Abs(discriminant)) / 2.f;
933 double c1 = theta0 - f / k;
934 double c2 = (c1 * gamma - dtheta0) / mu;
935 deltatheta =
936 c1 * std::exp(gamma * DeltaTime) * std::cos(mu * DeltaTime) +
937 c2 * std::exp(gamma * DeltaTime) * std::sin(mu * DeltaTime) + f / k;
938 angularvelocity =
939 c1 * std::exp(gamma * DeltaTime) * (gamma * std::cos(mu * DeltaTime) + mu * std::sin(mu * DeltaTime)) +
940 c2 * std::exp(gamma * DeltaTime) * (gamma * std::sin(mu * DeltaTime) - mu * std::cos(mu * DeltaTime));
941 SOLVER_LOG(Log, "gamma %f, mu %f, c1 %f, c2 %f, deltatheta %f", gamma, mu, c1, c2, deltatheta);
942 }
943 angularaccel = f - b * angularvelocity - k * deltatheta;
944 if (!FMath::IsNaN(deltatheta))
945 {
946 NewTheta(i) = deltatheta;
947 if (angularvelocity > 1e-4)
948 {
949 NewThetaVelocity(i) = angularvelocity;
950 }
951 if (angularaccel > 1e-2)
952 {
953 NewThetaAccel(i) = angularaccel;
954 }
955 }
956 }
957 Eigen::Vector3d FinalNewTheta = U * NewTheta;
958 Eigen::Vector3d FinalNewThetaVelocity = U * NewThetaVelocity;
959 Eigen::Vector3d FinalNewThetaAccel = U * NewThetaAccel;
960
961 auto NewPitch = FMath::RadiansToDegrees(FinalNewTheta(1));
962 auto NewYaw = FMath::RadiansToDegrees(FinalNewTheta(2));
963 auto NewRoll = FMath::RadiansToDegrees(FinalNewTheta(0));
964
965 FRotator NewAngularVelocity = EigenVectorToRotator(FinalNewThetaVelocity);
966 FRotator NewAngularAccel = EigenVectorToRotator(FinalNewThetaAccel);
967
968 FRotator NewAngle(
969 RestRotator.Pitch + NewPitch,
970 RestRotator.Yaw - NewYaw,
971 RestRotator.Roll + NewRoll);
972 SOLVER_LOG(Log, "FinalNewTheta \n %s \n FinalNewThetaVelocity \n %s \n FinalNewThetaAccel \n %s",
973 *EigenToFString(FinalNewTheta), *EigenToFString(FinalNewThetaVelocity), *EigenToFString(FinalNewThetaAccel));
974 SOLVER_LOG(Log, "New angle %s, new angular velocity %s, new angular accel %s",
975 *NewAngle.ToString(), *NewAngularVelocity.ToString(), *NewAngularAccel.ToString());
976 Joint.Transform.SetRotation(NewAngle.Quaternion());
977 Joint.AngularVelocity = NewAngularVelocity;
978 Joint.AngularAcceleration = NewAngularAccel;
979 }
980}
981
982void USpringBasedVegetationComponent::TickComponent(
983 float DeltaTime,
984 enum ELevelTick TickType,
985 FActorComponentTickFunction* ThisTickFunction)
986{
987 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::TickComponent);
988 Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
989
990 float DeltaTimeFinal = DeltaTime;
991 if (DeltaTimeOverride > 0)
992 DeltaTimeFinal = DeltaTimeOverride;
993
994 std::vector<FJointProperties> JointPropertiesList;
995 JointPropertiesList.resize(Skeleton.Joints.Num());
996 std::vector<FJointProperties> JointLocalPropertiesList;
997 JointLocalPropertiesList.resize(Skeleton.Joints.Num());
998
999 ComputePerJointProperties(JointLocalPropertiesList, JointPropertiesList);
1000
1001 ResolveContactsAndCollisions(JointLocalPropertiesList, JointPropertiesList);
1002
1003 ComputeCompositeBodyContribution(JointLocalPropertiesList, JointPropertiesList);
1004
1005 ComputeFictitiousForces(JointLocalPropertiesList, JointPropertiesList);
1006
1007 SolveEquationOfMotion(JointPropertiesList, DeltaTimeFinal);
1008
1009 UpdateSkeletalMesh();
1010 UpdateGlobalTransform();
1011 Skeleton.ClearExternalForces();
1012}
1013
1014void USpringBasedVegetationComponent::OnCollisionEvent(
1015 UPrimitiveComponent* HitComponent,
1016 AActor* OtherActor,
1017 UPrimitiveComponent* OtherComponent,
1018 FVector NormalImpulse,
1019 const FHitResult& Hit)
1020{
1021 // prevent self collision
1022 if (OtherActor == GetOwner())
1023 return;
1024 // prevent collision with other tree actors
1025 if (OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) != nullptr)
1026 return;
1027 ACarlaWheeledVehicle* Vehicle = nullptr;
1028 if (DebugEnableAllCollisions)
1029 {
1030 if (!IsValid(OtherActor))
1031 return;
1032 }
1033 else
1034 {
1035 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1036 if (!IsValid(Vehicle))
1037 return;
1038 }
1039 COLLISION_LOG(LogCarla, Log, TEXT("Collision with bone %s, with impulse %s"), *Hit.MyBoneName.ToString(), *NormalImpulse.ToString());
1040 Skeleton.AddForce(Hit.MyBoneName.ToString(), NormalImpulse);
1041}
1042
1043void USpringBasedVegetationComponent::OnBeginOverlapEvent(
1044 UPrimitiveComponent* OverlapComponent,
1045 AActor* OtherActor,
1046 UPrimitiveComponent* OtherComponent,
1047 int32 OtherBodyIndex,
1048 bool bFromSweep,
1049 const FHitResult& SweepResult)
1050{
1051 // prevent self collision
1052 if (OtherActor == GetOwner())
1053 return;
1054 // prevent collision with other tree actors
1055 if (OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) != nullptr)
1056 return;
1057 ACarlaWheeledVehicle* Vehicle = nullptr;
1058 if (DebugEnableAllCollisions)
1059 {
1060 if (!IsValid(OtherActor))
1061 return;
1062 }
1063 else
1064 {
1065 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1066 if (!IsValid(Vehicle))
1067 return;
1068 }
1069
1070 if (!OverlappingActors.Contains(OtherActor))
1071 {
1072 OverlappingActors.Add(OtherActor);
1073 }
1074 TArray<UPrimitiveComponent*>& OverlappingCapsules = OverlappingActors.FindOrAdd(OtherActor);
1075 if (!OverlappingCapsules.Contains(OverlapComponent))
1076 {
1077 OverlappingCapsules.Add(OverlapComponent);
1078 }
1079}
1080
1081void USpringBasedVegetationComponent::OnEndOverlapEvent(
1082 UPrimitiveComponent* OverlapComponent,
1083 AActor* OtherActor,
1084 UPrimitiveComponent* OtherComponent,
1085 int32 OtherBodyIndex)
1086{
1087 // prevent self collision
1088 if (OtherActor == GetOwner())
1089 return;
1090 // prevent collision with other tree actors
1091 if (OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) != nullptr)
1092 return;
1093 ACarlaWheeledVehicle* Vehicle = nullptr;
1094 if (DebugEnableAllCollisions)
1095 {
1096 if (!IsValid(OtherActor))
1097 return;
1098 }
1099 else
1100 {
1101 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1102 if (!IsValid(Vehicle))
1103 return;
1104 }
1105
1106 if (!OverlappingActors.Contains(OtherActor))
1107 return;
1108 TArray<UPrimitiveComponent*>& OverlappingCapsules = OverlappingActors.FindOrAdd(OtherActor);
1109 if (OverlappingCapsules.Contains(OverlapComponent))
1110 {
1111 OverlappingCapsules.RemoveSingle(OverlapComponent);
1112 if (OverlappingCapsules.Num() == 0)
1113 {
1114 OverlappingActors.Remove(OtherActor);
1115 }
1116 }
1117}
1118
1119void USpringBasedVegetationComponent::UpdateSkeletalMesh()
1120{
1121 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::UpdateSkeletalMesh);
1122 // get the walker animation class
1123 auto* AnimInst = SkeletalMesh->GetAnimInstance();
1124 if (!AnimInst) return;
1125 UWalkerAnim* WalkerAnim = Cast<UWalkerAnim>(AnimInst);
1126 if (!WalkerAnim) return;
1127
1128 // if pose is empty, then get a first version
1129 if (WalkerAnim->Snap.BoneNames.Num() == 0)
1130 {
1131 // get current pose
1132 SkeletalMesh->SnapshotPose(WalkerAnim->Snap);
1133 }
1134
1135 TMap<FName, FTransform> JointsMap;
1136 for (int i = 0; i < Skeleton.Joints.Num(); ++i)
1137 {
1138 FSkeletonJoint& Joint = Skeleton.Joints[i];
1139 FName JointName = FName(*Joint.JointName);
1140 JointsMap.Add(JointName, Joint.Transform);
1141 }
1142
1143 // assign common bones
1144 for (int i = 0; i < WalkerAnim->Snap.BoneNames.Num(); ++i)
1145 {
1146 FTransform* Trans = JointsMap.Find(WalkerAnim->Snap.BoneNames[i]);
1147 if (Trans)
1148 {
1149 WalkerAnim->Snap.LocalTransforms[i] = *Trans;
1150 }
1151 }
1152}
1153
1154void USpringBasedVegetationComponent::UpdateGlobalTransform()
1155{
1156 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::UpdateGlobalTransform);
1157 FTransform InitialTransform = SkeletalMesh->GetOwner()->GetActorTransform();
1158 FSkeletonJoint& RootJoint = Skeleton.Joints[0];
1159 RootJoint.GlobalTransform = RootJoint.Transform * InitialTransform;
1160 RootJoint.GlobalInverseTransform = RootJoint.GlobalTransform.Inverse();
1161 for (int i = 1; i < Skeleton.RootToEndOrder.Num(); i++)
1162 {
1163 int Id = Skeleton.RootToEndOrder[i];
1164 FSkeletonJoint& Joint = Skeleton.Joints[Id];
1165 FSkeletonJoint& ParentJoint = Skeleton.Joints[Joint.ParentId];
1166 FTransform Transform = Joint.Transform * ParentJoint.GlobalTransform;
1167 Joint.GlobalTransform = Transform;
1168 Joint.GlobalInverseTransform = Transform.Inverse();
1169 }
1170}
ToEigenVector
static Eigen::Vector3d ToEigenVector(const FVector &V1)
Definition SpringBasedVegetationComponent.cpp:142
SOLVER_LOG
#define SOLVER_LOG(...)
Definition SpringBasedVegetationComponent.cpp:55
ToUnrealVector
static FVector ToUnrealVector(const Eigen::Vector3d &V1)
Definition SpringBasedVegetationComponent.cpp:146
OuterProduct
static Eigen::Matrix3d OuterProduct(const Eigen::Vector3d &V1, const Eigen::Vector3d &V2)
Definition SpringBasedVegetationComponent.cpp:133
COLLISION_LOG
#define COLLISION_LOG(...)
Definition SpringBasedVegetationComponent.cpp:60
EigenVectorToRotator
static FRotator EigenVectorToRotator(const Eigen::Vector3d &Vector)
Definition SpringBasedVegetationComponent.cpp:170
OTHER_LOG
#define OTHER_LOG(...)
Definition SpringBasedVegetationComponent.cpp:75
GetDeltaRotator
FRotator GetDeltaRotator(const FRotator &Rotator1, const FRotator &Rotator2)
Definition SpringBasedVegetationComponent.cpp:96
ToEigenMatrix
static Eigen::Matrix3d ToEigenMatrix(const FMatrix &Matrix)
Definition SpringBasedVegetationComponent.cpp:150
ACC_LOG
#define ACC_LOG(...)
Definition SpringBasedVegetationComponent.cpp:65
GetSign
static T GetSign(T n)
Definition SpringBasedVegetationComponent.cpp:122
EigenToFString
static FString EigenToFString(T &t)
Definition SpringBasedVegetationComponent.cpp:127
FICT_LOG
#define FICT_LOG(...)
Definition SpringBasedVegetationComponent.cpp:70
RotatorToEigenVector
static Eigen::Vector3d RotatorToEigenVector(const FRotator &Rotator)
Definition SpringBasedVegetationComponent.cpp:163
IsValid
static bool IsValid(const ACarlaWheeledVehicle *Vehicle)
Definition TrafficLightBase.cpp:17
ACarlaWheeledVehicle
Base class for CARLA wheeled vehicles.
Definition CarlaWheeledVehicle.h:85
UWalkerAnim::Snap
FPoseSnapshot Snap
Definition WalkerAnim.h:27
carla::rpc::Transform
geom::Transform Transform
Definition rpc/Transform.h:16
FSkeletonHierarchy::AddForce
void AddForce(const FString &BoneName, const FVector &Force)
Definition SpringBasedVegetationComponent.cpp:255
FSkeletonHierarchy::Joints
TArray< FSkeletonJoint > Joints
Definition SpringBasedVegetationComponent.h:84
FSkeletonJoint::Bones
TArray< FSkeletonBone > Bones
Definition SpringBasedVegetationComponent.h:65
Generated by doxygen 1.9.8