CARLA
 
载入中...
搜索中...
未找到
SpringBasedVegetationComponent.cpp
浏览该文件的文档.
1// Copyright (c) 2022 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
9#include "Math/Matrix.h"
10#include "Components/CapsuleComponent.h"
11#include "DrawDebugHelpers.h"
12#include "Kismet/KismetMathLibrary.h"
13#include "BaseVegetationActor.h"
15#include <unordered_set>
16#include <vector>
17#include <cmath>
18#include <sstream>
19
21#include "carla/rpc/String.h"
23
24#define SPRINGVEGETATIONLOGS 0
25#define SOLVERLOGS 0
26#define COLLISIONLOGS 0
27#define ACCUMULATIONLOGS 0
28#define FICTITIOUSFORCELOGS 0
29#define OTHERLOGS 0
30
31#if SOLVERLOGS && SPRINGVEGETATIONLOGS
32#define SOLVER_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
33#else
34#define SOLVER_LOG(...)
35#endif
36#if COLLISIONLOGS && SPRINGVEGETATIONLOGS
37#define COLLISION_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
38#else
39#define COLLISION_LOG(...)
40#endif
41#if ACCUMULATIONLOGS && SPRINGVEGETATIONLOGS
42#define ACC_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
43#else
44#define ACC_LOG(...)
45#endif
46#if FICTITIOUSFORCELOGS && SPRINGVEGETATIONLOGS
47#define FICT_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
48#else
49#define FICT_LOG(...)
50#endif
51#if OTHERLOGS && SPRINGVEGETATIONLOGS
52#define OTHER_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__)
53#else
54#define OTHER_LOG(...)
55#endif
56
57
58FRotator GetDeltaRotator(const FRotator & Rotator1, const FRotator & Rotator2)
59{
60 float HalfCircle = 180.f;
61 float FullCircle = 360.f;
62 auto GetDeltaAngle = [&](float Angle1, float Angle2) -> float
63 {
64 if (Angle1 < 0)
65 {
66 Angle1 = FullCircle + Angle1;
67 }
68 if (Angle2 < 0)
69 {
70 Angle2 = FullCircle + Angle2;
71 }
72 float Diff = fmod( Angle1 - Angle2 + HalfCircle , FullCircle) - HalfCircle;
73 return Diff < -HalfCircle ? Diff + FullCircle : Diff;
74 };
75
76 return FRotator(
77 GetDeltaAngle(Rotator1.Pitch, Rotator2.Pitch),
78 GetDeltaAngle(Rotator1.Yaw, Rotator2.Yaw),
79 GetDeltaAngle(Rotator1.Roll, Rotator2.Roll)
80 );
81}
82
83template <class T>
84static T GetSign(T n)
85{
86 return n < 0.0f ? -1.0f : 1.0f;
87}
88template <class T>
89static FString EigenToFString(T& t)
90{
91 std::stringstream ss;
92 ss << t;
93 return carla::rpc::ToFString(ss.str());
94}
95static Eigen::Matrix3d OuterProduct(const Eigen::Vector3d& V1, const Eigen::Vector3d& V2)
96{
97 return V1 * V2.transpose();
98}
99static Eigen::Matrix3d OuterProduct(const Eigen::Vector3d& V1)
100{
101 return V1 * V1.transpose();
102}
103// 将向量转换为右手坐标系,通过翻转z坐标来实现
104static Eigen::Vector3d ToEigenVector(const FVector& V1)
105{
106 return Eigen::Vector3d(V1.X, V1.Y, -V1.Z);
107}
108static FVector ToUnrealVector(const Eigen::Vector3d& V1)
109{
110 return FVector(V1(0), V1(1), -V1(2));
111}
112static Eigen::Matrix3d ToEigenMatrix(const FMatrix& Matrix) //用二维数组表示矩阵,
113 //第一维为行,第二维为列。
114{
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();
120}
121static Eigen::Matrix3d ToEigenMatrix(const FTransform& Transform)
122{
123 FMatrix Matrix = Transform.ToMatrixNoScale(); //用二维数组表示矩阵,
124 //第一维为行,第二维为列。
125 return ToEigenMatrix(Matrix);
126}
127static Eigen::Vector3d RotatorToEigenVector(const FRotator& Rotator)
128{
129 return Eigen::Vector3d(
130 FMath::DegreesToRadians(Rotator.Roll),
131 FMath::DegreesToRadians(Rotator.Pitch),
132 -FMath::DegreesToRadians(Rotator.Yaw));
133}
134static FRotator EigenVectorToRotator(const Eigen::Vector3d& Vector)
135{
136 return FRotator(
137 FMath::RadiansToDegrees(Vector(1)),
138 -FMath::RadiansToDegrees(Vector(2)),
139 FMath::RadiansToDegrees(Vector(0)));
140}
141
143{
144 Joints.Empty();
145 EndJoints.Empty();
146 EndToRootOrder.Empty();
147 RootToEndOrder.Empty();
148}
150{
151 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::ComputeChildrenJointsAndBones);
152 for (int i = 1; i < Joints.Num(); i++)
153 {
154 FSkeletonJoint& Joint = Joints[i];
155 FSkeletonJoint& ParentJoint = Joints[Joint.ParentId];
156 ParentJoint.ChildrenIds.Add(i);
157 }
158
159 // 调试
160 for (int i = 0; i < Joints.Num(); i++)
161 {
162 FSkeletonJoint& Joint = Joints[i];
163 FString ChildrenIds = "";
164 for (int ChildrenId : Joint.ChildrenIds)
165 {
166 ChildrenIds += FString::FromInt(ChildrenId) + ", ";
167 }
168 OTHER_LOG(Warning, "Joint %d, Children: %s.", i, *ChildrenIds);
169 }
170}
171
173{
174 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::ComputeEndJoints);
175 EndJoints.Empty();
176 for (int i = 0; i < Joints.Num(); i++)
177 {
178 FSkeletonJoint& Joint = Joints[i];
179 if (!Joint.ChildrenIds.Num())
180 {
181 EndJoints.Add(i);
182 }
183 }
184
185 // 构建遍历顺序,确保父节点在任何子节点之前被访问
186 RootToEndOrder.Empty();
187 std::vector<int> JointsToVisit;
188 JointsToVisit.emplace_back(0); // 将根节点添加进带访问列表
189 RootToEndOrder.Add(0);
190 while (JointsToVisit.size())
191 {
192 FSkeletonJoint& Joint = Joints[JointsToVisit.back()];
193 JointsToVisit.pop_back();
194
195 for (int ChildrenId : Joint.ChildrenIds)
196 {
197 RootToEndOrder.Add(ChildrenId);
198 JointsToVisit.emplace_back(ChildrenId);
199 }
200 }
201
202 // 反转遍历顺序,在父节点之前访问子节点
203 EndToRootOrder.Empty();
204 FString IdOrder = "";
205 FString NameOrder = "";
206 for(int i = RootToEndOrder.Num() - 1; i >= 0; i--)
207 {
208 int Id = RootToEndOrder[i];
209 EndToRootOrder.Add(Id);
210 IdOrder += FString::FromInt(Id) + " ";
211 NameOrder += Joints[Id].JointName + " ";
212 }
213 //调试
214 OTHER_LOG(Warning, "Tree order: %s", *IdOrder);
215 OTHER_LOG(Warning, "Tree order (names): %s", *NameOrder);
216 OTHER_LOG(Warning, "Num elements: %d", EndToRootOrder.Num());
217}
218
219void FSkeletonHierarchy::AddForce(const FString& BoneName, const FVector& Force)
220{
221 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::AddForce);
222 for (FSkeletonJoint& Joint : Joints)
223 {
224 if(Joint.JointName == BoneName)
225 {
226 Joint.ExternalForces += Force;
227 }
228 }
229}
231{
232 TRACE_CPUPROFILER_EVENT_SCOPE(FSkeletonHierarchy::ClearExternalForces);
233 for (FSkeletonJoint& Joint : Joints)
234 {
235 Joint.ExternalForces = FVector(0,0,0);
236 }
237}
238
239USpringBasedVegetationComponent::USpringBasedVegetationComponent(const FObjectInitializer& ObjectInitializer)
240 : Super(ObjectInitializer)
241{
242 PrimaryComponentTick.bCanEverTick = true;
243 PrimaryComponentTick.bStartWithTickEnabled = true;
244}
245
246void USpringBasedVegetationComponent::GenerateSkeletonHierarchy()
247{
248 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::GenerateSkeletonHierarchy);
249 OTHER_LOG(Warning, "Get skeleton hierarchy");
250 // 获取骨骼层次结构
251 if (!SkeletalMesh)
252 {
253 UActorComponent* Component = GetOwner()->GetComponentByClass(USkeletalMeshComponent::StaticClass());
254 SkeletalMesh = Cast<USkeletalMeshComponent>(Component);
255 }
256 TArray<FName> BoneNames;
257 SkeletalMesh->GetBoneNames(BoneNames);
258 Skeleton.Clear();
259 for (int32 i = 0; i < BoneNames.Num(); i++)
260 {
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);
269 }
270
271 Skeleton.ComputeChildrenJointsAndBones();
272 Skeleton.ComputeEndJoints();
273
274 // 获取骨骼的静止姿态
275 auto *AnimInst = SkeletalMesh->GetAnimInstance();
276 if (!AnimInst)
277 {
278 OTHER_LOG(Error, "Could not get animation instance.");
279 return;
280 }
281 UWalkerAnim *WalkerAnim = Cast<UWalkerAnim>(AnimInst);
282 if (!WalkerAnim)
283 {
284 OTHER_LOG(Error, "Could not get UWalkerAnim.");
285 return;
286 }
287
288 // 获得当前姿态
289 FPoseSnapshot TempSnapshot;
290 SkeletalMesh->SnapshotPose(TempSnapshot);
291
292 // 复制这个姿态
293 WalkerAnim->Snap = TempSnapshot;
294
295 for (int i=0; i<Skeleton.Joints.Num(); ++i)
296 {
297 FSkeletonJoint& Joint = Skeleton.Joints[i];
298 FTransform JointTransform = SkeletalMesh->GetSocketTransform(FName(*Joint.JointName), ERelativeTransformSpace::RTS_ParentBoneSpace);
299 Joint.Transform = JointTransform;
300 Joint.RestingAngles = JointTransform.Rotator();
301 OTHER_LOG(Log, "Getting info for bone %s, %f, %f, %f, %f", *Joint.JointName, Joint.RestingAngles.Pitch, Joint.RestingAngles.Yaw, Joint.RestingAngles.Roll);
302 if(i > 0)
303 {
304 FSkeletonJoint& ParentJoint = Skeleton.Joints[Joint.ParentId];
305 FVector BoneCOM = Joint.Transform.GetLocation()*0.5f;
306 float BoneLength = Joint.Transform.GetLocation().Size();
307 ParentJoint.Bones.Add({10, BoneLength, BoneCOM});
308 }
309 }
310
311 UpdateGlobalTransform();
312}
313
314void USpringBasedVegetationComponent::BeginPlay()
315{
316 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::BeginPlay);
317 Super::BeginPlay();
318 OTHER_LOG(Warning, "USpringBasedVegetationComponent::BeginPlay");
319 OTHER_LOG(Warning, "Params: BaseSpringStrength %f, Num joints: %d, CollisionForceParameter %f", BaseSpringStrength, Skeleton.Joints.Num(), CollisionForceParameter);
320 if (!SkeletalMesh)
321 {
322 UActorComponent* Component = GetOwner()->GetComponentByClass(USkeletalMeshComponent::StaticClass());
323 SkeletalMesh = Cast<USkeletalMeshComponent>(Component);
324 }
325 if (!SkeletalMesh)
326 {
327 SetComponentTickEnabled(false);
328 OTHER_LOG(Error, "Could not find skeletal mesh component.");
329 return;
330 }
331
332 ABaseVegetationActor* BaseVegetation = Cast<ABaseVegetationActor>(GetOwner());
333 if(BaseVegetation)
334 {
335 BaseVegetation->SetParametersToComponent();
336 }
337
338 // 设置回调函数
339 SkeletalMesh->OnComponentHit.AddDynamic(this, &USpringBasedVegetationComponent::OnCollisionEvent);
340
341 if (!Skeleton.Joints.Num())
342 {
343 GenerateSkeletonHierarchy();
344 }
345
346 UpdateGlobalTransform();
347 GenerateCollisionCapsules();
348 if(bAutoComputeStrength)
349 {
350 ComputeSpringStrengthForBranches();
351 }
352
353 JointCollisionList.resize(Skeleton.Joints.Num());
354}
355
356void USpringBasedVegetationComponent::ResetComponent()
357{
358 Skeleton.ClearExternalForces();
359 SkeletalMesh->ResetAllBodiesSimulatePhysics();
360}
361
362void USpringBasedVegetationComponent::GenerateCollisionCapsules()
363{
364 for (FSkeletonJoint& Joint : Skeleton.Joints)
365 {
366 for (FSkeletonBone& Bone : Joint.Bones)
367 {
368 if (Bone.Length < MinBoneLength)
369 {
370 continue;
371 }
372 UCapsuleComponent* Capsule = NewObject<UCapsuleComponent>(GetOwner());
373 Capsule->AttachToComponent(SkeletalMesh, FAttachmentTransformRules::KeepRelativeTransform, FName(*Joint.JointName));
374 Capsule->RegisterComponent();
375 // 根据骨骼的Z方向创建一个旋转,使得Capsule对齐
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);
381 if (Joint.bIsStatic)
382 {
383 Capsule->SetGenerateOverlapEvents(false);
384 Capsule->SetCollisionProfileName("BlockAll");
385 Capsule->OnComponentHit.AddDynamic(this, &USpringBasedVegetationComponent::OnCollisionEvent);
386 }
387 else
388 {
389 Capsule->SetGenerateOverlapEvents(true);
390 Capsule->SetCollisionProfileName("OverlapAll");
391 Capsule->OnComponentBeginOverlap.AddDynamic(this, &USpringBasedVegetationComponent::OnBeginOverlapEvent);
392 Capsule->OnComponentEndOverlap.AddDynamic(this, &USpringBasedVegetationComponent::OnEndOverlapEvent);
393 }
394
395 BoneCapsules.Add(Capsule);
396 CapsuleToJointId.Add(Capsule, Joint.JointId);
397 }
398 }
399}
400
401void USpringBasedVegetationComponent::ComputeSpringStrengthForBranches()
402{
403 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeSpringStrengthForBranches);
404 FTransform RootTransform = Skeleton.GetRootJoint().GlobalTransform;
405 FVector RootLocation = RootTransform.GetLocation();
406 // 用RootTransform.GetRotation().GetUpVector()函数给FVector TreeAxis赋值
407 FVector TreeAxis = FVector(0,0,1);
408 for (FSkeletonJoint& Joint : Skeleton.Joints)
409 {
410 FVector JointLocation = Joint.GlobalTransform.GetLocation();
411 FVector ClosestPoint;
412 float HorizontalDistance = FMath::PointDistToLine(JointLocation, TreeAxis, RootLocation, ClosestPoint);
413 float VerticalDistance = FVector::Distance(ClosestPoint, RootLocation);
414
415 Joint.SpringStrength = FMath::Max(
416 BaseSpringStrength - HorizontalFallof*HorizontalDistance - VerticalFallof*VerticalDistance,
417 MinSpringStrength);
418
419 OTHER_LOG(Log, "Joint: %s, location %s, Strength %f", *Joint.JointName, *JointLocation.ToString(), Joint.SpringStrength);
420 }
421}
422
423void USpringBasedVegetationComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
424{
425 for (UPrimitiveComponent* Capsule : BoneCapsules)
426 {
427 Capsule->DestroyComponent();
428 }
429 BoneCapsules.Empty();
430}
431
432// 计算单个关节的属性(包括质心、惯性、力和扭矩)
433void USpringBasedVegetationComponent::ComputePerJointProperties(
434 std::vector<FJointProperties>& JointLocalPropertiesList,
435 std::vector<FJointProperties>& JointPropertiesList)
436{
437 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputePerJointProperties);
438 for(FSkeletonJoint& Joint : Skeleton.Joints)
439 {
440 FJointProperties& Properties = JointLocalPropertiesList[Joint.JointId];
441 Eigen::Vector3d JointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation())/100.f;
443 JointPropertiesList[Joint.JointId].JointToGlobalMatrix = Properties.JointToGlobalMatrix;
444 if (!Joint.Bones.Num())
445 continue;
446 // 质心和质量计算(COM-center of mass质心)
447 for (FSkeletonBone& Bone : Joint.Bones)
448 {
449 Properties.Mass += Bone.Mass;
450 FVector GlobalCenterOfMass = Joint.GlobalTransform.TransformPositionNoScale(Bone.CenterOfMass);
451 Properties.CenterOfMass += Bone.Mass*ToEigenVector(GlobalCenterOfMass)/100.f;
452 }
453 Properties.CenterOfMass = Properties.CenterOfMass/Properties.Mass;
454
455 // 重力的计算
456 Eigen::Vector3d GravityForce = ToEigenVector(Gravity)/100.f; // 地心引力(重力)
457 Properties.Force = Properties.Mass * GravityForce + ToEigenVector(Joint.ExternalForces)/100.f;
458 // 扭矩的计算
459 Properties.Torque = (Properties.CenterOfMass - JointGlobalPosition).cross(Properties.Force);
460 // 惯性张量的计算
461 for (FSkeletonBone& Bone : Joint.Bones)
462 {
463 if (Bone.Length < 1)
464 continue;
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));
477 LocalV2.normalize();
478 Eigen::Vector3d LocalV3 = LocalV1.cross(LocalV2);
479 Eigen::Matrix3d LocalToJointMatrix;
480 LocalToJointMatrix << LocalV1, LocalV2, LocalV3;
481 Eigen::Matrix3d JointSpaceInertia = LocalToJointMatrix*LocalCylinderInertia*(LocalToJointMatrix.transpose());
482 Properties.InertiaTensor += Properties.JointToGlobalMatrix*JointSpaceInertia*Properties.JointToGlobalMatrix.transpose();
483 Eigen::Vector3d BoneCenterWorldSpace = ToEigenVector(Joint.GlobalTransform.TransformPositionNoScale(Bone.CenterOfMass))/100.0;
484 Properties.InertiaTensor += Properties.Mass*OuterProduct(Properties.CenterOfMass - BoneCenterWorldSpace, Properties.CenterOfMass - BoneCenterWorldSpace);
485 }
486 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);
487 }
488}
489
490// 计算属性之和(质心、惯性、力和扭矩)
491void USpringBasedVegetationComponent::ComputeCompositeBodyContribution(
492 std::vector<FJointProperties>& JointLocalPropertiesList,
493 std::vector<FJointProperties>& JointPropertiesList)
494{
495 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeCompositeBodyContribution);
496 for (int Id : Skeleton.EndToRootOrder)
497 {
498 FSkeletonJoint& Joint = Skeleton.Joints[Id];
499 FJointProperties& JointProperties = JointPropertiesList[Joint.JointId];
500 FJointProperties& JointLocalProperties = JointLocalPropertiesList[Joint.JointId];
501
502 if (!Joint.ChildrenIds.Num())
503 {
504 continue;
505 }
506
507 Eigen::Vector3d CenterOfMass = JointLocalProperties.Mass*JointLocalProperties.CenterOfMass;
508 // 计算总质心和质量之和
509 JointProperties.Mass = JointLocalProperties.Mass;
510 for(int ChildrenId : Joint.ChildrenIds)
511 {
512 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
513 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
514 CenterOfMass += ChildrenProperties.Mass*ChildrenProperties.CenterOfMass;
515 JointProperties.Mass += ChildrenProperties.Mass;
516 }
517 JointProperties.CenterOfMass = CenterOfMass / JointProperties.Mass;
518
519 // 计算总力
520 JointProperties.Force = JointLocalProperties.Force;
521 for(int ChildrenId : Joint.ChildrenIds)
522 {
523 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
524 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
525 JointProperties.Force += ChildrenProperties.Force;
526 }
527
528 // 计算总扭矩
529 JointProperties.Torque = JointLocalProperties.Torque;
530 for(int ChildrenId : Joint.ChildrenIds)
531 {
532 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
533 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
534 Eigen::Vector3d ChildrenJointGlobalPosition = ToEigenVector(ChildrenJoint.GlobalTransform.GetLocation())/100.f;
535 Eigen::Vector3d ParentJointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation())/100.f;
536 JointProperties.Torque += ChildrenProperties.Torque + (ChildrenJointGlobalPosition - ParentJointGlobalPosition).cross(ChildrenProperties.Force);
537 }
538
539 // 计算总惯性张量
540 JointProperties.InertiaTensor = JointLocalProperties.InertiaTensor + JointLocalProperties.Mass*OuterProduct(JointProperties.CenterOfMass - JointLocalProperties.CenterOfMass, JointProperties.CenterOfMass - JointLocalProperties.CenterOfMass);
541 for(int ChildrenId : Joint.ChildrenIds)
542 {
543 FSkeletonJoint& ChildrenJoint = Skeleton.Joints[ChildrenId];
544 FJointProperties& ChildrenProperties = JointPropertiesList[ChildrenId];
545 Eigen::Vector3d ChildrenToCenterOfMass = JointProperties.CenterOfMass - ChildrenProperties.CenterOfMass;
546
547 JointProperties.InertiaTensor += ChildrenProperties.InertiaTensor + ChildrenProperties.Mass*OuterProduct(ChildrenToCenterOfMass, ChildrenToCenterOfMass);
548 }
549 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);
550 }
551}
552
553void USpringBasedVegetationComponent::ComputeFictitiousForces(
554 std::vector<FJointProperties>& JointLocalPropertiesList,
555 std::vector<FJointProperties>& JointPropertiesList)
556{
557 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ComputeFictitiousForces);
558 // 虚拟力
559 FSkeletonJoint& RootJoint = Skeleton.Joints[0];
560 FJointProperties& RootProperties = JointPropertiesList[0];
561 for (int i = 1; i < Skeleton.RootToEndOrder.Num(); i++)
562 {
563 int Id = Skeleton.RootToEndOrder[i];
564 FSkeletonJoint& Joint = Skeleton.Joints[Id];
565 FJointProperties& JointProperties = JointPropertiesList[Joint.JointId];
566 FSkeletonJoint& ParentJoint = Skeleton.Joints[Joint.ParentId];
567 FJointProperties& ParentProperties = JointPropertiesList[Joint.ParentId];
568
569 Eigen::Matrix3d JointToGlobalMatrix = JointProperties.JointToGlobalMatrix;
570 Eigen::Matrix3d GlobalToJointMatrix = JointToGlobalMatrix.transpose();
571 Eigen::Matrix3d ParentJointToGlobalMatrix = ParentProperties.JointToGlobalMatrix;
572 Eigen::Matrix3d GlobalToParentJointMatrix = ParentJointToGlobalMatrix.transpose();
573 Eigen::Vector3d JointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation())/100.f;
574 Eigen::Vector3d ParentJointGlobalPosition = ToEigenVector(ParentJoint.GlobalTransform.GetLocation())/100.f;
575
576 JointProperties.AngularVelocity = ParentProperties.AngularVelocity
577 + ParentJointToGlobalMatrix*RotatorToEigenVector(ParentJoint.AngularVelocity);
578 JointProperties.LinearVelocity = ParentProperties.LinearVelocity
579 + ParentProperties.AngularVelocity.cross(JointGlobalPosition - ParentJointGlobalPosition);
580 JointProperties.AngularAcceleration = ParentProperties.AngularAcceleration
581 + ParentJointToGlobalMatrix*RotatorToEigenVector(ParentJoint.AngularAcceleration)
582 + ParentProperties.AngularVelocity.cross(JointProperties.AngularVelocity);
583 JointProperties.LinearAcceleration = ParentProperties.LinearAcceleration
584 + ParentProperties.LinearAcceleration.cross(JointGlobalPosition - ParentJointGlobalPosition)
585 + ParentProperties.AngularVelocity.cross(JointProperties.AngularVelocity - ParentProperties.AngularVelocity);
586
587 Eigen::Vector3d CompositeCenterOfMass = JointProperties.CenterOfMass;
588 Eigen::Vector3d CompositeLinearVelocity = JointProperties.LinearVelocity
589 + JointProperties.AngularVelocity.cross(CompositeCenterOfMass - JointGlobalPosition);
590 Eigen::Vector3d CompositeLinearAcceleration = JointProperties.LinearAcceleration
591 + JointProperties.AngularAcceleration.cross(CompositeCenterOfMass - JointGlobalPosition)
592 + JointProperties.AngularVelocity.cross(CompositeLinearVelocity - JointProperties.LinearVelocity);
593
594 Eigen::Vector3d TotalForces = -Joint.Mass * JointProperties.LinearAcceleration;
595 Eigen::Vector3d TotalTorque = (CompositeCenterOfMass - JointGlobalPosition).cross(TotalForces);
596 JointProperties.FictitiousTorque = GlobalToJointMatrix*TotalTorque;
597
598 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));
599 }
600}
601
602void USpringBasedVegetationComponent::ResolveContactsAndCollisions(
603 std::vector<FJointProperties>& JointLocalPropertiesList,
604 std::vector<FJointProperties>& JointPropertiesList)
605{
606 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::ResolveContactsAndCollisions);
607
608 // set all joints that can rest
609 for (auto &Joint : JointCollisionList)
610 {
611 Joint.CanRest = true;
612 }
613
614 for (auto& ActorCapsules : OverlappingActors)
615 {
616 TRACE_CPUPROFILER_EVENT_SCOPE(ActorLoop);
617 AActor* CollidingActor = ActorCapsules.Key;
618 if (!IsValid(CollidingActor))
619 continue;
620 UPrimitiveComponent* Primitive = Cast<UPrimitiveComponent>(CollidingActor->GetRootComponent());
621 if (!IsValid(Primitive))
622 continue;
623 // force transferring momentum (for the initial collision frame)
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;
629 for (UPrimitiveComponent* Capsule : CollidingCapsules)
630 {
631 TRACE_CPUPROFILER_EVENT_SCOPE(CapsuleLoop);
632 if (!IsValid(Capsule))
633 continue;
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;
640 bool HitFound;
641 FVector ClosestPointOnCollider;
642 float DistanceToCollider;
643 {
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)
654 {
655 DrawDebugLine(GetWorld(), LineTraceStart, LineTraceEnd, FColor::Orange, false, 0.1f, 0.0f, 1.f);
656 }
657 }
658 if(!HitFound)
659 {
660 continue;
661 }
662
663 if (DebugEnableVisualization)
664 {
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));
672 }
673
674 const int JointId = CapsuleToJointId[Capsule];
675 const FSkeletonJoint& Joint = Skeleton.Joints[JointId];
676 FJointProperties& JointProperties = JointLocalPropertiesList[Joint.JointId];
677 FJointCollision& JointCollision = JointCollisionList[Joint.JointId];
678 const Eigen::Vector3d JointGlobalPosition = ToEigenVector(Joint.GlobalTransform.GetLocation()) / 100.f;
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();
683
684 // Contact forces due to spring strength
685 const FRotator CurrRotator = Joint.Transform.Rotator();
686 const FRotator RestRotator = Joint.RestingAngles;
687 const FRotator DeltaRotator =
688 GetDeltaRotator(CurrRotator, RestRotator);
689 const Eigen::Vector3d SpringTorque = Joint.SpringStrength * RotatorToEigenVector(DeltaRotator);
690 const Eigen::Vector3d JointCapsuleVector = JointGlobalPosition - CapsulePosition;
691 const Eigen::Vector3d RepulsionForce = SpringTorque.cross(JointCapsuleVector) * JointCapsuleVector.squaredNorm();
692
693 FVector RepulsionForceUE = -ToUnrealVector(RepulsionForce) * 100.f;
694 Primitive->AddForceAtLocation(RepulsionForceUE, ClosestPointOnCollider);
695
696 // 强制阻止几何体重叠
697 float ForceFactor = 1.f;
698 // from eq f = 1 - a*d^p, f: ForceFactor, a: ProportionalConstant, p: ForceDistanceFalloffExponent, d: DistanceOnCapsule
699 // float ProportionalConstant = 1.f/(FMath::Pow(ForceMaxDistance, ForceDistanceFalloffExponent));
700 // ForceFactor = 1.f - ProportionalConstant * FMath::Pow(DistanceOnCapsule, ForceDistanceFalloffExponent);
701 // from eq f = a*d^p, f: ForceFactor, a: ProportionalConstant, p: ForceDistanceFalloffExponent, d: DistanceToCollider
702 float ProportionalConstant = 1.f/(FMath::Pow(ForceMaxDistance, ForceDistanceFalloffExponent));
703 ForceFactor = ProportionalConstant * FMath::Pow(DistanceToCollider, ForceDistanceFalloffExponent);
704 ForceFactor = FMath::Clamp(ForceFactor, MinForceFactor, 1.f);
705
706 // const Eigen::Vector3d OverlappingForces = (ColliderPosition - CapsulePosition).normalized() * CollisionForceParameter * ForceFactor;
707 // const Eigen::Vector3d OverlappingForces = (ColliderPosition - PointOnCapsulePosition).normalized() * CollisionForceParameter * ForceFactor;
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;
713 // COLLISION_LOG(Log, "Joint: %s \n ProjectedSpeed %f, ProportionalFactor %f \n RepulsionForce %s \n", *Joint.JointName,ProjectedSpeed,ProportionalFactor,*EigenToFString(RepulsionForce),*EigenToFString(CollisionTorque));
714 //UE_LOG(LogCarla, Display, TEXT("DistanceToCollider: %f, ForceFactor: %f"), DistanceToCollider, ForceFactor);
715
716 // 阻止力使关节移动到其余角度
717 int TempId = JointId;
718 do
719 {
720 FJointCollision& TempJointCollision = JointCollisionList[TempId];
721 TempJointCollision.CanRest = false;
722
723 FSkeletonJoint &TempJoint = Skeleton.Joints[TempId];
724 TempId = TempJoint.ParentId;
725 }
726 while (TempId != -1);
727
728 if (DebugEnableVisualization)
729 {
730 static constexpr float DEBUG_SPHERE_SIZE = 5.0f;
731 // 绘制操作
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);
739 FVector UECOM = ToUnrealVector(JointProperties.CenterOfMass )*100.f;
740 DrawDebugSphere(GetWorld(), UECOM, DEBUG_SPHERE_SIZE, 64, FColor::Emerald);
741 FVector UEJointPos = ToUnrealVector(JointGlobalPosition )*100.f;
742 DrawDebugSphere(GetWorld(), UEJointPos, DEBUG_SPHERE_SIZE, 64, FColor::Purple);
743 DrawDebugLine(GetWorld(), ClosestPointOnCapsule, UEJointPos, FColor::Cyan, false, 0.1f, 0.0f, 1.f);
744 }
745 }
746 }
747
748 // 重新设置迭代器
749 for (auto &Joint : JointCollisionList)
750 {
751 if (Joint.CanRest)
752 {
753 if (Joint.Iteration > 1)
754 {
755 --Joint.Iteration;
756 }
757 }
758 else
759 {
760 if (Joint.Iteration <= 95)
761 {
762 Joint.Iteration += 5;
763 }
764 }
765 }
766}
767
768void USpringBasedVegetationComponent::SolveEquationOfMotion(
769 std::vector<FJointProperties>& JointPropertiesList,
770 float DeltaTime)
771{
772 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::SolveEquationOfMotion);
773 // solver
774 for (FSkeletonJoint& Joint : Skeleton.Joints)
775 {
776 if (!Joint.Bones.Num() || Joint.bIsStatic)
777 {
778 continue;
779 }
780 FJointProperties& JointProperties = JointPropertiesList[Joint.JointId];
781 FJointCollision& JointCollision = JointCollisionList[Joint.JointId];
782
783 // 调试绘制
784 if (DebugEnableVisualization)
785 {
786 if (Joint.ParentId != -1)
787 {
788 FVector Start = Joint.GlobalTransform.GetLocation();
789 FVector End = Skeleton.Joints[Joint.ParentId].GlobalTransform.GetLocation();
790 if (!JointCollision.CanRest)
791 {
792 DrawDebugLine(GetWorld(), Start, End, FColor::Red, false, 0.3f, 0.0f, 1.f);
793 }
794 else
795 {
796 DrawDebugLine(GetWorld(), Start, End, FColor::Blue, false, 0.1f, 0.0f, 1.f);
797 }
798 }
799 }
800
801 float Mass = JointProperties.Mass;
802 Eigen::Vector3d CenterToJoint = JointProperties.CenterOfMass - ToEigenVector(Joint.GlobalTransform.GetLocation())/100.f;
803 Eigen::Matrix3d GlobalToJointMatrix = JointProperties.JointToGlobalMatrix.transpose();
804 Eigen::Matrix3d JointSpaceIntertiaTensor = JointProperties.Mass*OuterProduct(CenterToJoint, CenterToJoint)
805 + GlobalToJointMatrix*JointProperties.InertiaTensor*GlobalToJointMatrix.transpose();
806 Eigen::Vector3d Torque = GlobalToJointMatrix*JointProperties.Torque + JointProperties.FictitiousTorque;
807 // 建立微分方程
808 Eigen::Matrix3d I = JointSpaceIntertiaTensor;
809 float SpringStrength = Joint.SpringStrength;
810 float beta = Beta;
811 float alpha = Alpha;
812 Eigen::Matrix3d K;
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();
822 // Eigen::Matrix3d Lambda = U.transpose()*K*U;
823 Eigen::Matrix3d U = Linv.transpose()*X;
824 // Eigen::Matrix3d Uinv = U.inverse();
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;
829 FString StringI = EigenToFString(I);
830 FString StringTorque = EigenToFString(Torque);
831 FString StringU = EigenToFString(U);
832 FString StringLambda = EigenToFString(Lambda);
833 FString StringGlobalToJoint = EigenToFString(GlobalToJointMatrix);
834 FString StringCenterToJoint = EigenToFString(CenterToJoint);
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);
836
837 FRotator CurrRotator = Joint.Transform.Rotator();
838 FRotator RestRotator = Joint.RestingAngles;
839 FRotator AngularVelocity = Joint.AngularVelocity;
840 FRotator DeltaRotator =
841 GetDeltaRotator(CurrRotator, RestRotator);
842 if (!JointCollision.CanRest)
843 {
844 float Factor = 1.0f - ((JointCollision.Iteration / 100.0f) * RestFactor);
845 // DeltaRotator *= Factor;
846 AngularVelocity *= Factor;
847 }
848 Eigen::Vector3d InitialTheta = Uinv*RotatorToEigenVector(DeltaRotator);
849 Eigen::Vector3d InitialThetaVelocity = Uinv*RotatorToEigenVector(AngularVelocity);
850 SOLVER_LOG(Log, "Old angle for joint %s, %s", *Joint.JointName, *CurrRotator.ToString());
851 SOLVER_LOG(Log, "InitialTheta \n %s", *EigenToFString(InitialTheta));
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;
856 // 第一行
857 for (int i = 0; i < 3; i++)
858 {
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);
864
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);
866 if(k == 0)
867 {
868 NewTheta(i) = theta0;
869 SOLVER_LOG(Log, "In Bone %s, K is 0 in component %d", *Joint.JointName, i);
870 }
871 double dtheta0 = InitialThetaVelocity(i); // compute
872 double deltatheta = 0;
873 double angularvelocity = 0;
874 float angularaccel = 0;
875 if (discriminant > 0)
876 {
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);
884 }
885 else if (discriminant == 0)
886 {
887 double r = -b/2.f;
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);
893 }
894 else
895 {
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;
900 deltatheta =
901 c1*std::exp(gamma*DeltaTime)*std::cos(mu*DeltaTime) +
902 c2*std::exp(gamma*DeltaTime)*std::sin(mu*DeltaTime) + f/k;
903 angularvelocity =
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);
907 }
908 angularaccel = f - b*angularvelocity - k*deltatheta;
909 if (!FMath::IsNaN(deltatheta))
910 {
911 NewTheta(i) = deltatheta;
912 if (angularvelocity > 1e-4)
913 {
914 NewThetaVelocity(i) = angularvelocity;
915 }
916 if (angularaccel > 1e-2)
917 {
918 NewThetaAccel(i) = angularaccel;
919 }
920 }
921 }
922 Eigen::Vector3d FinalNewTheta = U*NewTheta;
923 Eigen::Vector3d FinalNewThetaVelocity = U*NewThetaVelocity;
924 Eigen::Vector3d FinalNewThetaAccel = U*NewThetaAccel;
925
926 auto NewPitch = FMath::RadiansToDegrees(FinalNewTheta(1));
927 auto NewYaw = FMath::RadiansToDegrees(FinalNewTheta(2));
928 auto NewRoll = FMath::RadiansToDegrees(FinalNewTheta(0));
929
930 FRotator NewAngularVelocity = EigenVectorToRotator(FinalNewThetaVelocity);
931 FRotator NewAngularAccel = EigenVectorToRotator(FinalNewThetaAccel);
932
933 FRotator NewAngle(
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",
938 *EigenToFString(FinalNewTheta), *EigenToFString(FinalNewThetaVelocity), *EigenToFString(FinalNewThetaAccel));
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());
942 Joint.AngularVelocity = NewAngularVelocity;
943 Joint.AngularAcceleration = NewAngularAccel;
944 }
945}
946
947void USpringBasedVegetationComponent::TickComponent(
948 float DeltaTime,
949 enum ELevelTick TickType,
950 FActorComponentTickFunction * ThisTickFunction)
951{
952 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::TickComponent);
953 Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
954
955 float DeltaTimeFinal = DeltaTime;
956 if (DeltaTimeOverride > 0)
957 DeltaTimeFinal = DeltaTimeOverride;
958
959 std::vector<FJointProperties> JointPropertiesList;
960 JointPropertiesList.resize(Skeleton.Joints.Num());
961 std::vector<FJointProperties> JointLocalPropertiesList;
962 JointLocalPropertiesList.resize(Skeleton.Joints.Num());
963
964 ComputePerJointProperties(JointLocalPropertiesList, JointPropertiesList);
965
966 ResolveContactsAndCollisions(JointLocalPropertiesList, JointPropertiesList);
967
968 ComputeCompositeBodyContribution(JointLocalPropertiesList, JointPropertiesList);
969
970 ComputeFictitiousForces(JointLocalPropertiesList, JointPropertiesList);
971
972 SolveEquationOfMotion(JointPropertiesList, DeltaTimeFinal);
973
974 UpdateSkeletalMesh();
975 UpdateGlobalTransform();
976 Skeleton.ClearExternalForces();
977}
978
979void USpringBasedVegetationComponent::OnCollisionEvent(
980 UPrimitiveComponent* HitComponent,
981 AActor* OtherActor,
982 UPrimitiveComponent* OtherComponent,
983 FVector NormalImpulse,
984 const FHitResult& Hit)
985{
986 // 防止自碰撞
987 if (OtherActor == GetOwner())
988 return;
989 // 防止与其他树形结构Actor实例碰撞
990 if(OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) != nullptr)
991 return;
992 ACarlaWheeledVehicle* Vehicle = nullptr;
993 if (DebugEnableAllCollisions)
994 {
995 if (!IsValid(OtherActor))
996 return;
997 }
998 else
999 {
1000 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1001 if (!IsValid(Vehicle))
1002 return;
1003 }
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);
1006}
1007
1008void USpringBasedVegetationComponent::OnBeginOverlapEvent(
1009 UPrimitiveComponent* OverlapComponent,
1010 AActor* OtherActor,
1011 UPrimitiveComponent* OtherComponent,
1012 int32 OtherBodyIndex,
1013 bool bFromSweep,
1014 const FHitResult& SweepResult)
1015{
1016 // 防止自碰撞
1017 if (OtherActor == GetOwner())
1018 return;
1019 // 防止与其他树形结构Actor实例碰撞
1020 if(OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) != nullptr)
1021 return;
1022 ACarlaWheeledVehicle* Vehicle = nullptr;
1023 if (DebugEnableAllCollisions)
1024 {
1025 if (!IsValid(OtherActor))
1026 return;
1027 }
1028 else
1029 {
1030 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1031 if (!IsValid(Vehicle))
1032 return;
1033 }
1034
1035 if (!OverlappingActors.Contains(OtherActor))
1036 {
1037 OverlappingActors.Add(OtherActor);
1038 }
1039 TArray<UPrimitiveComponent*>& OverlappingCapsules = OverlappingActors.FindOrAdd(OtherActor);
1040 if (!OverlappingCapsules.Contains(OverlapComponent))
1041 {
1042 OverlappingCapsules.Add(OverlapComponent);
1043 }
1044}
1045
1046void USpringBasedVegetationComponent::OnEndOverlapEvent(
1047 UPrimitiveComponent* OverlapComponent,
1048 AActor* OtherActor,
1049 UPrimitiveComponent* OtherComponent,
1050 int32 OtherBodyIndex)
1051{
1052 // 防止自碰撞
1053 if (OtherActor == GetOwner())
1054 return;
1055 // 防止与其他树形结构Actor实例碰撞
1056 if(OtherActor->GetComponentByClass(USpringBasedVegetationComponent::StaticClass()) != nullptr)
1057 return;
1058 ACarlaWheeledVehicle* Vehicle = nullptr;
1059 if (DebugEnableAllCollisions)
1060 {
1061 if (!IsValid(OtherActor))
1062 return;
1063 }
1064 else
1065 {
1066 Vehicle = Cast<ACarlaWheeledVehicle>(OtherActor);
1067 if (!IsValid(Vehicle))
1068 return;
1069 }
1070
1071 if (!OverlappingActors.Contains(OtherActor))
1072 return;
1073 TArray<UPrimitiveComponent*>& OverlappingCapsules = OverlappingActors.FindOrAdd(OtherActor);
1074 if (OverlappingCapsules.Contains(OverlapComponent))
1075 {
1076 OverlappingCapsules.RemoveSingle(OverlapComponent);
1077 if (OverlappingCapsules.Num() == 0)
1078 {
1079 OverlappingActors.Remove(OtherActor);
1080 }
1081 }
1082}
1083
1084void USpringBasedVegetationComponent::UpdateSkeletalMesh()
1085{
1086 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::UpdateSkeletalMesh);
1087 // 获得行人动画类
1088 auto *AnimInst = SkeletalMesh->GetAnimInstance();
1089 if (!AnimInst) return;
1090 UWalkerAnim *WalkerAnim = Cast<UWalkerAnim>(AnimInst);
1091 if (!WalkerAnim) return;
1092
1093 // 如果姿态为空,设定一个初始姿态
1094 if (WalkerAnim->Snap.BoneNames.Num() == 0)
1095 {
1096 // 获得当前姿态
1097 SkeletalMesh->SnapshotPose(WalkerAnim->Snap);
1098 }
1099
1100 TMap<FName, FTransform> JointsMap;
1101 for (int i=0; i<Skeleton.Joints.Num(); ++i)
1102 {
1103 FSkeletonJoint& Joint = Skeleton.Joints[i];
1104 FName JointName = FName(*Joint.JointName);
1105 JointsMap.Add(JointName, Joint.Transform);
1106 }
1107
1108 // 为walker animation分配通用骨骼
1109 for (int i=0; i<WalkerAnim->Snap.BoneNames.Num(); ++i)
1110 {
1111 FTransform *Trans = JointsMap.Find(WalkerAnim->Snap.BoneNames[i]);
1112 if (Trans)
1113 {
1114 WalkerAnim->Snap.LocalTransforms[i] = *Trans;
1115 }
1116 }
1117}
1118
1119void USpringBasedVegetationComponent::UpdateGlobalTransform()
1120{
1121 TRACE_CPUPROFILER_EVENT_SCOPE(USpringBasedVegetationComponent::UpdateGlobalTransform);
1122 FTransform InitialTransform = SkeletalMesh->GetOwner()->GetActorTransform();
1123 FSkeletonJoint& RootJoint = Skeleton.Joints[0];
1124 RootJoint.GlobalTransform = RootJoint.Transform * InitialTransform;
1125 RootJoint.GolbalInverseTransform = RootJoint.GlobalTransform.Inverse();
1126 for (int i = 1; i < Skeleton.RootToEndOrder.Num(); i++)
1127 {
1128 int Id = Skeleton.RootToEndOrder[i];
1129 FSkeletonJoint& Joint = Skeleton.Joints[Id];
1130 FSkeletonJoint& ParentJoint = Skeleton.Joints[Joint.ParentId];
1131 FTransform Transform = Joint.Transform * ParentJoint.GlobalTransform;
1132 Joint.GlobalTransform = Transform;
1133 Joint.GolbalInverseTransform = Transform.Inverse();
1134 }
1135}
static Eigen::Vector3d ToEigenVector(const FVector &V1)
#define SOLVER_LOG(...)
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)
#define OTHER_LOG(...)
FRotator GetDeltaRotator(const FRotator &Rotator1, const FRotator &Rotator2)
static Eigen::Matrix3d ToEigenMatrix(const FMatrix &Matrix)
#define ACC_LOG(...)
static T GetSign(T n)
static FString EigenToFString(T &t)
#define FICT_LOG(...)
static Eigen::Vector3d RotatorToEigenVector(const FRotator &Rotator)
static bool IsValid(const ACarlaWheeledVehicle *Vehicle)
Base class for CARLA wheeled vehicles.
FPoseSnapshot Snap
Definition WalkerAnim.h:25
geom::Transform Transform
void AddForce(const FString &BoneName, const FVector &Force)
TArray< FSkeletonJoint > Joints
TArray< FSkeletonBone > Bones