172 lines
3.6 KiB
C++
172 lines
3.6 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include <functional>
|
|
#include <chrono>
|
|
|
|
#include "Constraint.h"
|
|
#include "FullColumn.h"
|
|
#include "enum.h"
|
|
|
|
using namespace MbD;
|
|
|
|
Constraint::Constraint() : Item()
|
|
{
|
|
aConstant = 0.0;
|
|
}
|
|
|
|
Constraint::Constraint(const char* str) : Item(str)
|
|
{
|
|
}
|
|
|
|
void Constraint::initialize()
|
|
{
|
|
auto now = std::chrono::high_resolution_clock::now();
|
|
auto nanoseconds = now.time_since_epoch().count();
|
|
name = std::to_string(nanoseconds);
|
|
}
|
|
|
|
void Constraint::postInput()
|
|
{
|
|
lam = 0.0;
|
|
Item::postInput();
|
|
}
|
|
|
|
void Constraint::prePosIC()
|
|
{
|
|
lam = 0.0;
|
|
iG = -1;
|
|
Item::prePosIC();
|
|
}
|
|
|
|
void Constraint::prePosKine()
|
|
{
|
|
//"Preserve lam calculated from AccIC for possible use by DynIntegrator if system is not kinematic."
|
|
auto lamOld = lam;
|
|
this->prePosIC();
|
|
lam = lamOld;
|
|
}
|
|
|
|
void Constraint::fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
|
|
{
|
|
if (this->type() == essential) {
|
|
essenConstraints->push_back(sptr);
|
|
}
|
|
}
|
|
void Constraint::fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
|
|
{
|
|
if (this->type() == displacement) {
|
|
dispConstraints->push_back(sptr);
|
|
}
|
|
}
|
|
void Constraint::fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
|
|
{
|
|
if (this->type() == perpendicular) {
|
|
perpenConstraints->push_back(sptr);
|
|
}
|
|
}
|
|
|
|
void Constraint::fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
|
|
{
|
|
if (this->type() == redundant) {
|
|
redunConstraints->push_back(sptr);
|
|
}
|
|
}
|
|
|
|
void Constraint::fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
|
|
{
|
|
allConstraints->push_back(sptr);
|
|
}
|
|
|
|
ConstraintType Constraint::type()
|
|
{
|
|
return essential;
|
|
}
|
|
|
|
void Constraint::fillqsulam(FColDsptr col)
|
|
{
|
|
col->atiput(iG, lam);
|
|
}
|
|
|
|
void Constraint::setqsulam(FColDsptr col)
|
|
{
|
|
lam = col->at(iG);
|
|
}
|
|
|
|
void Constraint::setqsudotlam(FColDsptr col)
|
|
{
|
|
lam = col->at(iG);
|
|
}
|
|
|
|
void Constraint::fillPosICError(FColDsptr col)
|
|
{
|
|
col->atiplusNumber(iG, aG);
|
|
}
|
|
|
|
void Constraint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos)
|
|
{
|
|
//My owner should handle this.
|
|
assert(false);
|
|
}
|
|
|
|
void MbD::Constraint::setConstant(double value)
|
|
{
|
|
aConstant = value;
|
|
}
|
|
|
|
void Constraint::reactivateRedundantConstraints()
|
|
{
|
|
//My owner should handle this.
|
|
assert(false);
|
|
}
|
|
|
|
bool Constraint::isRedundant()
|
|
{
|
|
return false;
|
|
}
|
|
|
|
void Constraint::preDyn()
|
|
{
|
|
mu = 0.0;
|
|
}
|
|
|
|
void Constraint::fillPosKineError(FColDsptr col)
|
|
{
|
|
col->atiplusNumber(iG, aG);
|
|
}
|
|
|
|
void Constraint::fillqsuddotlam(FColDsptr col)
|
|
{
|
|
col->atiput(iG, lam);
|
|
}
|
|
|
|
void Constraint::preAccIC()
|
|
{
|
|
lam = 0.0;
|
|
Item::preAccIC();
|
|
}
|
|
|
|
void Constraint::fillAccICIterJacob(SpMatDsptr mat)
|
|
{
|
|
//"Same as velIC."
|
|
this->fillVelICJacob(mat);
|
|
}
|
|
|
|
void Constraint::setqsuddotlam(FColDsptr col)
|
|
{
|
|
lam = col->at(iG);
|
|
}
|
|
|
|
void Constraint::addToJointForceI(FColDsptr col)
|
|
{
|
|
}
|
|
|
|
void Constraint::addToJointTorqueI(FColDsptr col)
|
|
{
|
|
}
|