Inheritance diagram for ModuleNonsmoothNode:

Collaboration diagram for ModuleNonsmoothNode:

Public Member Functions
	ModuleNonsmoothNode (unsigned uLabel, const DofOwner pDO, DataManager pDM, MBDynParser &HP)

virtual	~ModuleNonsmoothNode (void)

virtual unsigned int	iGetNumDof (void) const

virtual DofOrder::Order	GetDofType (unsigned int) const

virtual void	Output (OutputHandler &OH) const

virtual void	WorkSpaceDim (integer piNumRows, integer piNumCols) const

virtual void	AfterPredict (VectorHandler &X, VectorHandler &XP)

virtual void	AfterConvergence (const VectorHandler &X, const VectorHandler &XP)

VariableSubMatrixHandler &	AssJac (VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

SubVectorHandler &	AssRes (SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

unsigned int	iGetNumPrivData (void) const

int	iGetNumConnectedNodes (void) const

void	GetConnectedNodes (std::vector< const Node * > &connectedNodes) const

void	SetValue (DataManager pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints ph)

std::ostream &	Restart (std::ostream &out) const

virtual unsigned int	iGetInitialNumDof (void) const

virtual void	InitialWorkSpaceDim (integer piNumRows, integer piNumCols) const

VariableSubMatrixHandler &	InitialAssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)

SubVectorHandler &	InitialAssRes (SubVectorHandler &WorkVec, const VectorHandler &XCurr)

Public Member Functions inherited from Elem
	Elem (unsigned int uL, flag fOut)

virtual	~Elem (void)

virtual std::ostream &	DescribeDof (std::ostream &out, const char *prefix="", bool bInitial=false) const

virtual void	DescribeDof (std::vector< std::string > &desc, bool bInitial=false, int i=-1) const

virtual std::ostream &	DescribeEq (std::ostream &out, const char *prefix="", bool bInitial=false) const

virtual void	DescribeEq (std::vector< std::string > &desc, bool bInitial=false, int i=-1) const

virtual void	AssMats (VariableSubMatrixHandler &WorkMatA, VariableSubMatrixHandler &WorkMatB, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

virtual bool	bInverseDynamics (void) const

void	SetInverseDynamicsFlags (unsigned uIDF)

unsigned	GetInverseDynamicsFlags (void) const

bool	bIsErgonomy (void) const

bool	bIsRightHandSide (void) const

virtual VariableSubMatrixHandler &	AssJac (VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)

virtual SubVectorHandler &	AssRes (SubVectorHandler &WorkVec, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr, const VectorHandler &XPrimePrimeCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)

virtual int	GetNumConnectedNodes (void) const

Public Member Functions inherited from WithLabel
	WithLabel (unsigned int uL=0, const std::string &sN="")

virtual	~WithLabel (void)

void	PutLabel (unsigned int uL)

void	PutName (const std::string &sN)

unsigned int	GetLabel (void) const

const std::string &	GetName (void) const

Public Member Functions inherited from SimulationEntity
	SimulationEntity (void)

virtual	~SimulationEntity (void)

virtual bool	bIsValidIndex (unsigned int i) const

virtual DofOrder::Order	GetEqType (unsigned int i) const

virtual Hint *	ParseHint (DataManager pDM, const char s) const

virtual void	BeforePredict (VectorHandler &, VectorHandler &, VectorHandler &, VectorHandler &) const

virtual void	Update (const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

virtual void	DerivativesUpdate (const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)

virtual void	Update (const VectorHandler &XCurr, InverseDynamics::Order iOrder)

virtual void	AfterConvergence (const VectorHandler &X, const VectorHandler &XP, const VectorHandler &XPP)

virtual unsigned int	iGetPrivDataIdx (const char *s) const

virtual doublereal	dGetPrivData (unsigned int i) const

virtual std::ostream &	OutputAppend (std::ostream &out) const

virtual void	ReadInitialState (MBDynParser &HP)

Public Member Functions inherited from ToBeOutput
	ToBeOutput (flag fOut=fDefaultOut)

virtual	~ToBeOutput (void)

virtual void	OutputPrepare (OutputHandler &OH)

virtual void	Output (OutputHandler &OH, const VectorHandler &X, const VectorHandler &XP) const

virtual flag	fToBeOutput (void) const

virtual bool	bToBeOutput (void) const

virtual void	SetOutputFlag (flag f=flag(1))

Public Member Functions inherited from UserDefinedElem
	UserDefinedElem (unsigned uLabel, const DofOwner *pDO)

virtual	~UserDefinedElem (void)

bool	NeedsAirProperties (void) const

void	NeedsAirProperties (bool yesno)

virtual Elem::Type	GetElemType (void) const

virtual AerodynamicElem::Type	GetAerodynamicElemType (void) const

Public Member Functions inherited from InitialAssemblyElem
	InitialAssemblyElem (unsigned int uL, flag fOut)

virtual	~InitialAssemblyElem (void)

Public Member Functions inherited from SubjectToInitialAssembly
	SubjectToInitialAssembly (void)

virtual	~SubjectToInitialAssembly (void)

Public Member Functions inherited from AerodynamicElem
	AerodynamicElem (unsigned int uL, const DofOwner *pDO, flag fOut)

virtual	~AerodynamicElem (void)

virtual const InducedVelocity *	pGetInducedVelocity (void) const

Public Member Functions inherited from ElemWithDofs
	ElemWithDofs (unsigned int uL, const DofOwner *pDO, flag fOut)

virtual	~ElemWithDofs (void)

Public Member Functions inherited from DofOwnerOwner
	DofOwnerOwner (const DofOwner *pDO)

virtual	~DofOwnerOwner ()

virtual const DofOwner *	pGetDofOwner (void) const

virtual integer	iGetFirstIndex (void) const

virtual void	SetInitialValue (VectorHandler &X)

Public Member Functions inherited from AirPropOwner
	AirPropOwner (void)

virtual	~AirPropOwner (void)

virtual void	PutAirProperties (const AirProperties *pAP)

virtual flag	fGetAirVelocity (Vec3 &Velocity, const Vec3 &X) const

virtual doublereal	dGetAirDensity (const Vec3 &X) const

virtual doublereal	dGetAirPressure (const Vec3 &X) const

virtual doublereal	dGetAirTemperature (const Vec3 &X) const

virtual doublereal	dGetSoundSpeed (const Vec3 &X) const

virtual bool	GetAirProps (const Vec3 &X, doublereal &rho, doublereal &c, doublereal &p, doublereal &T) const

Public Member Functions inherited from ElemGravityOwner
	ElemGravityOwner (unsigned int uL, flag fOut)

virtual	~ElemGravityOwner (void)

virtual doublereal	dGetM (void) const

Vec3	GetS (void) const

Mat3x3	GetJ (void) const

Vec3	GetB (void) const

Vec3	GetG (void) const

Public Member Functions inherited from GravityOwner
	GravityOwner (void)

virtual	~GravityOwner (void)

void	PutGravity (const Gravity *pG)

virtual bool	bGetGravity (const Vec3 &X, Vec3 &Acc) const

Private Member Functions
void	mbs_get_force (NS_subsys &)

void	mbs_get_force_frictional (NS_subsys &)

Private Attributes
const DataManager *	m_pDM

const StructDispNode *	m_pNode

NS_subsys	NS_data

bool	bStepToggle

Vec3	Mu

bool	bFrictional

int	iIter

Additional Inherited Members
Public Types inherited from Elem
enum	Type { UNKNOWN = -1, AIRPROPERTIES = 0, INDUCEDVELOCITY, AUTOMATICSTRUCTURAL, GRAVITY, BODY, JOINT, JOINT_REGULARIZATION, BEAM, PLATE, FORCE, INERTIA, ELECTRICBULK, ELECTRIC, THERMAL, HYDRAULIC, BULK, LOADABLE, DRIVEN, EXTERNAL, AEROMODAL, AERODYNAMIC, GENEL, SOCKETSTREAM_OUTPUT, RTAI_OUTPUT = SOCKETSTREAM_OUTPUT, LASTELEMTYPE }

Public Types inherited from SimulationEntity
typedef std::vector< Hint * >	Hints

Public Types inherited from ToBeOutput
enum	{ OUTPUT = 0x1U, OUTPUT_MASK = 0xFU, OUTPUT_PRIVATE = 0x10U, OUTPUT_PRIVATE_MASK = ~OUTPUT_MASK }

Public Types inherited from AerodynamicElem
enum	Type { UNKNOWN = -1, INDUCEDVELOCITY = 0, AEROMODAL, AERODYNAMICBODY, AERODYNAMICBEAM, AERODYNAMICEXTERNAL, AERODYNAMICEXTERNALMODAL, AERODYNAMICLOADABLE, AIRCRAFTINSTRUMENTS, GENERICFORCE, LASTAEROTYPE }

Protected Member Functions inherited from ElemGravityOwner
virtual Vec3	GetS_int (void) const

virtual Mat3x3	GetJ_int (void) const

virtual Vec3	GetB_int (void) const

virtual Vec3	GetG_int (void) const

Protected Attributes inherited from WithLabel
unsigned int	uLabel

std::string	sName

Protected Attributes inherited from ToBeOutput
flag	fOutput

Protected Attributes inherited from UserDefinedElem
bool	needsAirProperties

Protected Attributes inherited from AirPropOwner
const AirProperties *	pAirProperties

Protected Attributes inherited from GravityOwner
Gravity *	pGravity

Detailed Description

Definition at line 116 of file module-nonsmooth-node.cc.

Constructor & Destructor Documentation

ModuleNonsmoothNode::ModuleNonsmoothNode	(	unsigned	uLabel,
		const DofOwner *	pDO,
		DataManager *	pDM,
		MBDynParser &	HP
	)

Definition at line 219 of file module-nonsmooth-node.cc.

References bFrictional, bIRa(), NS_subsys::bVerbose, c, NS_subsys::constr, NS_subsys::constraint_type, CPG, DriveHandler::dGetTimeStep(), dIPa, dIRa, DataManager::fReadOutput(), NS_subsys::gamma_pred, HighParser::GetInt(), WithLabel::GetLabel(), IncludeParser::GetLineData(), MBDynParser::GetPosRel(), HighParser::GetReal(), MBDynParser::GetRotRel(), StructDispNode::GetVCurr(), StructDispNode::GetXCurr(), HighParser::GetYesNoOrBool(), NS_subsys::hstep, solver_parameters::info, IPa, IRa, IRat, HighParser::IsArg(), HighParser::IsKeyWord(), NS_subsys::iterations, NS_subsys::Lambda_k, NS_subsys::Lambda_kp1, LATIN, LATIN_W, LEXICO_LEMKE, Elem::LOADABLE, m_pDM, m_pNode, NS_subsys::mass_ns, MBDYN_EXCEPT_ARGS, NEWTON_FB, NEWTON_MIN, NS_subsys::niter, NS_subsys::niterLCPmax, NS_subsys::Np, NS_data, NSQP, DataManager::pGetDrvHdl(), PGS, NS_subsys::Piter, POSITION_AND_VELOCITY, POSITION_ONLY, solver_parameters::processed_iterations, PSOR, QP, NS_subsys::radius_ns, DataManager::ReadNode(), FullMatrixHandler::Resize(), solver_parameters::resulting_error, RPGS, ToBeOutput::SetOutputFlag(), NS_subsys::solparam, solver_parameters::solver, solver_parameters::solveritermax, solver_parameters::solvertol, Node::STRUCTURAL, NS_subsys::theta_ts, VELOCITY_ONLY, NS_subsys::Vns_k, NS_subsys::Vns_kp1, NS_subsys::Xns_k, NS_subsys::Xns_kp1, and Zero3.

 : Elem(uLabel, flag(0)),
 UserDefinedElem(uLabel, pDO),
 m_pDM(pDM),
 m_pNode(0),
 bStepToggle(false),
 Mu(::Zero3),
 bFrictional(false),
 iIter(0)
 {
         NS_data.bVerbose = false;
         NS_data.niter = 0;
         NS_data.Piter = ::Zero3;
 
         // help
         if (HP.IsKeyWord("help")) {
                 silent_cout(
 "\n"
 "Module:        nonsmooth-node\n"
 "Author:        Matteo Fancello <matteo.fancello@gmail.com>\n"
 "               Pierangelo Masarati <masarati@aero.polimi.it>\n"
 "Organization:  Dipartimento di Ingegneria Aerospaziale\n"
 "               Politecnico di Milano\n"
 "               http://www.aero.polimi.it/\n"
 "\n"
 "       All rights reserved\n"
 "\n"
 "       Defines a unilateral constraint in form of a contact\n"
 "       between a node and one or more planes, optionally with friction.\n"
 "\n"
 "       # in the \"control data\" block: increase by 1 the count of \"loadable elements\"\n"
 "\n"
 "       # in the \"elements\" block: add a user-defined element according to the syntax\n"
 "       user defined :\n"
 "               <element_label> , nonsmooth node ,\n"
 "               [ frictional , { yes | no | (bool) <frictional> } , ]\n"
 "               (StructDispNode) <NonsmoothNODELABEL> ,\n"
 "               mass , (real) <mass> , radius , (real) <radius> ,\n"
 "               planes , (int) <number_of_planes> ,\n"
 "                       <PlaneDefiningNODELABEL> ,\n"
 "                       position ,\n"
 "                       (Vec3) <relative_plane_position> ,\n"
 "                       rotation orientation ,\n"
 "                       (OrientationMatrix) <rel_rot_orientation_1> ,\n"
 "                       restitution , (real) <rest_coeff>\n"
 "                       [ , frictional coefficient , <mu> ]\n"
 "                       [ , <other planes> ]\n"
 "               [ , constraint type , { position | velocity | both } ] # default: both\n"
 "               [ , theta , <theta> ] [ , gamma , <gamma> ]\n"
 "               [ , LCP solver , <solver> ]\n"
 "               [ , tolerance , <tolerance> ][ , max iterations , <num_iter> ]\n"
 "                       # these options depend on LCP solver support, see\n"
 "                       # http://siconos.gforge.inria.fr/Numerics/LCPSolvers.html\n"
 "               [ , limit iterations , <niterations> ]\n"
 "               [ , limit LCP iterations , <niterations> ]\n"
 "               [ , verbose , { yes | no | (bool) <verbose> } ] ;\n"
 "\n"
 "        <solver> ::= lexico lemke # the default\n"
 "                   | rpgs\n"
 "                   | qp\n"
 "                   | cpg\n"
 "                   | pgs\n"
 "                   | psor\n"
 "                   | nsqp\n"
 "                   | latin\n"
 "                   | latin_w\n"
 "                   | newton_min\n"
 "                   | newton_FB\n"
 "\n"
 "       OUTPUT:\n"
 "               1:     element label\n"
 "               2-4:   impulse on  NonSmooth node in global ref. frame\n"
 "               5-7:   position of NonSmooth node in global ref. frame\n"
 "               8-10:  velocity of Nonsmooth node in global ref. frame\n"
 "               11-13: Lambda: constr. reaction between mbs node and NS node\n"
 "               14:    p: norm of the impulse reaction in global ref. frame\n"
 "               15:    Ia: number of active constraints in this step\n"
 "\n"
 "       if verbose, also:\n"
 "               16-18: Mu: constraint relaxation factor\n"
 "               19:    LCPsolver status: 0 -> convergence, 1 -> iter=maxiter, >1 -> fail\n"
 "                      (only for some solvers)\n"
 "               20:    LCPsolver resulting_error\n"
 "                      (only for some solvers)\n"
                         << std::endl);
 
 
                 if (!HP.IsArg()) {
                         /*
                          * Exit quietly if nothing else is provided
                          */
                         throw NoErr(MBDYN_EXCEPT_ARGS);
                 }
         }
 
         m_pNode = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
 
         if (!HP.IsKeyWord("mass")) {
                 silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"mass\" expected at line "
                         << HP.GetLineData() << std::endl);
                 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
         }
 
         NS_data.mass_ns = HP.GetReal();
         if (NS_data.mass_ns <= std::numeric_limits<doublereal>::epsilon()) {
                 silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"mass\" too small at line "
                         << HP.GetLineData() << std::endl);
                 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
         }
 
         if (!HP.IsKeyWord("radius")) {
                 silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"radius\" expected at line "
                         << HP.GetLineData() << std::endl);
                 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
         }
 
         NS_data.radius_ns = HP.GetReal();
 
         if (!HP.IsKeyWord("planes")) {
                 silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"planes\" expected at line "
                         << HP.GetLineData() << std::endl);
                 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
         }
 
         NS_data.Np  = HP.GetInt();
         NS_data.constr.resize(NS_data.Np);
 
         for (int i = 0; i < NS_data.Np; i++) {
                 NS_data.constr[i].AttNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
 
                 ReferenceFrame RFnode(NS_data.constr[i].AttNode);
 
                 if (!HP.IsKeyWord("position")) {
                         silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"position\" expected at line "
                                 << HP.GetLineData() << std::endl);
                         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                 }
 
                 NS_data.constr[i].Point = HP.GetPosRel(RFnode);
 
                 if (!HP.IsKeyWord("rotation" "orientation")) {
                         silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"rotation orientation \" expected at line "
                                 << HP.GetLineData() << std::endl);
                         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                 }
 
                 NS_data.constr[i].RotRel = HP.GetRotRel(RFnode);
 
                 if (!HP.IsKeyWord("restitution")) {
                         silent_cerr("ModuleNonsmoothNode(" << uLabel << "): \"restitution \" expected at line "
                                 << HP.GetLineData() << std::endl);
                         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                 }
 
                 NS_data.constr[i].e_rest  = HP.GetReal();
 
                 if (HP.IsKeyWord("friction" "coefficient")) {
                         bFrictional = true;
                         NS_data.constr[i].mu = HP.GetReal();
 
                 } else if (bFrictional) {
                         silent_cerr("ModuleNonsmoothNode(" << uLabel << "): no friction coefficient for plane #" << i << " of " << NS_data.Np << " at line "
                                 << HP.GetLineData() << std::endl);
                 }
         }
 
         NS_data.constraint_type = POSITION_AND_VELOCITY;
 
         if (HP.IsKeyWord("constraint" "type")) {
                 if (HP.IsKeyWord("position")) {
                         NS_data.constraint_type = POSITION_ONLY;
                 } else if (HP.IsKeyWord("velocity")) {
                         NS_data.constraint_type = VELOCITY_ONLY;
                 } else if (HP.IsKeyWord("both")) {
                         NS_data.constraint_type = POSITION_AND_VELOCITY;
                 } else {
                         silent_cerr("ModuleNonsmoothNode: invalid constraint_type"
                                 << " at line " << HP.GetLineData() << std::endl);
                         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                 }
         }
 
         if (HP.IsKeyWord("theta")) {
                 NS_data.theta_ts = HP.GetReal();
 
         } else {
                 NS_data.theta_ts = 0.5;
         }
 
         if (HP.IsKeyWord("gamma")) {
                 NS_data.gamma_pred = HP.GetReal();
 
         } else {
                 NS_data.gamma_pred = 1.;
         }
 
         // default: LEXICO_LEMKE
         NS_data.solparam.solver = LEXICO_LEMKE;
         if (HP.IsKeyWord("LCP" "solver")) {
                 if (HP.IsKeyWord("lexico" "lemke")) {
                         NS_data.solparam.solver = LEXICO_LEMKE;
                 } else if (HP.IsKeyWord("rpgs")) {
                         NS_data.solparam.solver = RPGS;
                 } else if (HP.IsKeyWord("qp")) {
                         NS_data.solparam.solver = QP;
                 } else if (HP.IsKeyWord("cpg")) {
                         NS_data.solparam.solver = CPG;
                 } else if (HP.IsKeyWord("pgs")) {
                         NS_data.solparam.solver = PGS;
                 } else if (HP.IsKeyWord("psor")) {
                         NS_data.solparam.solver = PSOR;
                 } else if (HP.IsKeyWord("nsqp")) {
                         NS_data.solparam.solver = NSQP;
                 } else if (HP.IsKeyWord("latin")) {
                         NS_data.solparam.solver = LATIN;
                 } else if (HP.IsKeyWord("latin_w")) {
                         NS_data.solparam.solver = LATIN_W;
                 } else if (HP.IsKeyWord("newton_min")) {
                         NS_data.solparam.solver = NEWTON_MIN;
                 } else if (HP.IsKeyWord("newton_FB")) {
                         NS_data.solparam.solver = NEWTON_FB;
                 } else {
                         silent_cerr("ModuleNonsmoothNode(" << GetLabel() << "): invalid lcp solver type"
                                 << " at line " << HP.GetLineData() << std::endl);
                         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                 }
         }
 
         NS_data.solparam.solvertol = 1.e-14;
         if (HP.IsKeyWord("tolerance")) {
                 NS_data.solparam.solvertol = HP.GetReal();
         }
 
         NS_data.solparam.solveritermax = 100;
         if (HP.IsKeyWord("max" "iterations")) {
                 NS_data.solparam.solveritermax = HP.GetInt();
         }
 
         NS_data.iterations = 0;
         if (HP.IsKeyWord("limit" "iterations")) {
                 NS_data.iterations = HP.GetInt();
         }
 
         NS_data.niterLCPmax = 0;
         if (HP.IsKeyWord("limit" "LCP" "iterations")) {
                 NS_data.niterLCPmax = HP.GetInt();
         }
 
         if (HP.IsKeyWord("verbose")) {
                 NS_data.bVerbose = HP.GetYesNoOrBool(true);
         }
 
         SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
 
         //----------------------------------------------
         //              CHECK GAP
         //----------------------------------------------
 
         for (int i = 0; i < NS_data.Np; i++) {
                 NS_data.constr[i].H_kp1 = NS_data.constr[i].AttNode->GetRCurr() * NS_data.constr[i].RotRel;
                 Vec3 Plane_point = NS_data.constr[i].AttNode->GetXCurr() + NS_data.constr[i].AttNode->GetRCurr() * NS_data.constr[i].Point;
                 double Gap = (m_pNode->GetXCurr() - Plane_point) * NS_data.constr[i].H_kp1.GetVec(3) - NS_data.radius_ns;
 
                 if (NS_data.bVerbose) {
                         silent_cout("ModuleNonsmoothNode(" << GetLabel() << ")" << std::endl
                                 << "  initial nonsmooth node position " << m_pNode->GetXCurr() << std::endl
                                 << "  Plane_point " << Plane_point << std::endl
                                 << "  normal " << NS_data.constr[i].H_kp1.GetVec(3) << std::endl
                                 << "  Gap " << Gap << std::endl
                                 << "  RFnode " << NS_data.constr[i].AttNode->GetRCurr() << std::endl);
                 }
 
                 if (Gap <= 0.) {
                         silent_cerr("ModuleNonsmoothNode(" << GetLabel() << "): "
                                 "unilateral constraint " << i << " violated before simulation; Gap=" << Gap << std::endl);
                         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                 }
         }
 
         //Inizializzazione del sottosistema NonSmooth (struct) nel costruttore:
         // integration step
         NS_data.hstep = m_pDM->pGetDrvHdl()->dGetTimeStep();
 
         // Inizializzazione dello stato
         NS_data.Lambda_kp1 = Zero3;
         NS_data.Lambda_k = Zero3;
         NS_data.Xns_kp1 = m_pNode->GetXCurr();  //NS state at step k +1
         NS_data.Vns_kp1 = m_pNode->GetVCurr();  //NS velocity at step k + 1
         NS_data.Xns_k = NS_data.Xns_kp1;                //NS state at step k
         NS_data.Vns_k = NS_data.Vns_kp1;                //NS velocity at step k
 
         NS_data.solparam.info = 0;
         NS_data.solparam.resulting_error = 0;
         NS_data.solparam.processed_iterations = -1;
 
         if (NS_data.bVerbose) {
                 silent_cout("ModuleNonsmoothNode(" << GetLabel() << "): initial nonsmooth node position " << NS_data.Xns_kp1 << std::endl);
         }
 
         // Set-up approximated friction cone
         // TODO: allow per-element discretization
         if (bFrictional && !bIRa) {
                 bIRa = true;
 
                 IRa.Resize(2, omegan - 2);
                 for (int r = 0; r < 2; r++) {
                         for (int c = 0; c < omegan - 2; c++) {
                                 IRa(r + 1, c + 1) = dIRa[r][c];
                         }
                 }
 
                 IRat.Resize(omegan - 2, 2);
                 for (int r = 0; r < omegan - 2; r++) {
                         for (int c = 0; c < 2; c++) {
                                 IRat(r + 1, c + 1) = dIRa[c][r];
                         }
                 }
 
                 IPa.Resize(2, 2);
                 for (int r = 0; r < 2; r++) {
                         for (int c = 0; c < 2; c++) {
                                 IPa(r + 1, c + 1) = dIPa[r][c];
                         }
                 }
         }
 }

