CommonTests.h

Go to the documentation of this file.

// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#ifndef SURGSIM_PHYSICS_UNITTESTS_COMMONTESTS_H
#define SURGSIM_PHYSICS_UNITTESTS_COMMONTESTS_H

#include <gtest/gtest.h>

#include <string>
#include <memory>

#include "SurgSim/Math/Shapes.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintImplementation.h"
#include "SurgSim/Physics/ContactConstraintData.h"
#include "SurgSim/Physics/FixedConstraintFrictionlessContact.h"
#include "SurgSim/Physics/FixedRepresentation.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
#include "SurgSim/Physics/MlcpPhysicsSolution.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
#include "SurgSim/Physics/RigidConstraintFrictionlessContact.h"
#include "SurgSim/Physics/RigidRepresentation.h"

using SurgSim::Math::BoxShape;
using SurgSim::Math::Shape;
using SurgSim::Math::SphereShape;

namespace
{
    const double epsilon = 1e-10;
};

namespace SurgSim
{
namespace Physics
{

class CommonTests : public ::testing::Test
{
public:
    void SetUp()
    {
        // Set the time step
        dt = 1e-3;

        // Create a fixed world to define constraint against it
        m_fixedWorldRepresentation = std::make_shared<FixedRepresentation>("FixedPlane");
        m_fixedWorldRepresentation->setIsGravityEnabled(false);
        {
            // Simply do 1 time step to make sure things are initialized (compliance matrix...)
            m_fixedWorldRepresentation->beforeUpdate(dt);
            m_fixedWorldRepresentation->update(dt);
            m_fixedWorldRepresentation->afterUpdate(dt);
        }

        // Create the physics manager state
        m_physicsManagerState = std::make_shared<PhysicsManagerState>();

        // Create a Rigid Sphere
        std::shared_ptr<RigidRepresentation> rigidSphereRepresentation;
        rigidSphereRepresentation = std::make_shared<RigidRepresentation>("RigidSphere");
        {
            double radius = 1e-2;
            std::shared_ptr<Shape> shape = std::make_shared<SphereShape>(radius);
            rigidSphereRepresentation->setShape(shape);
            rigidSphereRepresentation->setDensity(1000);
            rigidSphereRepresentation->setIsGravityEnabled(false);
            {
                // Simply do 1 time step to make sure things are initialized (compliance matrix...)
                rigidSphereRepresentation->beforeUpdate(dt);
                rigidSphereRepresentation->update(dt);
                rigidSphereRepresentation->afterUpdate(dt);
            }
            m_allRepresentations.push_back(rigidSphereRepresentation);
        }

        // Create a Rigid Box
        std::shared_ptr<RigidRepresentation> rigidBoxRepresentation = std::make_shared<RigidRepresentation>("RigidBox");
        {
            double size[3]={0.01, 0.02, 0.03};
            rigidBoxRepresentation->setDensity(1000);
            std::shared_ptr<Shape> shape = std::make_shared<BoxShape>(size[0], size[1], size[2]);
            rigidBoxRepresentation->setShape(shape);
            rigidBoxRepresentation->setIsGravityEnabled(false);
            {
                // Simply do 1 time step to make sure things are initialized (compliance matrix...)
                rigidBoxRepresentation->beforeUpdate(dt);
                rigidBoxRepresentation->update(dt);
                rigidBoxRepresentation->afterUpdate(dt);
            }
            m_allRepresentations.push_back(rigidBoxRepresentation);
        }
    }

    void resetMlcpProblem(int nbDof, int nbConstraint)
    {
        if (m_physicsManagerState)
        {
            m_physicsManagerState->getMlcpProblem().A.setZero(nbConstraint, nbConstraint);
            m_physicsManagerState->getMlcpProblem().b.setZero(nbConstraint);
            m_physicsManagerState->getMlcpProblem().CHt.setZero(nbDof, nbConstraint);
            m_physicsManagerState->getMlcpProblem().H.resize(nbConstraint, nbDof);
            m_physicsManagerState->getMlcpProblem().mu.setZero(nbConstraint);
            m_physicsManagerState->getMlcpProblem().constraintTypes.clear();

            m_physicsManagerState->getMlcpSolution().x.setZero(nbConstraint);
            m_physicsManagerState->getMlcpSolution().dofCorrection.setZero(nbDof);
        }
    }

protected:
    double dt = std::numeric_limits<double>::signaling_NaN();

    std::shared_ptr<Representation> m_fixedWorldRepresentation;

    std::vector<std::shared_ptr<Representation>> m_allRepresentations;

    std::vector<std::shared_ptr<Representation>> m_usedRepresentations;

    std::vector<std::shared_ptr<Constraint>> m_usedConstraints;

    std::shared_ptr<PhysicsManagerState> m_physicsManagerState;
};

}; // namespace Physics
}; // namespace SurgSim

#endif // SURGSIM_PHYSICS_UNITTESTS_COMMONTESTS_H