2020-01-10 12:41:31 -05:00
// Copyright Epic Games, Inc. All Rights Reserved.
# include "HeadlessChaosTestGJK.h"
# include "HeadlessChaos.h"
# include "Chaos/Capsule.h"
# include "Chaos/Convex.h"
# include "Chaos/GJK.h"
# include "Chaos/ImplicitObjectScaled.h"
namespace ChaosTest
{
using namespace Chaos ;
void GJKSphereSphereDistanceTest ( )
{
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 1e-3 ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
FVec3 NearestA = { 0 , 0 , 0 } ;
FVec3 NearestB = { 0 , 0 , 0 } ;
FReal Distance = 0 ;
2021-02-16 22:06:10 -04:00
FVec3 Normal = { 0 , 0 , 1 } ;
2020-01-10 12:41:31 -05:00
// Fail - overlapping
{
2021-02-03 14:57:28 -04:00
TSphere < FReal , 3 > A ( FVec3 ( 12 , 0 , 0 ) , 5 ) ;
TSphere < FReal , 3 > B ( FVec3 ( 4 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 ( FVec3 ( 2 , 0 , 0 ) , FRotation3 : : FromIdentity ( ) ) , Distance , NearestA , NearestB , Normal ) ;
EXPECT_NE ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping
{
2021-02-03 14:57:28 -04:00
TSphere < FReal , 3 > A ( FVec3 ( 12 , 0 , 0 ) , 5 ) ;
TSphere < FReal , 3 > B ( FVec3 ( 4 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) 7 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) 6 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) 0 , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping
{
2021-02-03 14:57:28 -04:00
TSphere < FReal , 3 > A ( FVec3 ( 0 , 0 , 0 ) , 2 ) ;
TSphere < FReal , 3 > B ( FVec3 ( 0 , 0 , 0 ) , 2 ) ;
FVec3 BPos = FVec3 ( 3 , 3 , 0 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 ( BPos , FRotation3 : : FromIdentity ( ) ) , Distance , NearestA , NearestB , Normal ) ;
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
FVec3 CenterDelta = ( B . GetCenter ( ) + BPos ) - A . GetCenter ( ) ;
FVec3 CenterDir = CenterDelta . GetSafeNormal ( ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , CenterDelta . Size ( ) - ( A . GetRadius ( ) + B . GetRadius ( ) ) , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , A . GetCenter ( ) . X + A . GetRadius ( ) * CenterDir . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , A . GetCenter ( ) . Y + A . GetRadius ( ) * CenterDir . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , A . GetCenter ( ) . Z + A . GetRadius ( ) * CenterDir . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , B . GetCenter ( ) . X - B . GetRadius ( ) * CenterDir . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , B . GetCenter ( ) . Y - B . GetRadius ( ) * CenterDir . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , B . GetCenter ( ) . Z - B . GetRadius ( ) * CenterDir . Z , Tolerance ) ;
}
// Success - very close not overlapping
{
2021-02-03 14:57:28 -04:00
TSphere < FReal , 3 > A ( FVec3 ( 12 , 0 , 0 ) , 5 ) ;
TSphere < FReal , 3 > B ( FVec3 ( 4 , 0 , 0 ) , 2 ) ;
FVec3 BPos = FVec3 ( 0.99 , 0 , 0 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 ( BPos , FRotation3 : : FromIdentity ( ) ) , Distance , NearestA , NearestB , Normal ) ;
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 - BPos . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) 7 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) 6 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) 0 , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
}
TEST ( TestGJKDistance , GJKSphereSphereDistanceTest )
{
2021-02-03 14:57:28 -04:00
GJKSphereSphereDistanceTest ( ) ;
2020-01-10 12:41:31 -05:00
}
2021-02-03 14:57:28 -04:00
void GJKBoxSphereDistanceTest ( )
2020-01-10 12:41:31 -05:00
{
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 2e-3 ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
FVec3 NearestA = { 0 , 0 , 0 } ;
FVec3 NearestB = { 0 , 0 , 0 } ;
FReal Distance = 0 ;
2021-02-16 22:06:10 -04:00
FVec3 Normal = { 0 , 0 , 1 } ;
2020-01-10 12:41:31 -05:00
// Fail - overlapping
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 2 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 ( FVec3 ( 2 , 0 , 0 ) , FRotation3 : : FromIdentity ( ) ) , Distance , NearestA , NearestB , Normal ) ;
EXPECT_NE ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping - mid-face near point
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 2 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) 5 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) 4 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) 0 , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
// Other way round
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 2 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( B , A , FRigidTransform3 : : Identity , Distance , NearestB , NearestA , Normal ) ;
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) 5 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) 4 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) 0 , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping - vertex near point
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , 2 , 2 ) , FVec3 ( 8 , 4 , 4 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 2 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 NearPointOnA = A . Min ( ) ;
FVec3 SphereNearPointDir = ( NearPointOnA - B . GetCenter ( ) ) . GetSafeNormal ( ) ;
FVec3 NearPointOnB = B . GetCenter ( ) + SphereNearPointDir * B . GetRadius ( ) ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( NearPointOnA - NearPointOnB ) . Size ( ) , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , NearPointOnA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , NearPointOnA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , NearPointOnA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , NearPointOnB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , NearPointOnB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , NearPointOnB . Z , Tolerance ) ;
}
// Other way round
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , 2 , 2 ) , FVec3 ( 8 , 4 , 4 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 2 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( B , A , FRigidTransform3 : : Identity , Distance , NearestB , NearestA , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 NearPointOnA = A . Min ( ) ;
FVec3 SphereNearPointDir = ( NearPointOnA - B . GetCenter ( ) ) . GetSafeNormal ( ) ;
FVec3 NearPointOnB = B . GetCenter ( ) + SphereNearPointDir * B . GetRadius ( ) ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( NearPointOnA - NearPointOnB ) . Size ( ) , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , NearPointOnA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , NearPointOnA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , NearPointOnA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , NearPointOnB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , NearPointOnB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , NearPointOnB . Z , Tolerance ) ;
}
// Rotated
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( - 2 , - 2 , - 2 ) , FVec3 ( 4 , 4 , 4 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 0 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
FRigidTransform3 BToATm = FRigidTransform3 ( FVec3 ( 8 , 0 , 0 ) , FRotation3 : : FromAxisAngle ( FVec3 ( 0 , 1 , 0 ) , FMath : : DegreesToRadians ( 45 ) ) ) ; // Rotation won't affect contact depth, but does affect local contact position
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , BToATm , Distance , NearestA , NearestB , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 NearPointOnA = FVec3 ( 4 , 0 , 0 ) ;
FVec3 BPos = BToATm . TransformPositionNoScale ( B . GetCenter ( ) ) ;
FVec3 NearPointDir = ( NearPointOnA - BPos ) . GetSafeNormal ( ) ;
FVec3 NearPointOnB = BPos + NearPointDir * B . GetRadius ( ) ;
FVec3 NearPointOnBLocal = BToATm . InverseTransformPositionNoScale ( NearPointOnB ) ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( NearPointOnA - NearPointOnB ) . Size ( ) , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , NearPointOnA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , NearPointOnA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , NearPointOnA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , NearPointOnBLocal . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , NearPointOnBLocal . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , NearPointOnBLocal . Z , Tolerance ) ;
}
// Other way round
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( - 2 , - 2 , - 2 ) , FVec3 ( 4 , 4 , 4 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 0 , 0 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
FRigidTransform3 BToATm = FRigidTransform3 ( FVec3 ( - 8 , 0 , 0 ) , FRotation3 : : FromAxisAngle ( FVec3 ( 0 , 1 , 0 ) , FMath : : DegreesToRadians ( 45 ) ) ) ;
EGJKDistanceResult Result = GJKDistance < FReal > ( B , A , BToATm , Distance , NearestB , NearestA , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 NearPointOnA = FVec3 ( 4 , 0 , 4 ) ;
FVec3 BPos = BToATm . InverseTransformPositionNoScale ( B . GetCenter ( ) ) ;
FVec3 NearPointDir = ( NearPointOnA - BPos ) . GetSafeNormal ( ) ;
FVec3 NearPointOnB = BPos + NearPointDir * B . GetRadius ( ) ;
FVec3 NearPointOnBLocal = BToATm . TransformPositionNoScale ( NearPointOnB ) ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( NearPointOnA - NearPointOnB ) . Size ( ) , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , NearPointOnA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , NearPointOnA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , NearPointOnA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , NearPointOnBLocal . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , NearPointOnBLocal . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , NearPointOnBLocal . Z , Tolerance ) ;
}
// Success - specific test case that initially failed (using incorrect initialization of V which works for Overlap but not Distance)
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , 2 ) , FVec3 ( 8 , 2 , 4 ) ) ;
TSphere < FReal , 3 > B ( FVec3 ( 2 , 0 , 0 ) , 2 ) ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
bool bOverlap = GJKIntersection < FReal > ( A , B , FRigidTransform3 : : Identity ) ;
2020-01-10 12:41:31 -05:00
EXPECT_FALSE ( bOverlap ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 NearPointOnA = FVec3 ( 5 , 0 , 2 ) ;
FVec3 NearPointDir = ( NearPointOnA - B . GetCenter ( ) ) . GetSafeNormal ( ) ;
FVec3 NearPointOnB = B . GetCenter ( ) + NearPointDir * B . GetRadius ( ) ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( NearPointOnA - NearPointOnB ) . Size ( ) , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , NearPointOnA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , NearPointOnA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , NearPointOnA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , NearPointOnB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , NearPointOnB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , NearPointOnB . Z , Tolerance ) ;
}
}
TEST ( TestGJKDistance , GJKBoxSphereDistanceTest )
{
2021-02-03 14:57:28 -04:00
GJKBoxSphereDistanceTest ( ) ;
2020-01-10 12:41:31 -05:00
}
void GJKBoxCapsuleDistanceTest ( )
{
2021-02-03 14:57:28 -04:00
FVec3 NearestA = { 0 , 0 , 0 } ;
FVec3 NearestB = { 0 , 0 , 0 } ;
FReal Distance = 0 ;
2021-02-16 22:06:10 -04:00
FVec3 Normal = { 0 , 0 , 1 } ;
2020-01-10 12:41:31 -05:00
// Fail - overlapping
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
2021-03-05 19:27:14 -04:00
FCapsule B ( FVec3 ( 2 , - 2 , 0 ) , FVec3 ( 2 , 2 , 0 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 ( FVec3 ( 2 , 0 , 0 ) , FRotation3 : : FromIdentity ( ) ) , Distance , NearestA , NearestB , Normal ) ;
EXPECT_NE ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping, capsule axis parallel to nearest face (near points on cylinder and box face)
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
2021-03-05 19:27:14 -04:00
FCapsule B ( FVec3 ( 2 , 0 , - 1 ) , FVec3 ( 2 , 0 , 2 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 2e-3 ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) 5 , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_GT ( NearestA . Z , ( FReal ) - 2 - Tolerance ) ;
EXPECT_LT ( NearestA . Z , ( FReal ) 2 + Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) 4 , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) 0 , Tolerance ) ;
EXPECT_GT ( NearestB . Z , ( FReal ) - 1 - Tolerance ) ;
EXPECT_LT ( NearestB . Z , ( FReal ) 2 + Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping, capsule axis at angle to nearest face (near points on end-cap and box edge)
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
2021-03-05 19:27:14 -04:00
FCapsule B ( FVec3 ( - 2 , 0 , 3 ) , FVec3 ( 2 , 0 , - 3 ) , 2 ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 ExpectedNearestA = FVec3 ( 5 , 0 , - 2 ) ;
FVec3 ExpectedDir = ( ExpectedNearestA - B . GetX2 ( ) ) . GetSafeNormal ( ) ;
FVec3 ExpectedNearestB = B . GetX2 ( ) + ExpectedDir * B . GetRadius ( ) ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 2e-3 ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( ExpectedNearestB - ExpectedNearestA ) . Size ( ) , Tolerance ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( NearestA . X , ( FReal ) ExpectedNearestA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) ExpectedNearestA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , ( FReal ) ExpectedNearestA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) ExpectedNearestB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) ExpectedNearestB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) ExpectedNearestB . Z , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping, near point partway down wall of capsule
{
2021-03-05 19:27:14 -04:00
FCapsule A ( FVec3 ( 4 , 0 , - 1 ) , FVec3 ( 4 , 0 , - 7 ) , 1 ) ;
2021-02-03 14:57:28 -04:00
FAABB3 B ( FVec3 ( - 2 , - 2 , - 2 ) , FVec3 ( 2 , 2 , 2 ) ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 ExpectedNearestA = FVec3 ( 3 , 0 , ( FReal ) - 1.5 ) ;
FVec3 ExpectedNearestB = FVec3 ( 2 , 0 , ( FReal ) - 1.5 ) ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 2e-3 ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) ExpectedNearestA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) ExpectedNearestA . Y , Tolerance ) ;
EXPECT_LT ( NearestA . Z , ( FReal ) ExpectedNearestA . Z + ( FReal ) 0.5 + Tolerance ) ;
EXPECT_GT ( NearestA . Z , ( FReal ) ExpectedNearestA . Z - ( FReal ) 0.5 - Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) ExpectedNearestB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) ExpectedNearestB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) NearestA . Z , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
// Success - not overlapping, near point partway down wall of capsule.
// Same result as above, but using transform rather than the shape's built-in offsets.
{
2021-03-05 19:27:14 -04:00
FCapsule A ( FVec3 ( 0 , 0 , - 3 ) , FVec3 ( 0 , 0 , 3 ) , 1 ) ;
2021-02-03 14:57:28 -04:00
FAABB3 B ( FVec3 ( - 2 , - 2 , - 2 ) , FVec3 ( 2 , 2 , 2 ) ) ;
FRigidTransform3 BToA = FRigidTransform3 ( FVec3 ( - 4 , 0 , 4 ) , FRotation3 : : FromIdentity ( ) ) ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , BToA , Distance , NearestA , NearestB , Normal ) ;
2021-02-03 14:57:28 -04:00
FVec3 ExpectedNearestA = FVec3 ( - 1 , 0 , ( FReal ) 2 ) ;
FVec3 ExpectedNearestB = FVec3 ( 2 , 0 , ( FReal ) - 2 ) ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 2e-3 ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( Distance , ( FReal ) 1 , Tolerance ) ;
EXPECT_NEAR ( NearestA . X , ( FReal ) ExpectedNearestA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) ExpectedNearestA . Y , Tolerance ) ;
EXPECT_LT ( NearestA . Z , ( FReal ) ExpectedNearestA . Z + ( FReal ) 0.5 + Tolerance ) ;
EXPECT_GT ( NearestA . Z , ( FReal ) ExpectedNearestA . Z - ( FReal ) 0.5 - Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) ExpectedNearestB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) ExpectedNearestB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z + BToA . GetTranslation ( ) . Z , ( FReal ) NearestA . Z , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
}
TEST ( TestGJKDistance , GJKBoxCapsuleDistanceTest )
{
2021-02-03 14:57:28 -04:00
GJKBoxCapsuleDistanceTest ( ) ;
2020-01-10 12:41:31 -05:00
}
void GJKBoxCapsuleDistanceIterationCountTest ( )
{
2021-02-03 14:57:28 -04:00
FVec3 NearestA = { 0 , 0 , 0 } ;
FVec3 NearestB = { 0 , 0 , 0 } ;
FReal Distance = 0 ;
2021-02-16 22:06:10 -04:00
FVec3 Normal = { 0 , 0 , 1 } ;
2020-01-10 12:41:31 -05:00
// Capsule-box takes number of iterations at the moment (we can improve that with a better the choice of Initial V)
// so test that we still get an approximate answer with less iterations
{
2021-02-03 14:57:28 -04:00
FAABB3 A ( FVec3 ( 5 , - 2 , - 2 ) , FVec3 ( 8 , 2 , 2 ) ) ;
2021-03-05 19:27:14 -04:00
FCapsule B ( FVec3 ( - 2 , 0 , 3 ) , FVec3 ( 2 , 0 , - 3 ) , 2 ) ;
2021-02-03 14:57:28 -04:00
FReal Epsilon = ( FReal ) 1e-6 ;
2020-01-10 12:41:31 -05:00
int32 MaxIts = 5 ;
2021-02-16 22:06:10 -04:00
EGJKDistanceResult Result = GJKDistance < FReal > ( A , B , FRigidTransform3 : : Identity , Distance , NearestA , NearestB , Normal , Epsilon , MaxIts ) ;
2021-02-03 14:57:28 -04:00
FVec3 ExpectedNearestA = FVec3 ( 5 , 0 , - 2 ) ;
FVec3 ExpectedDir = ( ExpectedNearestA - B . GetX2 ( ) ) . GetSafeNormal ( ) ;
FVec3 ExpectedNearestB = B . GetX2 ( ) + ExpectedDir * B . GetRadius ( ) ;
2020-01-10 12:41:31 -05:00
2021-02-03 14:57:28 -04:00
const FReal Tolerance = ( FReal ) 0.3 ;
2021-02-16 22:06:10 -04:00
EXPECT_EQ ( Result , EGJKDistanceResult : : Separated ) ;
2020-01-10 12:41:31 -05:00
EXPECT_NEAR ( Distance , ( ExpectedNearestB - ExpectedNearestA ) . Size ( ) , Tolerance ) ;
2021-02-03 14:57:28 -04:00
EXPECT_NEAR ( NearestA . X , ( FReal ) ExpectedNearestA . X , Tolerance ) ;
EXPECT_NEAR ( NearestA . Y , ( FReal ) ExpectedNearestA . Y , Tolerance ) ;
EXPECT_NEAR ( NearestA . Z , ( FReal ) ExpectedNearestA . Z , Tolerance ) ;
EXPECT_NEAR ( NearestB . X , ( FReal ) ExpectedNearestB . X , Tolerance ) ;
EXPECT_NEAR ( NearestB . Y , ( FReal ) ExpectedNearestB . Y , Tolerance ) ;
EXPECT_NEAR ( NearestB . Z , ( FReal ) ExpectedNearestB . Z , Tolerance ) ;
2020-01-10 12:41:31 -05:00
}
}
TEST ( TestGJKDistance , GJKBoxCapsuleDistanceIterationCountTest )
{
2021-02-03 14:57:28 -04:00
GJKBoxCapsuleDistanceIterationCountTest ( ) ;
2020-01-10 12:41:31 -05:00
}
}