Here is the call graph for this function:

ModuleNonsmoothNode::~ModuleNonsmoothNode ( void )

virtual

Definition at line 547 of file module-nonsmooth-node.cc.

References NO_OP.

 {
         // destroy private data
         NO_OP;
 }

Member Function Documentation

void ModuleNonsmoothNode::AfterConvergence	(	const VectorHandler &	X,
		const VectorHandler &	XP
	)

virtual

Reimplemented from SimulationEntity.

Definition at line 638 of file module-nonsmooth-node.cc.

References bStepToggle, iIter, NS_subsys::Lambda_k, NS_subsys::Lambda_kp1, NS_subsys::niter, NS_data, NS_subsys::Piter, and Zero3.

 {
         // called after a step converged
         // store lambda at step k
         NS_data.Lambda_k = NS_data.Lambda_kp1;
         bStepToggle = false;
         iIter = 0;
         NS_data.niter = 0;
         NS_data.Piter = Zero3;
 }

void ModuleNonsmoothNode::AfterPredict	(	VectorHandler &	X,
		VectorHandler &	XP
	)

virtual

Reimplemented from SimulationEntity.

Definition at line 626 of file module-nonsmooth-node.cc.

References StructDispNode::GetVCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), NS_subsys::Lambda_kp1, m_pNode, NS_data, NS_subsys::Vns_kp1, and NS_subsys::Xns_kp1.

 {
         // called right after prediction, before any iteration within the time step
         // estrai Lambda1 e Lambda2 da XCurr e "mettili via" per il non-smooth
         integer iFirstIndex = iGetFirstIndex();
         NS_data.Lambda_kp1 = Vec3(X, iFirstIndex + 1);
         // Inizializzazione dello stato
         NS_data.Xns_kp1 = m_pNode->GetXCurr();  //NS state at step k +1
         NS_data.Vns_kp1 = m_pNode->GetVCurr();  //NS velocity at step k + 1
 }

