step/autotests/test_contacts.cc

0001 /*
0002     SPDX-FileCopyrightText: 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
0003
0004     SPDX-License-Identifier: GPL-2.0-or-later
0005 */
0006
0007 #include "test_contacts.h"
0008
0009 #include "stepcore/collisionsolver.h"
0010 #include "stepcore/rigidbody.h"
0011 #include "stepcore/types.h"
0012
0013 #include <cmath>
0014 #include <QTest>
0015
0016 class CollisionSolver: public StepCore::GJKCollisionSolver
0017 {
0018
0019 public:
0020     int testCheckContact(StepCore::Contact* contact) {
0021         return checkContact(contact);
0022     }
0023 };
0024
0025 void TestContacts::testCollisionDetection_data()
0026 {
0027     QTest::addColumn<StepCore::Vector2dList>("vertexes0");
0028     QTest::addColumn<StepCore::Vector2d>("position0");
0029     QTest::addColumn<double>("angle0");
0030
0031     QTest::addColumn<StepCore::Vector2dList>("vertexes1");
0032     QTest::addColumn<StepCore::Vector2d>("position1");
0033     QTest::addColumn<double>("angle1");
0034
0035     QTest::addColumn<int>("state");
0036     QTest::addColumn<StepCore::Vector2d>("distance");
0037
0038     QTest::addColumn<int>("pointsCount");
0039     QTest::addColumn<StepCore::Vector2d>("point0");
0040     QTest::addColumn<StepCore::Vector2d>("point1");
0041
0042     StepCore::Vector2dList vertexes;
0043     vertexes.push_back(StepCore::Vector2d(1,1));
0044     vertexes.push_back(StepCore::Vector2d(1,-1));
0045     vertexes.push_back(StepCore::Vector2d(-1,-1));
0046     vertexes.push_back(StepCore::Vector2d(-1,1));
0047
0048     QTest::newRow("vertex-vertex-1")
0049             << vertexes << StepCore::Vector2d(0,0) << 0.0
0050             << vertexes << StepCore::Vector2d(4,4) << 0.0
0051             << int(StepCore::Contact::Separated) << StepCore::Vector2d(2,2);
0052
0053     QTest::newRow("vertex-edge-1")
0054             << vertexes << StepCore::Vector2d(0,0) << 0.0
0055             << vertexes << StepCore::Vector2d(4,4) << M_PI_4
0056             << int(StepCore::Contact::Separated) << StepCore::Vector2d(3-M_SQRT1_2, 3-M_SQRT1_2);
0057
0058     QTest::newRow("vertex-edge-2")
0059             << vertexes << StepCore::Vector2d(0,0) << 0.0
0060             << vertexes << StepCore::Vector2d(3,0) << M_PI_4
0061             << int(StepCore::Contact::Separated) << StepCore::Vector2d(2-M_SQRT2,0.0);
0062
0063     QTest::newRow("edge-edge-1")
0064             << vertexes << StepCore::Vector2d(0,0) << 0.0
0065             << vertexes << StepCore::Vector2d(4,0) << 0.0
0066             << int(StepCore::Contact::Separated) << StepCore::Vector2d(2,0);
0067
0068     QTest::newRow("edge-edge-2")
0069             << vertexes << StepCore::Vector2d(0,0) << 0.0
0070             << vertexes << StepCore::Vector2d(4,1) << 0.0
0071             << int(StepCore::Contact::Separated) << StepCore::Vector2d(2,0);
0072
0073     QTest::newRow("contact-vertex-vertex-1")
0074             << vertexes << StepCore::Vector2d(0,0) << 0.0
0075             << vertexes << StepCore::Vector2d(2.001,2.001) << 0.0
0076             << int(StepCore::Contact::Contacted) << StepCore::Vector2d(0.001,0.001)
0077             << 1 << StepCore::Vector2d(1,1);
0078
0079     QTest::newRow("contact-vertex-edge-1")
0080             << vertexes << StepCore::Vector2d(0,0) << 0.0
0081             << vertexes << StepCore::Vector2d(1.001+M_SQRT1_2,1.001+M_SQRT1_2) << M_PI_4
0082             << int(StepCore::Contact::Contacted) << StepCore::Vector2d(0.001, 0.001)
0083             << 1 << StepCore::Vector2d(1,1);
0084
0085     QTest::newRow("contact-vertex-edge-2")
0086             << vertexes << StepCore::Vector2d(0,0) << 0.0
0087             << vertexes << StepCore::Vector2d(1.001+M_SQRT2,0.0) << M_PI_4
0088             << int(StepCore::Contact::Contacted) << StepCore::Vector2d(0.001,0.0)
0089             << 1 << StepCore::Vector2d(1,0);
0090
0091     QTest::newRow("contact-edge-edge-1")
0092             << vertexes << StepCore::Vector2d(0,0) << 0.0
0093             << vertexes << StepCore::Vector2d(2.001,0.0) << 0.0
0094             << int(StepCore::Contact::Contacted) << StepCore::Vector2d(0.001,0.0)
0095             << 2 << StepCore::Vector2d(1,1) << StepCore::Vector2d(1,-1);
0096
0097     QTest::newRow("contact-edge-edge-2")
0098             << vertexes << StepCore::Vector2d(0,0) << 0.0
0099             << vertexes << StepCore::Vector2d(2.001,0.0) << 0.00001
0100             << int(StepCore::Contact::Contacted) << StepCore::Vector2d(0.00099,0.0)
0101             << 2 << StepCore::Vector2d(1.000990,0.999990) << StepCore::Vector2d(1,-1);
0102
0103     QTest::newRow("contact-edge-edge-3")
0104             << vertexes << StepCore::Vector2d(0,0) << 0.0
0105             << vertexes << StepCore::Vector2d(2.001,1.0) << 0.0
0106             << int(StepCore::Contact::Contacted) << StepCore::Vector2d(0.001,0.0)
0107             << 2 << StepCore::Vector2d(1,1) << StepCore::Vector2d(1,0);
0108
0109     QTest::newRow("contact-edge-edge-4")
0110             << vertexes << StepCore::Vector2d(0,0) << 0.0
0111             << vertexes << StepCore::Vector2d(2.001,1.0) << 0.00001
0112             << int(StepCore::Contact::Contacted) << (StepCore::Vector2d(1.0,1e-5)*9.999999e-04).eval()
0113             << 2 << StepCore::Vector2d(1,1) << StepCore::Vector2d(1.001010,-0.000010);
0114
0115     QTest::newRow("intersection-vertex-vertex-1")
0116             << vertexes << StepCore::Vector2d(0,0) << 0.0
0117             << vertexes << StepCore::Vector2d(1.9,1.9) << 0.0
0118             << int(StepCore::Contact::Intersected);
0119
0120     QTest::newRow("intersection-vertex-vertex-2")
0121             << vertexes << StepCore::Vector2d(0,0) << 0.0
0122             << vertexes << StepCore::Vector2d(1.7,1.9) << 0.0
0123             << int(StepCore::Contact::Intersected);
0124
0125     QTest::newRow("intersection-vertex-vertex-3")
0126             << vertexes << StepCore::Vector2d(0,0) << 0.0
0127             << vertexes << StepCore::Vector2d(2,1) << M_PI_4
0128             << int(StepCore::Contact::Intersected);
0129
0130
0131     QTest::newRow("intersection-vertex-edge-1")
0132             << vertexes << StepCore::Vector2d(0,0) << 0.0
0133             << vertexes << StepCore::Vector2d(2,0) << M_PI_4
0134             << int(StepCore::Contact::Intersected);
0135 }
0136
0137 void TestContacts::testCollisionDetection()
0138 {
0139     QFETCH(StepCore::Vector2dList, vertexes0);
0140     QFETCH(StepCore::Vector2d, position0);
0141     QFETCH(double, angle0);
0142
0143     QFETCH(StepCore::Vector2dList, vertexes1);
0144     QFETCH(StepCore::Vector2d, position1);
0145     QFETCH(double, angle1);
0146
0147     QFETCH(int, state);
0148
0149     /*
0150     qDebug("Expected state = %d", state);
0151     qDebug("Expected distance = %lf", distance.norm());
0152     qDebug("Expected normal = (%lf, %lf)", normal[0], normal[1]);
0153     */
0154
0155     StepCore::Polygon* polygon0 = new StepCore::Polygon();
0156     polygon0->setVertexes(vertexes0);
0157     polygon0->setPosition(position0);
0158     polygon0->setAngle(angle0);
0159
0160     StepCore::Polygon* polygon1 = new StepCore::Polygon();
0161     polygon1->setVertexes(vertexes1);
0162     polygon1->setPosition(position1);
0163     polygon1->setAngle(angle1);
0164
0165     CollisionSolver *collisionSolver = new CollisionSolver();
0166     collisionSolver->setToleranceAbs(0.01);
0167
0168     StepCore::Contact contact;
0169     std::memset(&contact, 0, sizeof(contact));
0170     contact.body0 = polygon0;
0171     contact.body1 = polygon1;
0172     contact.type = StepCore::Contact::PolygonPolygonType;
0173     contact.state = StepCore::Contact::Unknown;
0174
0175 #ifdef __GNUC__
0176 #warning Collision solver tests are disabled!
0177 #endif
0178
0179     /* TODO
0180     collisionSolver->testCheckContact(&contact);
0181
0182     QCOMPARE(int(contact.state), state);
0183
0184     if(state == int(StepCore::Contact::Separated) ||
0185        state == int(StepCore::Contact::Contacted)) {
0186         QFETCH(StepCore::Vector2d, distance);
0187         StepCore::Vector2d normal = distance / distance.norm();
0188         //qDebug("(%e %e)*%e", contact.normal[0], contact.normal[1], contact.distance);
0189         QVERIFY( fabs(contact.distance - distance.norm()) < 1e-10 );
0190         QVERIFY( fabs(contact.normal[0] - normal[0]) < 1e-10 &&
0191                  fabs(contact.normal[1] - normal[1]) < 1e-10 );
0192     }
0193
0194     if(state == int(StepCore::Contact::Contacted)) {
0195         QFETCH(StepCore::Vector2d, distance);
0196         StepCore::Vector2d normal = distance / distance.norm();
0197
0198         QFETCH(int, pointsCount);
0199         QCOMPARE( contact.pointsCount, pointsCount );
0200
0201         StepCore::Vector2d line(-normal[1], normal[0]);
0202
0203         if(pointsCount == 1) {
0204             QFETCH(StepCore::Vector2d, point0);
0205             //qDebug("l: %e",  (contact.points[0] - point0).dot(line));
0206             //qDebug("n: %e",  (contact.points[0] - point0).dot(normal));
0207             //qDebug("%e %e", contact.points[0][0], contact.points[0][1]);
0208
0209             QVERIFY( fabs((contact.points[0] - point0).dot(line)) < 1e-10 );
0210             QVERIFY( fabs((contact.points[0] - point0).dot(normal)) < 0.02 );
0211
0212         } else if(pointsCount == 2) {
0213             QFETCH(StepCore::Vector2d, point0);
0214             QFETCH(StepCore::Vector2d, point1);
0215
0216             QVERIFY( (fabs((contact.points[0] - point0).dot(line)) < 1e-10 &&
0217                       fabs((contact.points[1] - point1).dot(line)) < 1e-10) ||
0218                      (fabs((contact.points[0] - point1).dot(line)) < 1e-10 &&
0219                       fabs((contact.points[1] - point0).dot(line)) < 1e-10) );
0220
0221             QVERIFY( (fabs((contact.points[0] - point0).dot(normal)) < 0.02 &&
0222                       fabs((contact.points[1] - point1).dot(normal)) < 0.02) ||
0223                      (fabs((contact.points[0] - point1).dot(normal)) < 0.02 &&
0224                       fabs((contact.points[1] - point0).dot(normal)) < 0.02) );
0225         }
0226     }
0227
0228     */
0229     delete collisionSolver;
0230     delete polygon1;
0231     delete polygon0;
0232 }
0233
0234 QTEST_MAIN(TestContacts)