diff --git a/engines/basic/include/shader_controls.h b/engines/basic/include/shader_controls.h index 77937252..f0e9677d 100644 --- a/engines/basic/include/shader_controls.h +++ b/engines/basic/include/shader_controls.h @@ -17,7 +17,7 @@ namespace dbasic { }; struct ShaderObjectVariables { - ysMatrix Transform = ysMath::LoadIdentity(); + ysMatrix Transform = ysMath::Constants::Identity; float TexOffset[2] = { 0.0f, 0.0f }; float TexScale[2] = { 1.0f, 1.0f }; @@ -43,8 +43,8 @@ namespace dbasic { }; struct ShaderScreenVariables { - ysMatrix CameraView = ysMath::LoadIdentity(); - ysMatrix Projection = ysMath::LoadIdentity(); + ysMatrix CameraView = ysMath::Constants::Identity; + ysMatrix Projection = ysMath::Constants::Identity; ysVector4 Eye = { 0.0f, 0.0f, 0.0f }; }; diff --git a/engines/basic/src/scene_object_asset.cpp b/engines/basic/src/scene_object_asset.cpp index 47f96e3d..45230eb0 100644 --- a/engines/basic/src/scene_object_asset.cpp +++ b/engines/basic/src/scene_object_asset.cpp @@ -9,7 +9,7 @@ dbasic::SceneObjectAsset::SceneObjectAsset() : ysObject("SceneObjectAsset") { m_skeletonIndex = -1; m_type = ysObjectData::ObjectType::Undefined; - m_localTransform = ysMath::LoadIdentity(); + m_localTransform = ysMath::Constants::Identity; m_localOrientation = ysMath::Constants::QuatIdentity; m_localPosition = ysMath::Constants::Zero; diff --git a/include/yds_expanding_array.h b/include/yds_expanding_array.h index 0c9dff3f..b99cde11 100644 --- a/include/yds_expanding_array.h +++ b/include/yds_expanding_array.h @@ -12,7 +12,7 @@ class ysExpandingArray { ysExpandingArray() { m_maxSize = START_SIZE; m_nObjects = 0; - m_array = NULL; + m_array = nullptr; Preallocate(m_maxSize); } @@ -140,7 +140,7 @@ class ysExpandingArray { } bool IsActive() const { - return (m_array != NULL); + return (m_array != nullptr); } void Delete(int index, bool maintainOrder = false) { @@ -149,7 +149,7 @@ class ysExpandingArray { m_array[i] = m_array[i + 1]; } - m_array[m_nObjects - 1] = NULL; + m_array[m_nObjects - 1] = nullptr; } else { m_array[index] = m_array[m_nObjects - 1]; diff --git a/include/yds_math.h b/include/yds_math.h index 54b36768..1ab3a3da 100644 --- a/include/yds_math.h +++ b/include/yds_math.h @@ -159,6 +159,12 @@ namespace ysMath { YS_MATH_CONST ysVector Half = { 0.5f, 0.5f, 0.5f, 0.5f }; YS_MATH_CONST ysVector Double = { 2.0f, 2.0f, 2.0f, 2.0f }; + YS_MATH_CONST ysMatrix Identity = { + IdentityRow1, + IdentityRow2, + IdentityRow3, + IdentityRow4 }; + // Numeral Constants YS_MATH_CONST float PI = 3.141592654f; YS_MATH_CONST float TWO_PI = 6.2831853071795864769252866f; diff --git a/physics/include/collision_detector.h b/physics/include/collision_detector.h index ecf92a67..fb98a0ae 100644 --- a/physics/include/collision_detector.h +++ b/physics/include/collision_detector.h @@ -11,17 +11,16 @@ namespace dphysics { CollisionDetector(); ~CollisionDetector(); - bool BoxBoxCollision(Collision &collision, RigidBody *body1, RigidBody *body2, BoxPrimitive *box1, BoxPrimitive *box2); - bool CircleBoxCollision(Collision &collision, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle, BoxPrimitive *box); - bool CircleCircleCollision(Collision &collision, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle1, CirclePrimitive *circle2); - bool RayBoxCollision(Collision &collision, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, BoxPrimitive *box); - bool RayCircleCollision(Collision &collision, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, CirclePrimitive *circle); + int BoxBoxCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, BoxPrimitive *box1, BoxPrimitive *box2); + int CircleBoxCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle, BoxPrimitive *box); + int CircleCircleCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle1, CirclePrimitive *circle2); + int RayBoxCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, BoxPrimitive *box); + int RayCircleCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, CirclePrimitive *circle); bool CircleCircleIntersect(RigidBody *body1, RigidBody *body2, CirclePrimitive *circle1, CirclePrimitive *circle2); protected: - bool _BoxBoxCollision(Collision *collision, BoxPrimitive *body1, BoxPrimitive *body2); - bool _BoxBoxEdgeDetect(Collision *collisions, BoxPrimitive *body1, BoxPrimitive *body2); + int _BoxBoxCollision(Collision *collisions, BoxPrimitive *body1, BoxPrimitive *body2); }; } /* namesapce dbasic */ diff --git a/physics/include/collision_primitives.h b/physics/include/collision_primitives.h index 19bf64cb..b6a1fdca 100644 --- a/physics/include/collision_primitives.h +++ b/physics/include/collision_primitives.h @@ -36,11 +36,23 @@ namespace dphysics { class Collision : public ysObject { friend RigidBodySystem; + public: + enum class CollisionType { + PointFace, + EdgeEdge, + Generic, + Unknown + }; + public: Collision(); - Collision(Collision &collision); + Collision(const Collision &collision); ~Collision(); + CollisionType m_collisionType; + int m_feature1; + int m_feature2; + float m_penetration; ysVector m_normal; ysVector m_position; @@ -80,6 +92,8 @@ namespace dphysics { ysVector GetContactVelocity() const { return m_initialContactVelocity; } ysVector GetContactVelocityWorld() const; + bool IsSameAs(Collision *other) const; + protected: ysVector m_relativePosition[2]; ysMatrix m_contactSpace; diff --git a/physics/include/rigid_body.h b/physics/include/rigid_body.h index 08ad329e..865e1620 100644 --- a/physics/include/rigid_body.h +++ b/physics/include/rigid_body.h @@ -139,6 +139,8 @@ namespace dphysics { bool CheckState(); + Collision *FindMatchingCollision(Collision *collision); + protected: // Properties bool m_registered; diff --git a/physics/include/rigid_body_system.h b/physics/include/rigid_body_system.h index 537c53d2..db8d326b 100644 --- a/physics/include/rigid_body_system.h +++ b/physics/include/rigid_body_system.h @@ -62,8 +62,12 @@ namespace dphysics { void CloseReplayFile(); protected: + bool CollisionExists(Collision *collision); + void GenerateCollisions(); void InitializeCollisions(); + void CleanCollisions(); + void ClearCollisions(); void GenerateCollisions(RigidBody *body1, RigidBody *body2); void ResolveCollisions(float dt); diff --git a/physics/src/collision_detector.cpp b/physics/src/collision_detector.cpp index cca3187e..823440b4 100644 --- a/physics/src/collision_detector.cpp +++ b/physics/src/collision_detector.cpp @@ -28,40 +28,47 @@ bool dphysics::CollisionDetector::CircleCircleIntersect(RigidBody *body1, RigidB return false; } -bool dphysics::CollisionDetector::BoxBoxCollision(Collision &collision, RigidBody *body1, RigidBody *body2, BoxPrimitive *box1, BoxPrimitive *box2) { - Collision collision1; - Collision collision2; - - bool collision1Valid = false; - bool collision2Valid = false; - - collision1Valid = _BoxBoxCollision(&collision1, box1, box2); - collision2Valid = _BoxBoxCollision(&collision2, box2, box1); - - if (collision1Valid) { - if (!collision2Valid || (collision2Valid && collision2.m_penetration >= collision1.m_penetration)) { - collision.m_body1 = body1; - collision.m_body2 = body2; - collision.m_normal = collision1.m_normal; - collision.m_penetration = collision1.m_penetration; - collision.m_position = collision1.m_position; +int dphysics::CollisionDetector::BoxBoxCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, BoxPrimitive *box1, BoxPrimitive *box2) { + Collision collisions1[2]; + Collision collisions2[2]; + + int n1 = _BoxBoxCollision(collisions1, box1, box2); + int n2 = _BoxBoxCollision(collisions2, box2, box1); + + float minPenetration1 = (n1 > 0) + ? collisions1->m_penetration + : FLT_MAX; + float minPenetration2 = (n2 > 0) + ? collisions2->m_penetration + : FLT_MAX; + + int n = 0; + if (n1 > 0) { + if (minPenetration2 >= minPenetration1) { + for (int i = 0; i < n1; ++i) { + collisions[i] = collisions1[i]; + collisions[i].m_body1 = body1; + collisions[i].m_body2 = body2; + } + n = n1; } } - if (collision2Valid) { - if (!collision1Valid || (collision1Valid && collision1.m_penetration >= collision2.m_penetration)) { - collision.m_body1 = body2; - collision.m_body2 = body1; - collision.m_normal = collision2.m_normal; - collision.m_penetration = collision2.m_penetration; - collision.m_position = collision2.m_position; + if (n2 > 0) { + if (minPenetration2 <= minPenetration1) { + for (int i = 0; i < n2; ++i) { + collisions[i] = collisions2[i]; + collisions[i].m_body1 = body2; + collisions[i].m_body2 = body1; + } + n = n2; } } - return (collision1Valid || collision2Valid); + return n; } -bool dphysics::CollisionDetector::CircleBoxCollision(Collision &collision, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle, BoxPrimitive *box) { +int dphysics::CollisionDetector::CircleBoxCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle, BoxPrimitive *box) { constexpr float epsilon = 1E-5f; ysVector relativePosition = ysMath::Sub(circle->Position, box->Position); @@ -76,7 +83,7 @@ bool dphysics::CollisionDetector::CircleBoxCollision(Collision &collision, Rigid float d0 = ysMath::GetScalar(ysMath::MagnitudeSquared3(ysMath::Sub(circle->Position, box->Position))); float d2 = ysMath::GetScalar(ysMath::MagnitudeSquared3(ysMath::Sub(circle->Position, realPosition))); - if (d2 > circle->Radius * circle->Radius) return false; + if (d2 > circle->Radius * circle->Radius) return 0; ysVector normal; if (d0 <= epsilon) { @@ -91,16 +98,17 @@ bool dphysics::CollisionDetector::CircleBoxCollision(Collision &collision, Rigid normal = ysMath::Mask(normal, ysMath::Constants::MaskOffZ); } - collision.m_normal = ysMath::Normalize(normal); - collision.m_body1 = body1; - collision.m_body2 = body2; - collision.m_penetration = circle->Radius - ysMath::GetScalar(ysMath::Magnitude(normal)); - collision.m_position = realPosition; + collisions[0].m_normal = ysMath::Normalize(normal); + collisions[0].m_body1 = body1; + collisions[0].m_body2 = body2; + collisions[0].m_penetration = circle->Radius - ysMath::GetScalar(ysMath::Magnitude(normal)); + collisions[0].m_position = realPosition; + collisions[0].m_collisionType = Collision::CollisionType::Generic; - return true; + return 1; } -bool dphysics::CollisionDetector::CircleCircleCollision(Collision &collision, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle1, CirclePrimitive *circle2) { +int dphysics::CollisionDetector::CircleCircleCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, CirclePrimitive *circle1, CirclePrimitive *circle2) { ysVector delta = ysMath::Sub(circle2->Position, circle1->Position); delta = ysMath::Mask(delta, ysMath::Constants::MaskOffZ); @@ -117,15 +125,14 @@ bool dphysics::CollisionDetector::CircleCircleCollision(Collision &collision, Ri float combinedRadius = circle1->Radius + circle2->Radius; - //if ((s_distance * s_distance) > (circle1->RadiusSquared - circle2->RadiusSquared)) || - if ((s_distance * s_distance) < combinedRadius * combinedRadius) - { - collision.m_body1 = body1; - collision.m_body2 = body2; - collision.m_normal = ysMath::Negate3(direction); - collision.m_penetration = circle1->Radius + circle2->Radius - s_distance; - collision.m_position = + if ((s_distance * s_distance) < combinedRadius * combinedRadius) { + collisions[0].m_body1 = body1; + collisions[0].m_body2 = body2; + collisions[0].m_normal = ysMath::Negate3(direction); + collisions[0].m_penetration = circle1->Radius + circle2->Radius - s_distance; + collisions[0].m_position = ysMath::Add(circle1->Position, ysMath::Mul(direction, ysMath::LoadScalar(circle1->Radius))); + collisions[0].m_collisionType = Collision::CollisionType::Generic; return true; } @@ -133,13 +140,13 @@ bool dphysics::CollisionDetector::CircleCircleCollision(Collision &collision, Ri return false; } -bool dphysics::CollisionDetector::RayBoxCollision(Collision &collision, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, BoxPrimitive *box) { +int dphysics::CollisionDetector::RayBoxCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, BoxPrimitive *box) { /* TODO */ - return false; + return 0; } -bool dphysics::CollisionDetector::RayCircleCollision(Collision &collision, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, CirclePrimitive *circle) { +int dphysics::CollisionDetector::RayCircleCollision(Collision *collisions, RigidBody *body1, RigidBody *body2, RayPrimitive *ray, CirclePrimitive *circle) { ysVector D = ray->Direction; ysVector P = ray->Position; ysVector C = circle->Position; @@ -165,17 +172,15 @@ bool dphysics::CollisionDetector::RayCircleCollision(Collision &collision, Rigid else if (t2 < 0) closest = t1; else closest = min(t1, t2); - collision.m_body1 = body1; - collision.m_body2 = body2; - collision.m_penetration = closest; - collision.m_position = ray->Position; + collisions[0].m_body1 = body1; + collisions[0].m_body2 = body2; + collisions[0].m_penetration = closest; + collisions[0].m_position = ray->Position; return true; } -bool dphysics::CollisionDetector::_BoxBoxCollision(Collision *collision, BoxPrimitive *body1, BoxPrimitive *body2) { - bool ret = false; - +int dphysics::CollisionDetector::_BoxBoxCollision(Collision *collisions, BoxPrimitive *body1, BoxPrimitive *body2) { ysVector point1 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(body1->HalfWidth, body1->HalfHeight)), body1->Position); ysVector point2 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(-body1->HalfWidth, body1->HalfHeight)), body1->Position); ysVector point3 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(body1->HalfWidth, -body1->HalfHeight)), body1->Position); @@ -224,8 +229,8 @@ bool dphysics::CollisionDetector::_BoxBoxCollision(Collision *collision, BoxPrim penetrationY[i] = ysMath::GetScalar(projy); } - Collision &retcol = *collision; float smallestPenetration = FLT_MAX; + Collision rawCollisions[4]; for (int i = 0; i < 4; i++) { float norX_1 = ysMath::GetX(normals_1[i]); @@ -233,172 +238,73 @@ bool dphysics::CollisionDetector::_BoxBoxCollision(Collision *collision, BoxPrim float norX_2 = ysMath::GetX(normals_2[i]); float norY_2 = ysMath::GetY(normals_2[i]); + Collision &col = rawCollisions[i]; + rawCollisions[i].m_collisionType = Collision::CollisionType::Unknown; + if (abs(penetrationX[i]) <= body2->HalfWidth && abs(penetrationY[i]) <= body2->HalfHeight) { float actualPenetrationX = body2->HalfWidth - abs(penetrationX[i]); float actualPenetrationY = body2->HalfHeight - abs(penetrationY[i]); - if (actualPenetrationX < smallestPenetration) { - if (norX_2 >= THRESH_0_NEGATIVE && norX_1 >= THRESH_0_NEGATIVE && penetrationX[i] < 0) { - smallestPenetration = actualPenetrationX; - ret = true; + bool foundValid = false; + if (norX_2 >= THRESH_0_NEGATIVE && norX_1 >= THRESH_0_NEGATIVE && penetrationX[i] < 0) { + smallestPenetration = min(actualPenetrationX, smallestPenetration); - retcol.m_normal = ysMath::Negate(xaxis); - retcol.m_penetration = smallestPenetration; - retcol.m_position = points[i]; - } - else if (norX_2 <= THRESH_0_POSITIVE && norX_1 <= THRESH_0_POSITIVE && penetrationX[i] > 0) { - smallestPenetration = actualPenetrationX; - ret = true; + col.m_normal = ysMath::Negate(xaxis); + col.m_penetration = actualPenetrationX; + col.m_position = points[i]; + col.m_collisionType = Collision::CollisionType::PointFace; + col.m_feature1 = i; + col.m_feature2 = -1; - retcol.m_normal = xaxis; - retcol.m_penetration = smallestPenetration; - retcol.m_position = points[i]; - } + foundValid = true; } + else if (norX_2 <= THRESH_0_POSITIVE && norX_1 <= THRESH_0_POSITIVE && penetrationX[i] > 0) { + smallestPenetration = min(actualPenetrationX, smallestPenetration); - if (actualPenetrationY < smallestPenetration) { - if (norY_2 >= THRESH_0_NEGATIVE && norY_1 >= THRESH_0_NEGATIVE && penetrationY[i] < 0) { - smallestPenetration = actualPenetrationY; - ret = true; + col.m_normal = xaxis; + col.m_penetration = actualPenetrationX; + col.m_position = points[i]; + col.m_collisionType = Collision::CollisionType::PointFace; + col.m_feature1 = i; + col.m_feature2 = -1; - retcol.m_normal = ysMath::Negate(yaxis); - retcol.m_penetration = smallestPenetration; - retcol.m_position = points[i]; + foundValid = true; + } + if (actualPenetrationY < actualPenetrationX || !foundValid) { + if (norY_2 >= THRESH_0_NEGATIVE && norY_1 >= THRESH_0_NEGATIVE && penetrationY[i] < 0) { + smallestPenetration = min(actualPenetrationY, smallestPenetration); + + col.m_normal = ysMath::Negate(yaxis); + col.m_penetration = actualPenetrationY; + col.m_position = points[i]; + col.m_collisionType = Collision::CollisionType::PointFace; + col.m_feature1 = i; + col.m_feature2 = -1; } else if (norY_2 <= THRESH_0_POSITIVE && norY_1 <= THRESH_0_POSITIVE && penetrationY[i] > 0) { - smallestPenetration = actualPenetrationY; - ret = true; - - retcol.m_normal = yaxis; - retcol.m_penetration = smallestPenetration; - retcol.m_position = points[i]; + smallestPenetration = min(actualPenetrationY, smallestPenetration); + + col.m_normal = yaxis; + col.m_penetration = actualPenetrationY; + col.m_position = points[i]; + col.m_collisionType = Collision::CollisionType::PointFace; + col.m_feature1 = i; + col.m_feature2 = -1; } } } } - return ret; -} - -bool dphysics::CollisionDetector::_BoxBoxEdgeDetect(Collision *collisions, BoxPrimitive *body1, BoxPrimitive *body2) { - bool ret = false; - - ysVector point1 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(body1->HalfWidth, body1->HalfHeight)), body1->Position); - ysVector point2 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(-body1->HalfWidth, body1->HalfHeight)), body1->Position); - ysVector point3 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(body1->HalfWidth, -body1->HalfHeight)), body1->Position); - ysVector point4 = ysMath::Add(ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(-body1->HalfWidth, -body1->HalfHeight)), body1->Position); - - ysVector edgeNormal_1 = ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(1.0f, 0.0f)); - ysVector edgeNormal_2 = ysMath::Negate(edgeNormal_1); - ysVector edgeNormal_3 = ysMath::QuatTransform(body1->Orientation, ysMath::LoadVector(0.0f, 1.0f)); - ysVector edgeNormal_4 = ysMath::Negate(edgeNormal_3); - - ysVector normal1_1 = edgeNormal_1; - ysVector normal2_1 = edgeNormal_2; - ysVector normal3_1 = edgeNormal_1; - ysVector normal4_1 = edgeNormal_2; - - ysVector normal1_2 = edgeNormal_3; - ysVector normal2_2 = edgeNormal_3; - ysVector normal3_2 = edgeNormal_4; - ysVector normal4_2 = edgeNormal_4; - - ysVector points[] = { point1, point2, point3, point4 }; - int edges[][2] = { {0, 2}, {1, 3}, {0, 1}, {2, 3} }; - ysVector edgeNormals[] = { edgeNormal_1, edgeNormal_2, edgeNormal_3, edgeNormal_4 }; - ysVector normals_1[] = { normal1_1, normal2_1, normal3_1, normal4_1 }; - ysVector normals_2[] = { normal1_2, normal2_2, normal3_2, normal4_2 }; - - float penetrationX[4]; - float penetrationY[4]; - - ysVector xaxis = ysMath::Constants::XAxis; - ysVector yaxis = ysMath::Constants::YAxis; - xaxis = ysMath::QuatTransform(body2->Orientation, xaxis); - yaxis = ysMath::QuatTransform(body2->Orientation, yaxis); - - ysVector del = ysMath::Sub(body2->Position, body1->Position); - ysVector delDir = ysMath::Normalize(del); - - int numCollisions = 0; - - for (int i = 0; i < 4; i++) { - ysVector relPoint = ysMath::Sub(points[i], body2->Position); - - ysVector projx = ysMath::Dot(relPoint, xaxis); - ysVector projy = ysMath::Dot(relPoint, yaxis); - - penetrationX[i] = ysMath::GetScalar(projx); - penetrationY[i] = ysMath::GetScalar(projy); - } - - float smallestPenetration = FLT_MAX; - for (int i = 0; i < 4; i++) { - float norX_1 = ysMath::GetX(normals_1[i]); - float norY_1 = ysMath::GetY(normals_1[i]); - float norX_2 = ysMath::GetX(normals_2[i]); - float norY_2 = ysMath::GetY(normals_2[i]); - - int *edge = edges[i]; - - if ((abs(penetrationX[edge[0]]) <= body2->HalfWidth && abs(penetrationX[edge[1]]) <= body2->HalfWidth) && - (min(penetrationY[edge[0]], penetrationY[edge[1]]) <= body2->HalfHeight) && - (max(penetrationY[edge[0]], penetrationY[edge[1]]) >= -body2->HalfHeight)) { - - float actualPenetrationX0 = body2->HalfWidth - abs(penetrationX[edge[0]]); - float actualPenetrationX1 = body2->HalfWidth - abs(penetrationX[edge[1]]); - float actualPenetrationX = (actualPenetrationX0 < actualPenetrationX1) ? actualPenetrationX0 : actualPenetrationX1; - - if (actualPenetrationX < smallestPenetration) { - numCollisions = 0; - if (abs(penetrationY[edge[0]]) <= body2->HalfHeight) { - collisions[numCollisions].m_penetration = actualPenetrationX0; - collisions[numCollisions].m_position = points[edge[0]]; - collisions[numCollisions].m_normal = ysMath::Negate(edgeNormals[i]); - numCollisions++; - } - - if (abs(penetrationY[edge[1]]) <= body2->HalfHeight) { - collisions[numCollisions].m_penetration = actualPenetrationX1; - collisions[numCollisions].m_position = points[edge[1]]; - collisions[numCollisions].m_normal = ysMath::Negate(edgeNormals[i]); - numCollisions++; - } - smallestPenetration = actualPenetrationX; - } - - ret = true; - } - - if ((abs(penetrationY[edge[0]]) <= body2->HalfHeight && abs(penetrationY[edge[1]]) <= body2->HalfHeight) && - (min(penetrationX[edge[0]], penetrationX[edge[1]]) <= body2->HalfWidth) && - (max(penetrationX[edge[0]], penetrationX[edge[1]]) >= -body2->HalfWidth)) { - - float actualPenetrationY0 = body2->HalfHeight - abs(penetrationY[edge[0]]); - float actualPenetrationY1 = body2->HalfHeight - abs(penetrationY[edge[1]]); - float actualPenetrationY = (actualPenetrationY0 < actualPenetrationY1) ? actualPenetrationY0 : actualPenetrationY1; - - if (actualPenetrationY < smallestPenetration) { - numCollisions = 0; - if (abs(penetrationX[edge[0]]) <= body2->HalfWidth) { - collisions[numCollisions].m_penetration = actualPenetrationY0; - collisions[numCollisions].m_position = points[edge[0]]; - collisions[numCollisions].m_normal = ysMath::Negate(edgeNormals[i]); - numCollisions++; - } - - if (abs(penetrationX[edge[1]]) <= body2->HalfWidth) { - collisions[numCollisions].m_penetration = actualPenetrationY1; - collisions[numCollisions].m_position = points[edge[1]]; - collisions[numCollisions].m_normal = ysMath::Negate(edgeNormals[i]); - numCollisions++; - } - smallestPenetration = actualPenetrationY; - } + int nCollisions = 0; + for (int i = 0; i < 4; ++i) { + if (nCollisions >= 2) break; + if (rawCollisions[i].m_collisionType == Collision::CollisionType::Unknown) continue; - ret = true; + if (std::abs(rawCollisions[i].m_penetration - smallestPenetration) < 1E-4) { + collisions[nCollisions] = rawCollisions[i]; + ++nCollisions; } } - return ret; + return nCollisions; } diff --git a/physics/src/collision_primitives.cpp b/physics/src/collision_primitives.cpp index 380d9dd0..ac2e2683 100644 --- a/physics/src/collision_primitives.cpp +++ b/physics/src/collision_primitives.cpp @@ -22,10 +22,14 @@ dphysics::Collision::Collision() : ysObject("Collision") { m_staticFriction = 0.0f; m_restitution = 0.0f; m_contactVelocity = ysMath::Constants::Zero; - m_contactSpace = ysMath::LoadIdentity(); + m_contactSpace = ysMath::Constants::Identity; + + m_collisionType = CollisionType::Unknown; + m_feature1 = -1; + m_feature2 = -1; } -dphysics::Collision::Collision(Collision &collision) : ysObject("Collision") { +dphysics::Collision::Collision(const Collision &collision) : ysObject("Collision") { m_penetration = collision.m_penetration; m_relativePosition[0] = collision.m_relativePosition[0]; m_relativePosition[1] = collision.m_relativePosition[1]; @@ -49,7 +53,11 @@ dphysics::Collision::Collision(Collision &collision) : ysObject("Collision") { m_initialContactVelocity = collision.m_initialContactVelocity; m_contactVelocity = collision.m_contactVelocity; - m_contactSpace = ysMath::LoadIdentity(); + m_contactSpace = collision.m_contactSpace; + + m_collisionType = collision.m_collisionType; + m_feature1 = collision.m_feature1; + m_feature2 = collision.m_feature2; } dphysics::Collision::~Collision() { @@ -86,6 +94,8 @@ void dphysics::Collision::UpdateInternals(float timestep) { dphysics::Collision &dphysics::Collision::operator=(dphysics::Collision &collision) { m_penetration = collision.m_penetration; + m_relativePosition[0] = collision.m_relativePosition[0]; + m_relativePosition[1] = collision.m_relativePosition[1]; m_normal = collision.m_normal; m_position = collision.m_position; @@ -100,7 +110,17 @@ dphysics::Collision &dphysics::Collision::operator=(dphysics::Collision &collisi m_relativePosition[0] = collision.m_relativePosition[0]; m_relativePosition[1] = collision.m_relativePosition[1]; + m_dynamicFriction = collision.m_dynamicFriction; + m_staticFriction = collision.m_staticFriction; + m_restitution = collision.m_restitution; + m_initialContactVelocity = collision.m_initialContactVelocity; + m_contactVelocity = collision.m_contactVelocity; + m_contactSpace = collision.m_contactSpace; + + m_collisionType = collision.m_collisionType; + m_feature1 = collision.m_feature1; + m_feature2 = collision.m_feature2; return *this; } @@ -125,6 +145,20 @@ ysVector dphysics::Collision::GetContactVelocityWorld() const { return ysMath::MatMult(m_contactSpace, m_initialContactVelocity); } +bool dphysics::Collision::IsSameAs(Collision *other) const { + if (m_sensor != other->m_sensor) return false; + if (IsGhost() != other->IsGhost()) return false; + if (m_body1 != other->m_body1 && m_body1 != other->m_body2) return false; + if (m_body2 != other->m_body1 && m_body2 != other->m_body2) return false; + if (m_collisionObject1 != other->m_collisionObject1 && m_collisionObject1 != other->m_collisionObject2) return false; + if (m_collisionObject2 != other->m_collisionObject1 && m_collisionObject2 != other->m_collisionObject2) return false; + if (m_collisionType != other->m_collisionType) return false; + if (m_feature1 != other->m_feature1 && m_feature1 != other->m_feature2) return false; + if (m_feature2 != other->m_feature1 && m_feature2 != other->m_feature2) return false; + + return true; +} + void dphysics::Collision::CalculateDesiredDeltaVelocity(float timestep) { const static float VelocityLimit = 0.25f; diff --git a/physics/src/ledge_link.cpp b/physics/src/ledge_link.cpp index e7a5a835..39a5cfda 100644 --- a/physics/src/ledge_link.cpp +++ b/physics/src/ledge_link.cpp @@ -36,8 +36,8 @@ int dphysics::LedgeLink::GenerateCollisions(Collision *collisionArray) { retcol.m_collisionObject1 = nullptr; retcol.m_collisionObject2 = nullptr; retcol.m_restitution = 0.0f; - retcol.m_staticFriction = 10.0f; - retcol.m_dynamicFriction = 10.0f; + retcol.m_staticFriction = 100.0f; + retcol.m_dynamicFriction = 100.0f; return 1; } diff --git a/physics/src/rigid_body.cpp b/physics/src/rigid_body.cpp index d7fed6f8..c3018b3f 100644 --- a/physics/src/rigid_body.cpp +++ b/physics/src/rigid_body.cpp @@ -40,12 +40,20 @@ void dphysics::RigidBody::Integrate(float timeStep) { if (m_hint == RigidBodyHint::Static) return; else m_derivedValid = false; + ysVector vTimeStep = ysMath::LoadScalar(timeStep); + + ysQuaternion orientation = Transform.GetLocalOrientation(); + ysVector position = Transform.GetLocalPosition(); + orientation = ysMath::QuatAddScaled(orientation, m_angularVelocity, timeStep); + position = ysMath::Add(position, ysMath::Mul(m_velocity, vTimeStep)); + + Transform.SetOrientation(orientation); + Transform.SetPosition(position); + ysVector acceleration = m_acceleration; acceleration = ysMath::Add(acceleration, ysMath::Mul(m_forceAccum, ysMath::LoadScalar(m_inverseMass))); ysVector angularAcceleration = ysMath::MatMult(m_inverseInertiaTensor, m_torqueAccum); - ysVector vTimeStep = ysMath::LoadScalar(timeStep); - m_velocity = ysMath::Add(m_velocity, ysMath::Mul(m_impulseAccum, ysMath::LoadScalar(m_inverseMass))); m_angularVelocity = @@ -54,14 +62,6 @@ void dphysics::RigidBody::Integrate(float timeStep) { m_velocity = ysMath::Add(m_velocity, ysMath::Mul(acceleration, vTimeStep)); m_angularVelocity = ysMath::Add(m_angularVelocity, ysMath::Mul(angularAcceleration, vTimeStep)); - ysQuaternion orientation = Transform.GetLocalOrientation(); - ysVector position = Transform.GetLocalPosition(); - orientation = ysMath::QuatAddScaled(orientation, m_angularVelocity, timeStep); - position = ysMath::Add(position, ysMath::Mul(m_velocity, vTimeStep)); - - Transform.SetOrientation(orientation); - Transform.SetPosition(position); - m_angularVelocity = ysMath::Mul(m_angularVelocity, ysMath::LoadScalar(pow(m_angularDamping, timeStep))); m_velocity = ysMath::Mul(m_velocity, ysMath::LoadScalar(pow(m_linearDamping, timeStep))); @@ -235,6 +235,15 @@ bool dphysics::RigidBody::CheckState() { return true; } +dphysics::Collision *dphysics::RigidBody::FindMatchingCollision(Collision *collision) { + int collisionCount = m_collisions.GetNumObjects(); + for (int i = 0; i < collisionCount; ++i) { + if (m_collisions[i]->IsSameAs(collision)) return m_collisions[i]; + } + + return nullptr; +} + void dphysics::RigidBody::ClearAccumulators() { ClearForceAccumulator(); ClearTorqueAccumulator(); diff --git a/physics/src/rigid_body_system.cpp b/physics/src/rigid_body_system.cpp index 493fb5b2..0db7fedd 100644 --- a/physics/src/rigid_body_system.cpp +++ b/physics/src/rigid_body_system.cpp @@ -5,7 +5,7 @@ #include #include -float dphysics::RigidBodySystem::ResolutionPenetrationEpsilon = 10e-3f; +float dphysics::RigidBodySystem::ResolutionPenetrationEpsilon = 1e-4f; dphysics::RigidBodySystem::RigidBodySystem() : ysObject("RigidBodySystem") { m_currentStep = 0.1f; @@ -136,7 +136,12 @@ void dphysics::RigidBodySystem::GenerateCollisions(int start, int count) { const int REQUEST_THRESHOLD = 0; int rigidBodyCount = m_rigidBodyRegistry.GetNumObjects(); - std::vector> visited(rigidBodyCount, std::vector(rigidBodyCount, false)); + //std::vector> visited(rigidBodyCount, std::vector(rigidBodyCount, false)); + bool **visited = new bool *[rigidBodyCount]; + for (int i = 0; i < rigidBodyCount; ++i) { + visited[i] = new bool[rigidBodyCount]; + memset((void *)visited[i], 0, sizeof(bool) * rigidBodyCount); + } for (auto cell: m_gridPartitionSystem.m_gridCells) { GridCell *gridCell = cell.second; @@ -163,6 +168,11 @@ void dphysics::RigidBodySystem::GenerateCollisions(int start, int count) { gridCell->m_processed = true; } } + + for (int i = 0; i < rigidBodyCount; ++i) { + delete[] visited[i]; + } + delete[] visited; } void dphysics::RigidBodySystem::WriteFrameToReplayFile() { @@ -213,7 +223,8 @@ void dphysics::RigidBodySystem::GenerateCollisions(RigidBody *body1, RigidBody * // Check whether these objects have compatible collision layers/masks if (!prim1->CheckCollisionMask(prim2)) continue; - Collision newCollision; + int nCollisions = 0; + Collision newCollisions[4]; CollisionObject::Mode mode1 = prim1->GetMode(); CollisionObject::Mode mode2 = prim2->GetMode(); @@ -234,82 +245,26 @@ void dphysics::RigidBodySystem::GenerateCollisions(RigidBody *body1, RigidBody * if (mode1 == CollisionObject::Mode::Fine) { if (prim1->GetType() == CollisionObject::Type::Circle) { if (prim2->GetType() == CollisionObject::Type::Circle) { - bool collisionValid = CollisionDetector.CircleCircleCollision( - newCollision, + nCollisions = CollisionDetector.CircleCircleCollision( + newCollisions, body1Ord->GetRoot(), body2Ord->GetRoot(), prim1->GetAsCircle(), prim2->GetAsCircle()); - - if (collisionValid) { - Collision *newCollisionEntry = m_dynamicCollisions.NewGeneric(); - m_collisionAccumulator.New() = newCollisionEntry; - - body1->AddCollision(newCollisionEntry); - body2->AddCollision(newCollisionEntry); - - *newCollisionEntry = newCollision; - - newCollisionEntry->m_collisionObject1 = prim1; - newCollisionEntry->m_collisionObject2 = prim2; - newCollisionEntry->m_sensor = sensorTest; - newCollisionEntry->m_dynamicFriction = - GetDynamicFriction(body1->GetMaterial(), body2->GetMaterial()); - newCollisionEntry->m_staticFriction = - GetStaticFriction(body1->GetMaterial(), body2->GetMaterial()); - } } else if (prim2->GetType() == CollisionObject::Type::Box) { - bool collisionValid = CollisionDetector.CircleBoxCollision( - newCollision, + nCollisions = CollisionDetector.CircleBoxCollision( + newCollisions, body1Ord->GetRoot(), body2Ord->GetRoot(), prim1->GetAsCircle(), prim2->GetAsBox()); - - if (collisionValid) { - Collision *newCollisionEntry = m_dynamicCollisions.NewGeneric(); - m_collisionAccumulator.New() = newCollisionEntry; - - body1->AddCollision(newCollisionEntry); - body2->AddCollision(newCollisionEntry); - - *newCollisionEntry = newCollision; - - newCollisionEntry->m_collisionObject1 = prim1; - newCollisionEntry->m_collisionObject2 = prim2; - newCollisionEntry->m_sensor = sensorTest; - - newCollisionEntry->m_dynamicFriction = - GetDynamicFriction(body1->GetMaterial(), body2->GetMaterial()); - newCollisionEntry->m_staticFriction = - GetStaticFriction(body1->GetMaterial(), body2->GetMaterial()); - } } } } if (prim1->GetType() == CollisionObject::Type::Box) { if (prim2->GetType() == CollisionObject::Type::Box) { - bool collisionValid = CollisionDetector.BoxBoxCollision( - newCollision, + nCollisions = CollisionDetector.BoxBoxCollision( + newCollisions, body1Ord->GetRoot(), body2Ord->GetRoot(), prim1->GetAsBox(), prim2->GetAsBox()); - - if (collisionValid) { - Collision *newCollisionEntry = m_dynamicCollisions.NewGeneric(); - m_collisionAccumulator.New() = newCollisionEntry; - - body1->AddCollision(newCollisionEntry); - body2->AddCollision(newCollisionEntry); - - *newCollisionEntry = newCollision; - - newCollisionEntry->m_collisionObject1 = prim1; - newCollisionEntry->m_collisionObject2 = prim2; - newCollisionEntry->m_sensor = sensorTest; - - newCollisionEntry->m_dynamicFriction = - GetDynamicFriction(body1->GetMaterial(), body2->GetMaterial()); - newCollisionEntry->m_staticFriction = - GetStaticFriction(body1->GetMaterial(), body2->GetMaterial()); - } } } @@ -318,50 +273,51 @@ void dphysics::RigidBodySystem::GenerateCollisions(RigidBody *body1, RigidBody * { if (prim1->GetType() == CollisionObject::Type::Circle) { if (prim2->GetType() == CollisionObject::Type::Circle) { - bool sense = CollisionDetector.CircleCircleIntersect( + bool intersect = CollisionDetector.CircleCircleIntersect( body1Ord->GetRoot(), body2Ord->GetRoot(), prim1->GetAsCircle(), prim2->GetAsCircle()); - if (sense) { - Collision *newCollisionEntry = m_dynamicCollisions.NewGeneric(); - newCollisionEntry->m_body1 = body1Ord; - newCollisionEntry->m_body2 = body2Ord; - - m_collisionAccumulator.New() = newCollisionEntry; - if (body1->RequestsInformation()) body1->AddCollision(newCollisionEntry); - if (body2->RequestsInformation()) body2->AddCollision(newCollisionEntry); - - newCollisionEntry->m_collisionObject1 = prim1; - newCollisionEntry->m_collisionObject2 = prim2; - newCollisionEntry->m_sensor = sensorTest; - } + if (intersect) nCollisions = 1; + newCollisions[0].m_body1 = body1Ord; + newCollisions[0].m_body2 = body2Ord; + newCollisions[0].m_collisionObject1 = prim1; + newCollisions[0].m_collisionObject2 = prim2; if (mode1 == CollisionObject::Mode::Coarse) { coarsePresent = true; - coarseCollision = sense; + coarseCollision = nCollisions > 0; } } else if (prim2->GetType() == CollisionObject::Type::Ray) { - bool collisionValid = CollisionDetector.RayCircleCollision( - newCollision, + nCollisions = CollisionDetector.RayCircleCollision( + newCollisions, body2Ord->GetRoot(), body1Ord->GetRoot(), prim2->GetAsRay(), prim1->GetAsCircle()); + } + } + } - if (collisionValid) { - Collision *newCollisionEntry = m_dynamicCollisions.NewGeneric(); - m_collisionAccumulator.New() = newCollisionEntry; - - if (body1->RequestsInformation()) body1->AddCollision(newCollisionEntry); - if (body2->RequestsInformation()) body2->AddCollision(newCollisionEntry); + for (int i = 0; i < nCollisions; ++i) { + Collision *newCollisionEntry = m_dynamicCollisions.NewGeneric(); + m_collisionAccumulator.New() = newCollisionEntry; - *newCollisionEntry = newCollision; + *newCollisionEntry = newCollisions[i]; - newCollisionEntry->m_collisionObject1 = prim1; - newCollisionEntry->m_collisionObject2 = prim2; - newCollisionEntry->m_sensor = sensorTest; - } - } + if (!sensorTest || body1->RequestsInformation()) { + body1->AddCollision(newCollisionEntry); } + + if (!sensorTest || body2->RequestsInformation()) { + body2->AddCollision(newCollisionEntry); + } + + newCollisionEntry->m_collisionObject1 = prim1; + newCollisionEntry->m_collisionObject2 = prim2; + newCollisionEntry->m_sensor = sensorTest; + newCollisionEntry->m_dynamicFriction = + GetDynamicFriction(body1->GetMaterial(), body2->GetMaterial()); + newCollisionEntry->m_staticFriction = + GetStaticFriction(body1->GetMaterial(), body2->GetMaterial()); } } } @@ -370,8 +326,12 @@ void dphysics::RigidBodySystem::GenerateCollisions(RigidBody *body1, RigidBody * #define sgn(x) ( ((x) > 0.0f) ? 1.0f : -1.0f ) +bool dphysics::RigidBodySystem::CollisionExists(Collision *collision) { + return collision->m_body1->FindMatchingCollision(collision) != nullptr; +} + void dphysics::RigidBodySystem::GenerateCollisions() { - m_collisionAccumulator.Clear(); + ClearCollisions(); int nObjects = m_rigidBodyRegistry.GetNumObjects(); // Generate grid cells @@ -385,9 +345,6 @@ void dphysics::RigidBodySystem::GenerateCollisions() { m_rigidBodyRegistry.Get(i)->CollisionGeometry.UpdatePrimitives(); } - m_dynamicCollisions.Clear(); - m_collisionAccumulator.Clear(); - GenerateCollisions(0, 0); char buffer[1024]; @@ -431,6 +388,29 @@ void dphysics::RigidBodySystem::InitializeCollisions() { } } +void dphysics::RigidBodySystem::CleanCollisions() { + constexpr float MinPenetration = -0.1f; + + int numContacts = m_collisionAccumulator.GetNumObjects(); + for (int i = 0; i < numContacts; ++i) { + Collision &collision = *m_collisionAccumulator[i]; + + if (collision.m_penetration < MinPenetration || + collision.IsGhost() || + collision.m_sensor) + { + m_collisionAccumulator.Delete(i, false); + m_dynamicCollisions.Delete(collision.GetIndex()); + --numContacts; + } + } +} + +void dphysics::RigidBodySystem::ClearCollisions() { + m_collisionAccumulator.Clear(); + m_dynamicCollisions.Clear(); +} + void dphysics::RigidBodySystem::ResolveCollision(Collision *collision, ysVector *velocityChange, ysVector *rotationDirection, float rotationAmount[2], float penetration) { float angularLimit = 0.1f; float angularMove[2], linearMove[2]; @@ -562,7 +542,7 @@ void dphysics::RigidBodySystem::AdjustVelocities(float timestep) { // Match the awake state at the contact //c[index].matchAwakeState(); - // Do the resolution on the contact that came out top. + // Do the resolution on the contact with the largest desired velocity AdjustVelocity(biggestCollision, velocityChange, rotationChange); // With the change in velocity of the two bodies, the update of diff --git a/physics/test/collision_tests.cpp b/physics/test/collision_tests.cpp index b87b4989..fdddac83 100644 --- a/physics/test/collision_tests.cpp +++ b/physics/test/collision_tests.cpp @@ -19,13 +19,13 @@ TEST(CollisionTests, CircleCircleSanityCheck) { c2.Radius = 1.0f; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.CircleCircleCollision(collision, nullptr, nullptr, &c1, &c2); + dphysics::Collision collisions[2]; + int n = detector.CircleCircleCollision(collisions, nullptr, nullptr, &c1, &c2); - EXPECT_TRUE(colliding); - EXPECT_NEAR(collision.m_penetration, 1.5f, 1E-4); + EXPECT_TRUE(n == 1); + EXPECT_NEAR(collisions[0].m_penetration, 1.5f, 1E-4); - VecEq(collision.m_position, ysMath::LoadVector(2.0f, 0.0f, 0.0f, 0.0f)); + VecEq(collisions[0].m_position, ysMath::LoadVector(2.0f, 0.0f, 0.0f, 0.0f)); } TEST(CollisionTests, CircleCircleNotColliding) { @@ -39,10 +39,10 @@ TEST(CollisionTests, CircleCircleNotColliding) { c2.Radius = 1.0f; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.CircleCircleCollision(collision, nullptr, nullptr, &c1, &c2); + dphysics::Collision collisions[2]; + int n = detector.CircleCircleCollision(collisions, nullptr, nullptr, &c1, &c2); - EXPECT_FALSE(colliding); + EXPECT_TRUE(n == 0); } TEST(CollisionTests, BoxBoxSanityCheck) { @@ -60,12 +60,12 @@ TEST(CollisionTests, BoxBoxSanityCheck) { b2.Orientation = ysMath::Constants::QuatIdentity; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.BoxBoxCollision(collision, nullptr, nullptr, &b1, &b2); + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, nullptr, nullptr, &b1, &b2); - EXPECT_TRUE(colliding); - EXPECT_GE(collision.m_penetration, 0); - EXPECT_LE(collision.m_penetration, 0.001f); + EXPECT_TRUE(n > 0); + EXPECT_GE(collisions[0].m_penetration, 0); + EXPECT_LE(collisions[0].m_penetration, 0.001f); } TEST(CollisionTests, BoxBoxOffCenter) { @@ -83,12 +83,12 @@ TEST(CollisionTests, BoxBoxOffCenter) { b2.Orientation = ysMath::Constants::QuatIdentity; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.BoxBoxCollision(collision, nullptr, nullptr, &b1, &b2); + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, nullptr, nullptr, &b1, &b2); - EXPECT_TRUE(colliding); - EXPECT_GE(collision.m_penetration, 0); - EXPECT_LE(collision.m_penetration, 0.001f); + EXPECT_TRUE(n > 0); + EXPECT_GE(collisions[0].m_penetration, 0); + EXPECT_LE(collisions[1].m_penetration, 0.001f); } TEST(CollisionTests, BoxBoxOffCenterDifferentSizes) { @@ -108,12 +108,12 @@ TEST(CollisionTests, BoxBoxOffCenterDifferentSizes) { b2.Orientation = ysMath::Constants::QuatIdentity; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.BoxBoxCollision(collision, nullptr, nullptr, &b1, &b2); + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, nullptr, nullptr, &b1, &b2); - EXPECT_TRUE(colliding); - EXPECT_GE(collision.m_penetration, 0); - EXPECT_LE(collision.m_penetration, Penetration); + EXPECT_TRUE(n > 0); + EXPECT_GE(collisions[0].m_penetration, 0); + EXPECT_LE(collisions[1].m_penetration, Penetration); } TEST(CollisionTests, BoxBoxOffCenterDifferentSizesDeep) { @@ -136,14 +136,14 @@ TEST(CollisionTests, BoxBoxOffCenterDifferentSizesDeep) { dphysics::RigidBody b; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.BoxBoxCollision(collision, &a, &b, &b1, &b2); - - EXPECT_TRUE(colliding); - EXPECT_GE(collision.m_penetration, 0); - EXPECT_LE(collision.m_penetration, Penetration); - EXPECT_EQ(collision.m_body1, &b); - EXPECT_EQ(collision.m_body2, &a); + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, &a, &b, &b1, &b2); + + EXPECT_TRUE(n > 0); + EXPECT_GE(collisions[0].m_penetration, 0); + EXPECT_LE(collisions[0].m_penetration, Penetration); + EXPECT_EQ(collisions[0].m_body1, &b); + EXPECT_EQ(collisions[0].m_body2, &a); } TEST(CollisionTests, BoxBoxOffCenterDifferentSizesDeep2) { @@ -164,14 +164,14 @@ TEST(CollisionTests, BoxBoxOffCenterDifferentSizesDeep2) { dphysics::RigidBody b; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.BoxBoxCollision(collision, &b, &a, &b2, &b1); - - EXPECT_TRUE(colliding); - EXPECT_GE(collision.m_penetration, 0); - EXPECT_EQ(collision.m_body1, &a); - EXPECT_EQ(collision.m_body2, &b); - VecEq(collision.m_normal, ysMath::LoadVector(-1.0f, 0.0f, 0.0f)); + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, &b, &a, &b2, &b1); + + EXPECT_TRUE(n > 0); + EXPECT_GE(collisions[0].m_penetration, 0); + EXPECT_EQ(collisions[0].m_body1, &a); + EXPECT_EQ(collisions[0].m_body2, &b); + VecEq(collisions[0].m_normal, ysMath::LoadVector(-1.0f, 0.0f, 0.0f)); } TEST(CollisionTests, BoxBoxBigSmall) { @@ -192,11 +192,11 @@ TEST(CollisionTests, BoxBoxBigSmall) { dphysics::RigidBody b; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.BoxBoxCollision(collision, &b, &a, &b2, &b1); + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, &b, &a, &b2, &b1); - EXPECT_TRUE(colliding); - EXPECT_NEAR(collision.m_penetration, 0.1f, 1E-4); + EXPECT_TRUE(n == 2); + EXPECT_NEAR(collisions[0].m_penetration, 0.1f, 1E-4); } TEST(CollisionTests, RayCircleCollision) { @@ -213,22 +213,22 @@ TEST(CollisionTests, RayCircleCollision) { dphysics::RigidBody b; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.RayCircleCollision(collision, &b, &a, &b1, &b2); + dphysics::Collision collisions[2]; + int n = detector.RayCircleCollision(collisions, &b, &a, &b1, &b2); - EXPECT_TRUE(colliding); - EXPECT_NEAR(collision.m_penetration, 9.0f, 1E-4); + EXPECT_TRUE(n == 1); + EXPECT_NEAR(collisions[0].m_penetration, 9.0f, 1E-4); b2.Position = ysMath::LoadVector(10.0f, 1.0f, 0.0f, 0.0f); - colliding = detector.RayCircleCollision(collision, &b, &a, &b1, &b2); + n = detector.RayCircleCollision(collisions, &b, &a, &b1, &b2); - EXPECT_TRUE(colliding); - EXPECT_NEAR(collision.m_penetration, 10.0f, 1E-4); + EXPECT_TRUE(n == 1); + EXPECT_NEAR(collisions[0].m_penetration, 10.0f, 1E-4); b2.Position = ysMath::LoadVector(-10.0f, 1.0f, 0.0f, 0.0f); - colliding = detector.RayCircleCollision(collision, &b, &a, &b1, &b2); + n = detector.RayCircleCollision(collisions, &b, &a, &b1, &b2); - EXPECT_FALSE(colliding); + EXPECT_TRUE(n == 0); } TEST(CollisionTests, BoxCircleEdgeCase) { @@ -247,8 +247,33 @@ TEST(CollisionTests, BoxCircleEdgeCase) { dphysics::RigidBody b; dphysics::CollisionDetector detector; - dphysics::Collision collision; - bool colliding = detector.CircleBoxCollision(collision, &b, &a, &b2, &b1); + dphysics::Collision collisions[2]; + int colliding = detector.CircleBoxCollision(collisions, &b, &a, &b2, &b1); - EXPECT_TRUE(colliding); + EXPECT_TRUE(colliding == 1); +} + +TEST(CollisionTests, BoxBoxSideGlitch) { + dphysics::BoxPrimitive b1; + dphysics::BoxPrimitive b2; + + b1.Position = ysMath::LoadVector(0.0f, 0.0f, 0.0f, 0.0f); + b1.HalfHeight = 3.0f; + b1.HalfWidth = 1.0f; + b1.Orientation = ysMath::Constants::QuatIdentity; + + b2.Position = ysMath::LoadVector(1.0f + 0.5f - 0.1f, 1.0f, 0.0f, 0.0f); + b2.HalfHeight = 1.5f; + b2.HalfWidth = 0.5f; + b2.Orientation = ysMath::Constants::QuatIdentity; + + dphysics::RigidBody a; + dphysics::RigidBody b; + + dphysics::CollisionDetector detector; + dphysics::Collision collisions[2]; + int n = detector.BoxBoxCollision(collisions, &b, &a, &b2, &b1); + + EXPECT_TRUE(n == 2); + EXPECT_NEAR(collisions[0].m_penetration, 0.1f, 1E-4); } diff --git a/physics/test/system_tests.cpp b/physics/test/system_tests.cpp index cc3391c7..36e2aacd 100644 --- a/physics/test/system_tests.cpp +++ b/physics/test/system_tests.cpp @@ -418,10 +418,6 @@ TEST(DeltaPhysicsSystemTests, SpinningWheel) { rb.OpenReplayFile("SystemTest_replay.txt"); for (int i = 0; i < 1000; ++i) { - if (i == 420) { - int a = 0; - } - A.ClearAccumulators(); A.AddForceWorldSpace(ysMath::LoadVector(0.0f, -20.0f, 0.0f), A.Transform.GetWorldPosition()); @@ -556,3 +552,56 @@ TEST(DeltaPhysicsSystemTests, RectangleCircleCollision) { rb.CloseReplayFile(); } + +TEST(DeltaPhysicsSystemTests, GroundJitterTest) { + dphysics::RigidBodySystem rb; + + dphysics::RigidBody A; + A.SetHint(dphysics::RigidBody::RigidBodyHint::Dynamic); + A.SetInverseMass(1.0f / 100.0f); + A.SetAlwaysAwake(true); + A.SetInverseInertiaTensor(A.GetRectangleTensor(1.0f * 4.0f, 1.48f * 4.0f)); + A.Transform.SetPosition(ysMath::LoadVector(0.0f, 5.0f, 0.0f)); + A.Transform.SetOrientation(ysMath::Constants::QuatIdentity); + + dphysics::CollisionObject *col; + A.CollisionGeometry.NewBoxObject(&col); + col->SetMode(dphysics::CollisionObject::Mode::Fine); + col->GetAsBox()->Position = ysMath::Constants::Zero; + col->GetAsBox()->HalfHeight = 1.48f; + col->GetAsBox()->HalfWidth = 1.0f; + + dphysics::RigidBody B; + B.SetHint(dphysics::RigidBody::RigidBodyHint::Dynamic); + B.SetInverseMass(0.0f); + B.Transform.SetPosition(ysMath::LoadVector(0.0f, 0.0f, 0.0f)); + B.Transform.SetOrientation(ysMath::Constants::QuatIdentity); + + B.CollisionGeometry.NewBoxObject(&col); + col->SetMode(dphysics::CollisionObject::Mode::Fine); + col->GetAsBox()->Position = ysMath::Constants::Zero; + col->GetAsBox()->HalfHeight = 1.0f; + col->GetAsBox()->HalfWidth = 1.0f; + col->GetAsBox()->Orientation = ysMath::Constants::QuatIdentity; + + B.SetVelocity(ysMath::LoadVector(0.0f, 0.0f, 0.0f)); + + rb.RegisterRigidBody(&B); + rb.RegisterRigidBody(&A); + + rb.OpenReplayFile("SystemTest_replay.txt"); + + for (int i = 0; i < 1000; ++i) { + if (i == 44) { + int a = 0; + } + + A.ClearAccumulators(); + A.AddForceWorldSpace(ysMath::LoadVector(0.0f, -100.0f * 10.0f, 0.0f), A.Transform.GetWorldPosition()); + + rb.Update(1 / 60.0f); + EXPECT_TRUE(rb.CheckState()) << "Check failed on iteration: " << i; + } + + rb.CloseReplayFile(); +}