Here is the call graph for this function:

VariableSubMatrixHandler & ModuleNonsmoothNode::AssJac	(	VariableSubMatrixHandler &	WorkMat,
		doublereal	dCoef,
		const VectorHandler &	XCurr,
		const VectorHandler &	XPrimeCurr
	)

virtual

Implements Elem.

Definition at line 651 of file module-nonsmooth-node.cc.

References ASSERT, NS_subsys::constraint_type, DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstMomentumIndex(), StructDispNode::iGetFirstPositionIndex(), m_pNode, NS_data, POSITION_AND_VELOCITY, POSITION_ONLY, SparseSubMatrixHandler::PutItem(), SparseSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetSparse(), and VELOCITY_ONLY.

 {
         SparseSubMatrixHandler& WM = WorkMat.SetSparse();
         integer iNode1FirstPositionIndex = m_pNode->iGetFirstPositionIndex();
         integer iNode1FirstMomentumIndex = m_pNode->iGetFirstMomentumIndex();
         integer iFirstIndex = iGetFirstIndex();
 
         switch (NS_data.constraint_type) {
         case POSITION_ONLY:
                 WM.ResizeReset(6, 1);
                 // add    + I Dl = -lambda      to existing momentum equation
                 WM.PutItem(1, iNode1FirstMomentumIndex + 1, iFirstIndex + 1, 1.);
                 WM.PutItem(2, iNode1FirstMomentumIndex + 2, iFirstIndex + 2, 1.);
                 WM.PutItem(3, iNode1FirstMomentumIndex + 3, iFirstIndex + 3, 1.);
                 // constraint equation on position: I dq  = q-q
                 WM.PutItem(4, iFirstIndex + 1, iNode1FirstPositionIndex + 1, 1.);
                 WM.PutItem(5, iFirstIndex + 2, iNode1FirstPositionIndex + 2, 1.);
                 WM.PutItem(6, iFirstIndex + 3, iNode1FirstPositionIndex + 3, 1.);
                 break;
 
         case VELOCITY_ONLY:
                 WM.ResizeReset(6, 1);
                 // add    + I Dl = -lambda      to existing momentum equation
                 WM.PutItem(1, iNode1FirstMomentumIndex + 1, iFirstIndex + 1, 1.);
                 WM.PutItem(2, iNode1FirstMomentumIndex + 2, iFirstIndex + 2, 1.);
                 WM.PutItem(3, iNode1FirstMomentumIndex + 3, iFirstIndex + 3, 1.);
                 // constraint equation on velocity:             I Dv = v-v
                 WM.PutItem(4, iFirstIndex + 1, iNode1FirstPositionIndex + 1, 1.);
                 WM.PutItem(5, iFirstIndex + 2, iNode1FirstPositionIndex + 2, 1.);
                 WM.PutItem(6, iFirstIndex + 3, iNode1FirstPositionIndex + 3, 1.);
                 break;
 
         case POSITION_AND_VELOCITY:
                 WM.ResizeReset(12, 1);
                 // add    + I Dl = -lambda      to existing momentum equation
                 WM.PutItem(1, iNode1FirstMomentumIndex + 1, iFirstIndex + 1, 1.);
                 WM.PutItem(2, iNode1FirstMomentumIndex + 2, iFirstIndex + 2, 1.);
                 WM.PutItem(3, iNode1FirstMomentumIndex + 3, iFirstIndex + 3, 1.);
                 // constraint equation on position, relaxed: I dq + I dmu = q-q -mu
                 WM.PutItem(4, iFirstIndex + 1, iNode1FirstPositionIndex + 1, dCoef);
                 WM.PutItem(5, iFirstIndex + 2, iNode1FirstPositionIndex + 2, dCoef);
                 WM.PutItem(6, iFirstIndex + 3, iNode1FirstPositionIndex + 3, dCoef);
                 WM.PutItem(7, iFirstIndex + 1, iFirstIndex + 4, 1.);
                 WM.PutItem(8, iFirstIndex + 2, iFirstIndex + 5, 1.);
                 WM.PutItem(9, iFirstIndex + 3, iFirstIndex + 6, 1.);
                 // constraint equation on velocity:             I Dv = v-v
                 WM.PutItem(10, iFirstIndex + 4, iNode1FirstPositionIndex + 1, 1.);
                 WM.PutItem(11, iFirstIndex + 5, iNode1FirstPositionIndex + 2, 1.);
                 WM.PutItem(12, iFirstIndex + 6, iNode1FirstPositionIndex + 3, 1.);
                 break;
 
         default:
                 // impossible
                 ASSERT(0);
         }
 
         return WorkMat;
 }

Here is the call graph for this function:

SubVectorHandler & ModuleNonsmoothNode::AssRes	(	SubVectorHandler &	WorkVec,
		doublereal	dCoef,
		const VectorHandler &	XCurr,
		const VectorHandler &	XPrimeCurr
	)

virtual

Implements Elem.

Definition at line 714 of file module-nonsmooth-node.cc.

References bFrictional, GravityOwner::bGetGravity(), NS_subsys::bGravity, NS_subsys::bVerbose, NS_subsys::constraint_type, DriveHandler::dGetTimeStep(), WithLabel::GetLabel(), StructDispNode::GetVCurr(), StructDispNode::GetVPrev(), StructDispNode::GetXCurr(), StructDispNode::GetXPrev(), NS_subsys::GravityAcceleration, NS_subsys::hstep, DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstMomentumIndex(), DriveHandler::iGetStep(), iIter, NS_subsys::iterations, NS_subsys::Lambda_kp1, m_pDM, m_pNode, mbs_get_force(), mbs_get_force_frictional(), NS_subsys::mu, Mu, NS_data, DataManager::pGetDrvHdl(), POSITION_AND_VELOCITY, POSITION_ONLY, SubVectorHandler::PutItem(), VectorHandler::ResizeReset(), VELOCITY_ONLY, NS_subsys::Vns_k, NS_subsys::Vns_kp1, NS_subsys::Xns_k, and NS_subsys::Xns_kp1.

 {
         integer iNode1FirstIndex = m_pNode->iGetFirstMomentumIndex();
         integer iFirstIndex = iGetFirstIndex();
         //      Variables needed for the iteration:
         //      timestep value, gravity acceleration (if it is set),
         //      position and velocity of the node coincident with the nonsmooth node,
         //      forces applied on that node by the rest of the model.
         NS_data.hstep = m_pDM->pGetDrvHdl()->dGetTimeStep();
         const Vec3& Xn = m_pNode->GetXCurr();
         NS_data.bGravity = GravityOwner::bGetGravity(Xn, NS_data.GravityAcceleration);
 
         NS_data.Xns_k = m_pNode->GetXPrev();
         NS_data.Vns_k = m_pNode->GetVPrev();
 
         NS_data.Xns_kp1 = m_pNode->GetXCurr();
 //      std::cout << "NS_data.Xns_kp1="<<NS_data.Xns_kp1;
         NS_data.Vns_kp1 = m_pNode->GetVCurr();
         NS_data.Lambda_kp1 = Vec3(XCurr, iFirstIndex + 1);
 
         // integra un'iterazione del systema NonSmooth e restituisci il nuovo Lambda
         // e Xns1 e Vns1 all'interno di NS_data
 
         if (((NS_data.iterations > 0) && (iIter < NS_data.iterations)) || (NS_data.iterations == 0)) {
                 if (bFrictional) {
                         mbs_get_force_frictional(NS_data);
 
                 } else {
                         mbs_get_force(NS_data);
                 }
                 iIter++;
         }
 
         Vec3 DX1(NS_data.Xns_kp1 - m_pNode->GetXCurr());
         Vec3 DV1(NS_data.Vns_kp1 - m_pNode->GetVCurr());
 
         if (NS_data.bVerbose) {
                 silent_cout("ModuleNonsmoothNode(" << GetLabel() << ") step=" << m_pDM->pGetDrvHdl()->iGetStep() << std::endl
                         << "  NS_data.Xns_kp1={" << NS_data.Xns_kp1 << "} DX={" << DX1 << "}" << std::endl
                         << "  NS_data.Vns_kp1={" << NS_data.Vns_kp1 << "} DV={" << DV1 << "}" << std::endl
                         << "  NS_data.Lambda_kp1={" << NS_data.Lambda_kp1 << "}" << std::endl);
         }
 
         switch (NS_data.constraint_type) {
         case POSITION_ONLY:
                 WorkVec.ResizeReset(6);
                 WorkVec.PutItem(1, iNode1FirstIndex + 1, - NS_data.Lambda_kp1(1));
                 WorkVec.PutItem(2, iNode1FirstIndex + 2, - NS_data.Lambda_kp1(2));
                 WorkVec.PutItem(3, iNode1FirstIndex + 3, - NS_data.Lambda_kp1(3));
                 WorkVec.PutItem(4, iFirstIndex + 1, DX1(1)/dCoef);
                 WorkVec.PutItem(5, iFirstIndex + 2, DX1(2)/dCoef);
                 WorkVec.PutItem(6, iFirstIndex + 3, DX1(3)/dCoef);
                 break;
 
         case VELOCITY_ONLY:
                 WorkVec.ResizeReset(6);
                 WorkVec.PutItem(1, iNode1FirstIndex + 1, - NS_data.Lambda_kp1(1));
                 WorkVec.PutItem(2, iNode1FirstIndex + 2, - NS_data.Lambda_kp1(2));
                 WorkVec.PutItem(3, iNode1FirstIndex + 3, - NS_data.Lambda_kp1(3));
                 WorkVec.PutItem(4, iFirstIndex + 1, DV1(1));
                 WorkVec.PutItem(5, iFirstIndex + 2, DV1(2));
                 WorkVec.PutItem(6, iFirstIndex + 3, DV1(3));
                 break;
 
         case POSITION_AND_VELOCITY:
                 WorkVec.ResizeReset(9);
                 Mu = Vec3(XCurr, iFirstIndex + 4);
                 // residuo per le equazioni di congruenza del nodo 1
                 DX1 -= Mu;
                 NS_data.mu = Mu;
                 WorkVec.PutItem(1, iNode1FirstIndex + 1, - NS_data.Lambda_kp1(1));
                 WorkVec.PutItem(2, iNode1FirstIndex + 2, - NS_data.Lambda_kp1(2));
                 WorkVec.PutItem(3, iNode1FirstIndex + 3, - NS_data.Lambda_kp1(3));
                 WorkVec.PutItem(4, iFirstIndex + 1, DX1(1));
                 WorkVec.PutItem(5, iFirstIndex + 2, DX1(2));
                 WorkVec.PutItem(6, iFirstIndex + 3, DX1(3));
                 WorkVec.PutItem(7, iFirstIndex + 4, DV1(1));
                 WorkVec.PutItem(8, iFirstIndex + 5, DV1(2));
                 WorkVec.PutItem(9, iFirstIndex + 6, DV1(3));
                 break;
         }
 
         if (NS_data.bVerbose) {
                 silent_cout("ModuleNonsmoothNode(" << GetLabel() << ") workvec:" << std::endl << WorkVec << std::endl);
         }
 
         return WorkVec;
 }

Here is the call graph for this function:

void ModuleNonsmoothNode::GetConnectedNodes ( std::vector< const Node * > & connectedNodes ) const

virtual

Reimplemented from Elem.

Definition at line 820 of file module-nonsmooth-node.cc.

References NS_subsys::constr, iGetNumConnectedNodes(), m_pNode, and NS_data.

 {
         connectedNodes.resize(iGetNumConnectedNodes());
         connectedNodes[0] = m_pNode;
         unsigned i = 0;
         for (; i < NS_data.constr.size(); i++) {
                 connectedNodes[i + 1] = NS_data.constr[i].AttNode;
         }
 }

Here is the call graph for this function:

DofOrder::Order ModuleNonsmoothNode::GetDofType ( unsigned int ) const

virtual

Reimplemented from Elem.

Definition at line 596 of file module-nonsmooth-node.cc.

References DofOrder::ALGEBRAIC.

 {
         return DofOrder::ALGEBRAIC;
 }

unsigned int ModuleNonsmoothNode::iGetInitialNumDof ( void ) const

virtual

Implements SubjectToInitialAssembly.

Definition at line 845 of file module-nonsmooth-node.cc.

 {
         return 0;
 }

int ModuleNonsmoothNode::iGetNumConnectedNodes ( void ) const

Definition at line 813 of file module-nonsmooth-node.cc.

References NS_subsys::constr, and NS_data.

Referenced by GetConnectedNodes().

 {
         // silent_cerr("start iGetNumConnectedNodes " << std::endl);
         return 1 + NS_data.constr.size();
 }

unsigned int ModuleNonsmoothNode::iGetNumDof ( void ) const

virtual

Reimplemented from Elem.

Definition at line 554 of file module-nonsmooth-node.cc.

References ASSERT, NS_subsys::constraint_type, MBDYN_EXCEPT_ARGS, NS_data, POSITION_AND_VELOCITY, POSITION_ONLY, and VELOCITY_ONLY.

 {
         switch (NS_data.constraint_type) {
         case POSITION_ONLY:
                 return 3;
 
         case VELOCITY_ONLY:
                 return 3;
 
         case POSITION_AND_VELOCITY:
                 return 6;
         }
 
         // impossible
         ASSERT(0);
         throw ErrGeneric(MBDYN_EXCEPT_ARGS);
 }

unsigned int ModuleNonsmoothNode::iGetNumPrivData ( void ) const

virtual

Reimplemented from SimulationEntity.

Definition at line 807 of file module-nonsmooth-node.cc.

 {
         return 0;
 }

VariableSubMatrixHandler & ModuleNonsmoothNode::InitialAssJac	(	VariableSubMatrixHandler &	WorkMat,
		const VectorHandler &	XCurr
	)

virtual

Implements SubjectToInitialAssembly.

Definition at line 858 of file module-nonsmooth-node.cc.

References ASSERT, and VariableSubMatrixHandler::SetNullMatrix().

 {
         // should not be called, since initial workspace is empty
         ASSERT(0);
         WorkMat.SetNullMatrix();
         return WorkMat;
 }

Here is the call graph for this function:

SubVectorHandler & ModuleNonsmoothNode::InitialAssRes	(	SubVectorHandler &	WorkVec,
		const VectorHandler &	XCurr
	)

virtual

Implements SubjectToInitialAssembly.

Definition at line 869 of file module-nonsmooth-node.cc.

References ASSERT, and VectorHandler::ResizeReset().

 {
         // should not be called, since initial workspace is empty
         ASSERT(0);
         WorkVec.ResizeReset(0);
         return WorkVec;
 }

Here is the call graph for this function:

void ModuleNonsmoothNode::InitialWorkSpaceDim	(	integer *	piNumRows,
		integer *	piNumCols
	)		const

virtual

Implements SubjectToInitialAssembly.

Definition at line 851 of file module-nonsmooth-node.cc.

 {
         *piNumRows = 0;
         *piNumCols = 0;
 }

void ModuleNonsmoothNode::mbs_get_force ( NS_subsys & NS )

private

Definition at line 907 of file module-nonsmooth-node.cc.

References NS_subsys::bGravity, bStepToggle, NS_subsys::constr, Eye3, NS_subsys::gamma_pred, NS_subsys::GravityAcceleration, NS_subsys::hstep, NS_subsys::ia, NS_subsys::Lambda_k, NS_subsys::Lambda_kp1, NS_subsys::mass_ns, mbdyn_siconos_LCP_call(), MatNxN::Mult(), NS_subsys::niter, NS_subsys::niterLCPmax, Vec3::Norm(), NS_subsys::Np, VecN::pGetVec(), NS_subsys::Piter, NS_subsys::pkp1, NS_subsys::Poutput, Mat3xN::PutVec(), NS_subsys::radius_ns, MatNx3::RightMult(), NS_subsys::solparam, NS_subsys::theta_ts, MatNx3::Transpose(), NS_subsys::Vns_k, NS_subsys::Vns_kp1, NS_subsys::Xns_k, NS_subsys::Xns_kp1, and Zero3.

Referenced by AssRes().

 {
         // Some Output initialization
         Vec3 p_kp1 = Zero3;
         NS.pkp1 = 0.;
         NS.ia = 0;
 
 //      Mat3x3 M_ns = Eye3 * NS.mass_ns;
 //      Mat3x3 K_ns = Zero3x3;
 //      Mat3x3 C_ns = Zero3x3;
 //      Mat3x3 Mhat = M_ns + C_ns * (NS.hstep * NS.theta_ts) + K_ns * (NS.hstep * NS.hstep * NS.theta_ts * NS.theta_ts);
 //      Mat3x3 invMhat = Mhat.Inv();
         Mat3x3 M_ns = Eye3 * NS.mass_ns;
         Mat3x3 invMhat = Eye3 * (1./NS.mass_ns);
 
         // Determine the predicted NS state at the step k+1, to verify if the gap is closed
         Vec3 x_NS_predicted = NS.Xns_k + NS.Vns_k * (NS.hstep *  NS.gamma_pred);
 
         // Create index of active constraints: Ia, through evaluation of the gap distance.
         std::vector<int> Ia;
         for (int i = 0; i < NS.Np; i++) {
                 // Store the orientation matrix H of the reference frame with Z aligned with contact direction
                 NS.constr[i].H_kp1 = NS.constr[i].AttNode->GetRCurr() * NS.constr[i].RotRel;
                 NS.constr[i].H_k = NS.constr[i].AttNode->GetRPrev() * NS.constr[i].RotRel;
 
                 Vec3 Plane_point = NS.constr[i].AttNode->GetXCurr() + NS.constr[i].AttNode->GetRCurr() * NS.constr[i].Point;
                 NS.constr[i].Gap = (x_NS_predicted - Plane_point) * NS.constr[i].H_kp1.GetVec(3) - NS.radius_ns;
 
                 // Valuto gap(xpredicted) e creo indice vincoli attivi
                 if (NS.constr[i].Gap <= 0.) {
                         Ia.push_back(i);
                 }
         }
 
 #if 0
         // NOTE: if no active contact, directly integrate the dynamics of the mass
         if (Ia.empty()) {
         }
 #endif
 
         // External forces: gravity and the reaction passed from the rest of the system integrated by MBDYN
         Vec3 F_k = NS.Lambda_k;
         Vec3 F_kp1 = NS.Lambda_kp1;
         Vec3 Fg;
         if (NS.bGravity) {
                 Fg = NS.GravityAcceleration * NS.mass_ns;
                 F_k += Fg;
                 F_kp1 += Fg;
         }
 
         // Velocity of the non-smooth subsystem calculated without the contribute of contact impulse
         // Vec3 vfree_kp1= NS.Vns_k + invMhat * ( - C_ns*NS.Vns_k*NS.hstep - K_ns*NS.Xns_k*NS.hstep - K_ns*NS.Vns_k*(NS.hstep*NS.hstep*NS.theta_ts) + (F_kp1*NS.theta_ts + F_k* (1.-NS.theta_ts) ) *NS.hstep);
         Vec3 vfree_kp1 = NS.Vns_k + invMhat * (F_kp1*NS.theta_ts + F_k* (1. - NS.theta_ts)) * NS.hstep;
 
         Vec3 VfreeRel = vfree_kp1 - NS.constr[0].AttNode->GetVCurr();           // VfreeMinusVplane
         Vec3 VnskRel = NS.Vns_k - NS.constr[0].AttNode->GetVCurr();
 
         // Composing and solving the Linear Complementarity System to determine the contact impulse p_kp1
         if (!Ia.empty()) {
                 Mat3xN Hfull_kp1(Ia.size());
                 Mat3xN Hfull_k(Ia.size());
 
                 for (unsigned i = 0; i < Ia.size(); i++) {
                         Hfull_kp1.PutVec(i + 1, NS.constr[Ia[i]].H_kp1.GetVec(3));
                         Hfull_k.PutVec(i + 1, NS.constr[Ia[i]].H_k.GetVec(3));
                 }
 
                 MatNx3 Hfull_kp1T(Ia.size());
                 Hfull_kp1T.Transpose(Hfull_kp1);
                 MatNx3 Hfull_kT(Ia.size());
                 Hfull_kT.Transpose(Hfull_k);
                 MatNx3 HTW(Ia.size());
                 HTW.RightMult(Hfull_kp1T, invMhat);
                 MatNxN W_delassus(Ia.size(), 0.);
                 W_delassus.Mult(HTW, Hfull_kp1);
 
                 // Attenzione, matrici riempite alla FORTRAN! Passo a Siconos le matrici come le vuole lui.
                 double W_passed[Ia.size()*Ia.size()];
                 int indice = 0;
                 for (unsigned i = 1; i < Ia.size() + 1; i++) {
                         for (unsigned j = 1; j < Ia.size() + 1; j++) {
                                 W_passed[indice] = W_delassus(j, i);
                                 indice++;
                         }
                 }
 
                 double bLCP_new[Ia.size()];
                 for (unsigned i = 0; i < Ia.size(); i++) {
                         bLCP_new[i] = Hfull_kp1T.GetVec(i + 1) * VfreeRel + Hfull_kT.GetVec(i + 1) * VnskRel * NS.constr[Ia[i]].e_rest;
                 }
 
                 VecN P(Ia.size(), 0.);
                 VecN U(Ia.size(), 0.);
 
                 if ((NS.niter < NS.niterLCPmax) || (NS.niterLCPmax == 0)) {
                         // Solving the LCP with one of Siconos solvers
                         mbdyn_siconos_LCP_call(Ia.size(), W_passed, bLCP_new, P.pGetVec(), U.pGetVec(), NS.solparam);
 
                         // Reazioni nel sistema di riferimento globale
                         p_kp1 = Hfull_kp1 * P;
 
                         NS.Piter = p_kp1;
 
                         NS.niter++;
                 }
         }
 
         // Updating: state actualization with the contribution of the contact impulse
         NS.Vns_kp1 = vfree_kp1 + invMhat * NS.Piter;
         NS.Xns_kp1  = NS.Xns_k + (NS.Vns_kp1 * NS.theta_ts + NS.Vns_k* (1. - NS.theta_ts)) * NS.hstep;
         // Reaction force to be exchanged with the rest of the system in MBDYN
         NS.Lambda_kp1 = p_kp1 / NS.hstep - M_ns * (NS.Vns_kp1 - NS.Vns_k) / NS.hstep + NS.Lambda_k*(1. - NS.theta_ts);
         if (NS.bGravity) {
                 NS.Lambda_kp1 += Fg;
         }
         NS.Lambda_kp1 /= -NS.theta_ts;
 
         // Store output
         NS.ia = Ia.size();
         NS.pkp1 = p_kp1.Norm();
         NS.Poutput = p_kp1;
         // p_kp1 = NS.Piter;
 
         // Indicatore di nuovo step, per evitare troppo output con bVerbose.
         bStepToggle = true;
 }

Here is the call graph for this function:

void ModuleNonsmoothNode::mbs_get_force_frictional ( NS_subsys & NS )

private

Definition at line 1035 of file module-nonsmooth-node.cc.

References FullMatrixHandler::Add(), NS_subsys::bGravity, bStepToggle, NS_subsys::constr, Eye3, NS_subsys::gamma_pred, NS_subsys::GravityAcceleration, NS_subsys::hstep, NS_subsys::ia, NS_subsys::Lambda_k, NS_subsys::Lambda_kp1, NS_subsys::mass_ns, FullMatrixHandler::MatMul(), MatrixHandler::MatVecMul(), mbdyn_siconos_LCP_call(), NS_subsys::niter, NS_subsys::niterLCPmax, NS_subsys::Np, FullMatrixHandler::pdGetMat(), NS_subsys::Piter, NS_subsys::pkp1, NS_subsys::Poutput, STLVectorHandler::Put(), FullMatrixHandler::Put(), FullMatrixHandler::PutT(), NS_subsys::radius_ns, NS_subsys::solparam, FullMatrixHandler::Sub(), NS_subsys::theta_ts, NS_subsys::Vns_k, NS_subsys::Vns_kp1, NS_subsys::Xns_k, NS_subsys::Xns_kp1, and Zero3.

Referenced by AssRes().

 {
 //------------------------------------------------------------------------------
 
 //      Mat3x3 M_ns = Eye3 * NS.mass_ns;
 //      Mat3x3 K_ns = Zero3x3;
 //      Mat3x3 C_ns = Zero3x3;
 //      Mat3x3 Mhat = M_ns + C_ns * (NS.hstep * NS.theta_ts) + K_ns * (NS.hstep * NS.hstep * NS.theta_ts * NS.theta_ts);
 //      Mat3x3 invMhat = Mhat.Inv();
         Mat3x3 M_ns = Eye3 * NS.mass_ns;
         Mat3x3 invMhat = Eye3 * (1./NS.mass_ns);
 
         Vec3 p_kp1 = Zero3;
         NS.Poutput = Zero3;
         NS.ia = 0;
 
         // Determine the predicted NS state at the step k+1, to verify if the gap is closed
         Vec3 x_NS_predicted = NS.Xns_k + NS.Vns_k * NS.hstep * NS.gamma_pred;
 
         // Create index of active constraints: Ia, through evaluation of the gap distance.
         std::vector<int> Ia;
         for (int i = 0; i < NS.Np; i++) {
                 // Store the orientation matrix H of the reference frame with Z aligned with contact direction
                 NS.constr[i].H_kp1 = NS.constr[i].AttNode->GetRCurr() * NS.constr[i].RotRel;
                 NS.constr[i].H_k = NS.constr[i].AttNode->GetRPrev() * NS.constr[i].RotRel;
 
                 Vec3 Plane_point = NS.constr[i].AttNode->GetXCurr() + NS.constr[i].AttNode->GetRCurr() * NS.constr[i].Point;
                 NS.constr[i].Gap = (x_NS_predicted -  Plane_point) * NS.constr[i].H_kp1.GetVec(3) - NS.radius_ns;
 
                 // Valuto gap(xpredicted) e creo indice vincoli attivi
                 if (NS.constr[i].Gap <= 0.) {
                         Ia.push_back(i);
                 }
         }
 
 #if 0
         // NOTE: if no active contact, directly integrate the dynamics of the mass
         if (Ia.empty()) {
         }
 #endif
 
         // External forces: gravity and the reaction passed from the rest of the system integrated by MBDYN
         Vec3 F_k = NS.Lambda_k;
         Vec3 F_kp1 = NS.Lambda_kp1;
         Vec3 Fg;
         if (NS.bGravity) {
                 Fg = NS.GravityAcceleration * NS.mass_ns;
                 F_k += Fg;
                 F_kp1 += Fg;
         }
 
         // Velocity of the non-smooth subsystem calculated without the contribute of contact impulse
         // Vec3 vfree_kp1= NS.Vns_k + invMhat * ( - C_ns*NS.Vns_k*NS.hstep - K_ns*NS.Xns_k*NS.hstep - K_ns*NS.Vns_k*(NS.hstep*NS.hstep*NS.theta_ts) + (F_kp1*NS.theta_ts + F_k* (1.-NS.theta_ts) ) *NS.hstep);
         Vec3 vfree_kp1 = NS.Vns_k + invMhat * (F_kp1*NS.theta_ts + F_k * (1. - NS.theta_ts)) * NS.hstep;
         Vec3 VfreeRel = vfree_kp1 - NS.constr[0].AttNode->GetVCurr();   // VfreeMinusVplane
 
         if (Ia.size() > 0) {
                 int ac = Ia.size();                     // active constraints !!!
                 FullMatrixHandler H_NN(3, ac);
                 FullMatrixHandler H_TT(3, 2*ac);
                 FullMatrixHandler H_NNt(ac, 3);
                 FullMatrixHandler H_TTt(2*ac, 3);
 
                 FullMatrixHandler Htot(3, 3*ac);
 
                 FullMatrixHandler I_P(2*ac, 2*ac);
                 FullMatrixHandler I_R(2*ac, (omegan - 2)*ac);
                 FullMatrixHandler I_Rt((omegan - 2)*ac, 2*ac);
                 FullMatrixHandler mu_P(2*ac, ac);
                 FullMatrixHandler mu_R((omegan - 2)*ac, ac);
                 FullMatrixHandler en(1, ac);
 
                 for (unsigned i = 0; i < Ia.size(); i++) {
                         en(1, i + 1) = NS.constr[Ia[i]].e_rest;
 
                         H_NN.Put(1, i + 1, NS.constr[Ia[i]].H_kp1.GetVec(3));
                         H_TT.Put(1, 2*i + 1, NS.constr[Ia[i]].H_kp1.GetVec(1));
                         H_TT.Put(1, 2*i + 2, NS.constr[Ia[i]].H_kp1.GetVec(2));
                         H_NNt.PutT(i + 1, 1, NS.constr[Ia[i]].H_kp1.GetVec(3));
                         H_TTt.PutT(2*i + 1, 1, NS.constr[Ia[i]].H_kp1.GetVec(1));
                         H_TTt.PutT(2*i + 2, 1, NS.constr[Ia[i]].H_kp1.GetVec(2));
 
                         I_P.Put(2*(i + 1 - 1) + 1, 2*(i + 1 - 1) + 1, IPa);
                         I_R.Put(2*(i + 1 - 1) + 1, (omegan - 2)*(i + 1 - 1) + 1, IRa);
                         I_Rt.Put((omegan - 2)*(i + 1 - 1) + 1, 2*(i + 1 - 1) + 1, IRat);
 
                         FullMatrixHandler mu_Piw(2, 1);
                         mu_Piw(1, 1) = NS.constr[Ia[i]].mu;
                         mu_Piw(2, 1) = NS.constr[Ia[i]].mu;
 
                         FullMatrixHandler mu_Riw(omegan - 2, 1);
 
                         for (int in = 1; in < omegan - 2 + 1; in++) {
                                 mu_Riw(in, 1) = NS.constr[Ia[i]].mu;
                         }
 
                         mu_P.Put(2*(i + 1 - 1) + 1, i + 1, mu_Piw);
                         mu_R.Put((omegan - 2)*(i + 1 - 1) + 1, i + 1, mu_Riw);
                 }
 
                 Htot.Put(1, 1, H_TT);
                 Htot.Put(1, 2*ac + 1, H_NN);
 
                 FullMatrixHandler v_free(3, 1);
                 v_free.Put(1, 1, VfreeRel);
                 FullMatrixHandler U_Nfree(1*ac, 1);
                 FullMatrixHandler U_Tfree(2*ac, 1);
                 U_Nfree.MatMul(H_NNt, v_free);
                 U_Tfree.MatMul(H_TTt, v_free);
 
                 FullMatrixHandler iMhat(3, 3);
                 iMhat.Put(1, 1, invMhat);
 
                 FullMatrixHandler WNNl(ac, 3);
                 WNNl.MatMul(H_NNt, iMhat);
                 FullMatrixHandler WNN(ac, ac);
                 WNN.MatMul(WNNl, H_NN);
 
                 FullMatrixHandler WTTl(2*ac, 3);
                 WTTl.MatMul(H_TTt, iMhat);
                 FullMatrixHandler WTT(2*ac, 2*ac);
                 WTT.MatMul(WTTl, H_TT);
 
                 FullMatrixHandler WNTl(ac, 3);
                 WNTl.MatMul(H_NNt, iMhat);
                 FullMatrixHandler WNT(ac, 2*ac);
                 WNT.MatMul(WNTl, H_TT);
 
                 FullMatrixHandler WTNl(2*ac, 3);
                 WTNl.MatMul(H_TTt, iMhat);
                 FullMatrixHandler WTN(2*ac, ac);
                 WTN.MatMul(WTNl, H_NN);
 
                 // FIXME: adesso è facile perchè eye(2*ac)
                 FullMatrixHandler IPmT(2*ac, 2*ac);
                 for (int ipmti = 1; ipmti < 2*ac + 1; ipmti++) {
                         IPmT(ipmti, ipmti) = 1;
                 }
 
                 // MLCP construction
                 int ProbDim = (3 + omegan - 2)*ac;
                 FullMatrixHandler Mlcp(ProbDim, ProbDim);
                 FullMatrixHandler mlcp11(ac, ac);
                 FullMatrixHandler mlcp12(ac, 2*ac);
                 FullMatrixHandler mlcp21(2*ac, ac);
                 FullMatrixHandler mlcp22(2*ac, 2*ac);
                 FullMatrixHandler mlcp23(2*ac, (omegan - 2)*ac);
                 FullMatrixHandler mlcp31((omegan - 2)*ac, ac);
                 FullMatrixHandler mlcp32((omegan - 2)*ac, 2*ac);
 
                 FullMatrixHandler WNT_IPmT(ac, 2*ac);
                 WNT_IPmT.MatMul(WNT, IPmT);
                 FullMatrixHandler WNT_IPmT_muP(ac, ac);
                 WNT_IPmT_muP.MatMul(WNT_IPmT, mu_P);
 
                 mlcp11.Put(1, 1, WNN);
                 mlcp11.Add(1, 1, WNT_IPmT_muP);
                 mlcp12.Sub(1, 1, WNT_IPmT);
 
                 FullMatrixHandler WTT_IPmT(2*ac, 2*ac);
                 WTT_IPmT.MatMul(WTT, IPmT);
                 FullMatrixHandler WTT_IPmT_muP(2*ac, ac);
                 WTT_IPmT_muP.MatMul(WTT_IPmT, mu_P);
                 WTT_IPmT_muP.Add(1, 1, WTN);
                 mlcp21.Sub(1, 1, WTT_IPmT_muP);
 
                 mlcp22.Put(1, 1, WTT);
 
                 mlcp23.Sub(1, 1, I_R);
 
                 FullMatrixHandler I_Rt_mu_P((omegan - 2)*ac, ac);
                 I_Rt_mu_P.MatMul(I_Rt, mu_P);
 
                 mlcp31.Add(1, 1, mu_R);
                 mlcp31.Sub(1, 1, I_Rt_mu_P);
                 mlcp32.Add(1, 1, I_Rt);
 
                 Mlcp.Put(1, 1, mlcp11);
                 Mlcp.Put(1, ac + 1, mlcp12);
                 Mlcp.Put(ac + 1, 1, mlcp21);
                 Mlcp.Put(ac + 1, ac + 1, mlcp22);
                 Mlcp.Put(ac + 1, 3*ac + 1, mlcp23);
                 Mlcp.Put(3*ac + 1, 1, mlcp31);
                 Mlcp.Put(3*ac + 1, ac + 1, mlcp32);
 
                 // FIXME: use STLVectorHandler?
                 FullMatrixHandler qlcp(ProbDim, 1);
                 FullMatrixHandler qlcp1(ac, 1);
 
                 STLVectorHandler VnskMinusVplane(3);
                 VnskMinusVplane.Put(1, NS.Vns_k - NS.constr[0].AttNode->GetVCurr());
 
                 STLVectorHandler U_N(ac);
                 H_NNt.MatVecMul(U_N, VnskMinusVplane);
 
                 for (int ien = 1; ien < ac + 1; ien++) {
                         qlcp1(ien, 1) = U_N(ien)*en(1, ien);
                 }
 
                 qlcp1.Add(1, 1, U_Nfree);
                 qlcp.Put(1, 1, qlcp1);
                 qlcp.Sub(ac + 1, 1, U_Tfree);
 
 #if 0
                 // SOME OUTPUT TO DEBUG
 
                 std::cout<<" H_NN" << std::endl << H_NN << std::endl;
                 std::cout<<" H_TT" << std::endl << H_TT << std::endl;
                 std::cout<<" I_P" << std::endl << I_P << std::endl;
                 std::cout<<" I_R" << std::endl << I_R << std::endl;
                 std::cout<<" I_Rt" << std::endl << I_Rt << std::endl;
                 std::cout<<" mu_P" << std::endl << mu_P << std::endl;
                 std::cout<<" mu_R" << std::endl << mu_R << std::endl;
                 std::cout<<" WNN" << std::endl << WNN << std::endl;
                 std::cout<<" WNT" << std::endl << WNT << std::endl;
                 std::cout<<" WTN" << std::endl << WTN << std::endl;
                 std::cout<<" WTT" << std::endl << WTT << std::endl;
                 std::cout<<" IPmT" << std::endl << IPmT << std::endl;
 //              std::cout<< "MLCP" << std::endl << MLCP << std::endl;
 //              std::cout<< "qLCP" << std::endl << qLCP << std::endl;
 #endif
 
                 double Pkp1[ProbDim];
                 double Unkp1[ProbDim];
 
                 // NOTE: resetting Pkp1, Unkp1 is mandatory because some times
                 // they are not (r)eset in mbdyn_siconos_LCP_call()
                 memset(Pkp1, 0, sizeof(Pkp1));
                 memset(Unkp1, 0, sizeof(Unkp1));
 
                 if ((NS.niter < NS.niterLCPmax) || (NS.niterLCPmax == 0)) {
                         // Solving the LCP with one of Siconos solvers
                         mbdyn_siconos_LCP_call(ProbDim, Mlcp.pdGetMat(), qlcp.pdGetMat(), &Pkp1[0], &Unkp1[0], NS.solparam);
 
                         STLVectorHandler PN(ac);
                         STLVectorHandler sigmaP(2*ac);
                         STLVectorHandler PT(2*ac);
                         STLVectorHandler Ptot(3*ac);
                         STLVectorHandler ptot(3);
 
                         for (int i = 1; i <= ac; i++) {
                                 PN(i) = Pkp1[i - 1];
                         }
                         for (int i = ac + 1; i <= 3*ac; i++) {
                                 sigmaP(i - ac) = Pkp1[i - 1];
                         }
                         mu_P.MatVecMul(PT, PN);
                         PT -= sigmaP;
                         for (int i = 1; i <= 2*ac; i++) {
                                 Ptot(i) = PT(i);
                         }
                         for (int i = 2*ac + 1; i <= 3*ac; i++) {
                                 Ptot(i) = PN(i - (2*ac));
                         }
                         Htot.MatVecMul(ptot, Ptot);
 
                         p_kp1(1) = ptot(1);
                         p_kp1(2) = ptot(2);
                         p_kp1(3) = ptot(3);
 
                         NS.Piter = p_kp1;
 
                         NS.niter++;
                 }
 
 //              // Solving the LCP with one of Siconos solvers
 //              mbdyn_siconos_LCP_call(ProbDim, Mlcp_passed, qlcp_passed,  Pkp1,  Unkp1, NS.solparam);
 
 //              FullMatrixHandler PN(ac, 1);
 //              FullMatrixHandler sigmaP(2*ac, 1);
 //              FullMatrixHandler PT(2*ac, 1);
 //              FullMatrixHandler Ptot(3*ac, 1);
 //              FullMatrixHandler ptot(3, 1);
 //
 //              for (int i = 1; i < ac + 1; i++) {
 //                      PN(i, 1) = Pkp1[i - 1];
 //              }
 //              for (int i = ac; i < 3*ac; i++) {
 //                      sigmaP(i - ac + 1, 1) = Pkp1[i];
 //              }
 //              PT.MatMul(mu_P, PN);
 //              PT.Sub(1, 1, sigmaP);
 //              for (int i = 1; i < 2*ac + 1; i++) {
 //                      Ptot(i, 1) = PT(i, 1);
 //              }
 //              for (int i = 2*ac + 1; i < 3*ac + 1; i++) {
 //                      Ptot(i, 1) = PN(i - (2*ac), 1);
 //              }
 //
 //              ptot.MatMul(Htot, Ptot);
 //
 //              p_kp1(1) = ptot(1, 1);
 //              p_kp1(2) = ptot(2, 1);
 //              p_kp1(3) = ptot(3, 1);
         }
 
         // Updating: state actualization with the contribution of the contact impulse
 //      NS.Vns_kp1 = vfree_kp1 + invMhat * p_kp1;
         NS.Vns_kp1 = vfree_kp1 + invMhat * NS.Piter;
         NS.Xns_kp1 = NS.Xns_k + (NS.Vns_kp1 * NS.theta_ts + NS.Vns_k* (1. - NS.theta_ts)) * NS.hstep;
 
         // Reaction force to be exchanged with the rest of the system in MBDYN
         NS.Lambda_kp1 = p_kp1 / NS.hstep - M_ns * (NS.Vns_kp1 - NS.Vns_k) / NS.hstep + NS.Lambda_k*(1. - NS.theta_ts);
         if (NS.bGravity) {
                 NS.Lambda_kp1 += Fg;
         }
         NS.Lambda_kp1 /= -NS.theta_ts;
 
         // Store Output
         NS.ia = Ia.size();
         NS.pkp1 = p_kp1.Norm();
         NS.Poutput = p_kp1;
 
         // Indicatore di nuovo step, per evitare troppo output con bVerbose.
         bStepToggle = true;
 }

Here is the call graph for this function:

void ModuleNonsmoothNode::Output ( OutputHandler & OH ) const

virtual

Reimplemented from ToBeOutput.

Definition at line 880 of file module-nonsmooth-node.cc.

References ToBeOutput::bToBeOutput(), NS_subsys::bVerbose, WithLabel::GetLabel(), NS_subsys::ia, solver_parameters::info, NS_subsys::Lambda_kp1, OutputHandler::Loadable(), NS_subsys::mu, NS_data, NS_subsys::pkp1, NS_subsys::Poutput, solver_parameters::processed_iterations, solver_parameters::resulting_error, NS_subsys::solparam, NS_subsys::Vns_kp1, and NS_subsys::Xns_kp1.

 {
         if (bToBeOutput()) {
                 std::ostream& out = OH.Loadable();
                 out << std::setw(8) << GetLabel()
                         << " " << NS_data.Poutput
                         << " " << NS_data.Xns_kp1
                         << " " << NS_data.Vns_kp1
                         << " " << NS_data.Lambda_kp1
                         << " " << NS_data.pkp1
                         << " " << NS_data.ia;
 
                 if (NS_data.bVerbose) {
                         out
                                 << " " << NS_data.mu
                                 << " " << NS_data.solparam.info
                                 << " " << NS_data.solparam.resulting_error
                                 << " " << NS_data.solparam.processed_iterations;
                 }
 
                 out << std::endl;
         }
 
         // TODO: use NetCDF?
 }

Here is the call graph for this function:

std::ostream & ModuleNonsmoothNode::Restart ( std::ostream & out ) const

virtual

Implements Elem.

Definition at line 839 of file module-nonsmooth-node.cc.

 {
         return out << "# ModuleNonsmoothNode: not implemented" << std::endl;
 }

void ModuleNonsmoothNode::SetValue	(	DataManager *	pDM,
		VectorHandler &	X,
		VectorHandler &	XP,
		SimulationEntity::Hints *	ph
	)

virtual

Reimplemented from SimulationEntity.

Definition at line 831 of file module-nonsmooth-node.cc.

References NO_OP.

 {
         NO_OP;
 }

void ModuleNonsmoothNode::WorkSpaceDim	(	integer *	piNumRows,
		integer *	piNumCols
	)		const

virtual

Implements Elem.

Definition at line 602 of file module-nonsmooth-node.cc.

References ASSERT, NS_subsys::constraint_type, NS_data, POSITION_AND_VELOCITY, POSITION_ONLY, and VELOCITY_ONLY.

 {
         switch (NS_data.constraint_type) {
         case POSITION_ONLY:
                 *piNumRows = 6;
                 *piNumCols = 6;
                 break;
 
         case VELOCITY_ONLY:
                 *piNumRows = 6;
                 *piNumCols = 6;
                 break;
 
         case POSITION_AND_VELOCITY:
                 *piNumRows = 9;
                 *piNumCols = 9;
                 break;
 
         default:
                 ASSERT(0);
         }
 }