#include <totalequation.h>

Inheritance diagram for TotalEquation:

Collaboration diagram for TotalEquation:

Public Member Functions
	TotalEquation (unsigned int uL, const DofOwner pDO, bool bPos[3], bool bVel[3], TplDriveCaller< Vec3 > const pDCPos[3], bool bRot[3], bool bAgv[3], TplDriveCaller< Vec3 > const pDCRot[3], const StructNode pN1, const Vec3 &f1Tmp, const Mat3x3 &R1hTmp, const Mat3x3 &R1hrTmp, const StructNode *pN2, const Vec3 &f2Tmp, const Mat3x3 &R2hTmp, const Mat3x3 &R2hrTmp, flag fOut)

	~TotalEquation (void)

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

virtual Joint::Type	GetJointType (void) const

virtual unsigned int	iGetNumDof (void) const

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

DofOrder::Order	GetDofType (unsigned int i) const

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

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

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

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

void	WorkSpaceDim (integer piNumRows, integer piNumCols) const

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

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

void	Output (OutputHandler &OH) 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)

virtual unsigned int	iGetNumPrivData (void) const

virtual unsigned int	iGetPrivDataIdx (const char *s) const

virtual doublereal	dGetPrivData (unsigned int i) const

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

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

virtual	~Elem (void)

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 void	BeforePredict (VectorHandler &, VectorHandler &, VectorHandler &, VectorHandler &) const

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

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

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

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 Joint
	Joint (unsigned int uL, const DofOwner *pD, flag fOut)

virtual	~Joint (void)

virtual Elem::Type	GetElemType (void) const

std::ostream &	Output (std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const

virtual void	SetInitialValue (VectorHandler &)

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

bool	bIsPrescribedMotion (void) const

bool	bIsTorque (void) 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

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

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)

Private Attributes
const StructNode *	pNode1

const StructNode *	pNode2

Vec3	f1

Mat3x3	R1h

Mat3x3	R1hr

Vec3	f2

Mat3x3	R2h

Mat3x3	R2hr

bool	bPosActive [3]

bool	bRotActive [3]

bool	bVelActive [3]

bool	bAgvActive [3]

TplDriveOwner< Vec3 >	XDrv

TplDriveOwner< Vec3 >	XPDrv

TplDriveOwner< Vec3 >	XPPDrv

TplDriveOwner< Vec3 >	ThetaDrv

TplDriveOwner< Vec3 >	OmegaDrv

TplDriveOwner< Vec3 >	OmegaPDrv

unsigned int	nConstraints

unsigned int	nPosConstraints

unsigned int	nRotConstraints

unsigned int	nVelConstraints

unsigned int	nAgvConstraints

unsigned int	iPosIncid [3]

unsigned int	iRotIncid [3]

unsigned int	iVelIncid [3]

unsigned int	iAgvIncid [3]

unsigned int	iPosEqIndex [3]

unsigned int	iRotEqIndex [3]

unsigned int	iVelEqIndex [3]

unsigned int	iAgvEqIndex [3]

Vec3	tilde_f1

Vec3	M

Vec3	F

Vec3	ThetaDelta

Vec3	ThetaDeltaPrev

Friends
class	TotalReaction

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 Joint
enum	Type { UNKNOWN = -1, DISTANCE = 0, DISTANCEWITHOFFSET, CLAMP, SPHERICALHINGE, PIN, UNIVERSALHINGE, UNIVERSALROTATION, UNIVERSALPIN, PLANEHINGE, PLANEROTATION, PLANEPIN, AXIALROTATION, PLANEDISP, PLANEDISPPIN, INPLANE, INPLANECONTACT, J_INLINE, ROD, RODBEZIER, DEFORMABLEHINGE, DEFORMABLEDISPJOINT, DEFORMABLEJOINT, DEFORMABLEAXIALJOINT, VISCOUSBODY, LINEARVELOCITY, ANGULARVELOCITY, LINEARACCELERATION, ANGULARACCELERATION, PRISMATIC, DRIVEHINGE, DRIVEDISP, DRIVEDISPPIN, IMPOSEDORIENTATION, IMPOSEDDISP, IMPOSEDDISPPIN, IMPOSEDKINEMATICS, BEAMSLIDER, BRAKE, GIMBAL, POINT_SURFACE_CONTACT, TOTALJOINT, TOTALPINJOINT, TOTALEQUATION, TOTALREACTION, MODAL, SCREWJOINT, LASTJOINTTYPE }

Protected Member Functions inherited from Joint
virtual void	OutputPrepare_int (const std::string &type, OutputHandler &OH, std::string &name)

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 GravityOwner
Gravity *	pGravity

Detailed Description

Definition at line 46 of file totalequation.h.

Constructor & Destructor Documentation

TotalEquation::TotalEquation	(	unsigned int	uL,
		const DofOwner *	pDO,
		bool	bPos[3],
		bool	bVel[3],
		TplDriveCaller< Vec3 > *const	pDCPos[3],
		bool	bRot[3],
		bool	bAgv[3],
		TplDriveCaller< Vec3 > *const	pDCRot[3],
		const StructNode *	pN1,
		const Vec3 &	f1Tmp,
		const Mat3x3 &	R1hTmp,
		const Mat3x3 &	R1hrTmp,
		const StructNode *	pN2,
		const Vec3 &	f2Tmp,
		const Mat3x3 &	R2hTmp,
		const Mat3x3 &	R2hrTmp,
		flag	fOut
	)

Definition at line 44 of file totalequation.cc.

References ASSERT, bAgvActive, bPosActive, bRotActive, bVelActive, iAgvEqIndex, iAgvIncid, iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, iVelEqIndex, iVelIncid, nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

 : Elem(uL, fOut),
 Joint(uL, pDO, fOut),
 pNode1(pN1), pNode2(pN2),
 f1(f1Tmp), R1h(R1hTmp), R1hr(R1hrTmp),
 f2(f2Tmp), R2h(R2hTmp), R2hr(R2hrTmp),
 XDrv(pDCPos[0]), XPDrv(pDCPos[1]), XPPDrv(pDCPos[2]),
 ThetaDrv(pDCRot[0]), OmegaDrv(pDCRot[1]), OmegaPDrv(pDCRot[2]),
 nConstraints(0), nPosConstraints(0), nRotConstraints(0),
 nVelConstraints(0), nAgvConstraints(0),
 tilde_f1(R1h.MulTV(f1)),
 M(Zero3), F(Zero3), ThetaDelta(Zero3), ThetaDeltaPrev(Zero3)
 {
         /* Equations 1->3: Positions
          * Equations 4->6: Rotations */
         
         unsigned int index = 0;
 
         for (unsigned int i = 0; i < 3; i++) {
                 ASSERT(bPos[i] == false || bVel[i] == false);
 
                 bPosActive[i] = bPos[i];
                 bVelActive[i] = bVel[i];
                 
                 if (bPosActive[i]) {
                         iPosIncid[nPosConstraints] = i + 1;
                         iPosEqIndex[nPosConstraints] = index;
                         nPosConstraints++;
                         index++;
                 }
                 
                 if (bVelActive[i]) {
                         iVelIncid[nVelConstraints] = i + 1;
                         iVelEqIndex[nVelConstraints] = index;
                         nVelConstraints++;
                         index++;
                 }
         }       
         
         index = 0;
         for (unsigned int i = 0; i < 3; i++) {
                 ASSERT(bRot[i] == false || bAgv[i] == false);
 
                 bRotActive[i] = bRot[i];
                 bAgvActive[i] = bAgv[i];
                 
                 if (bRotActive[i]) {
                         iRotIncid[nRotConstraints] = i + 1;
                         iRotEqIndex[nRotConstraints] = nPosConstraints + nVelConstraints + index;
                         nRotConstraints++;
                         index++;
                 }
                 
                 if (bAgvActive[i]) {
                         iAgvIncid[nAgvConstraints] = i + 1;
                         iAgvEqIndex[nAgvConstraints] = nPosConstraints + nVelConstraints + index;
                         nAgvConstraints++;
                         index++;
                 }
         }
         nConstraints = nPosConstraints + nRotConstraints + nVelConstraints + nAgvConstraints;
 
 }

TotalEquation::~TotalEquation ( void )

Definition at line 117 of file totalequation.cc.

References NO_OP.

 {
         NO_OP;
 };

Member Function Documentation

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

virtual

Reimplemented from SimulationEntity.

Definition at line 564 of file totalequation.cc.

References ThetaDelta, ThetaDeltaPrev, and Unwrap().

 {
         ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDelta);
 }

Here is the call graph for this function:

void TotalEquation::AfterConvergence	(	const VectorHandler &	X,
		const VectorHandler &	XP,
		const VectorHandler &	XPP
	)

virtual

Reimplemented from SimulationEntity.

Definition at line 571 of file totalequation.cc.

References ThetaDelta, ThetaDeltaPrev, and Unwrap().

 {
         ThetaDeltaPrev = Unwrap(ThetaDeltaPrev, ThetaDelta);
 }

Here is the call graph for this function:

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

virtual

Implements Elem.

Definition at line 637 of file totalequation.cc.

References FullSubMatrixHandler::AddT(), Vec3::Cross(), grad::Cross(), DEBUGCOUT, f2, StructNode::GetRCurr(), StructNode::GetRRef(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), iAgvEqIndex, iAgvIncid, DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, iVelEqIndex, iVelIncid, MatCross, MatCrossCross, nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, nVelConstraints, pNode1, pNode2, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), R1h, R1hr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), FullSubMatrixHandler::SubT(), and WorkSpaceDim().

 {
         /*
          See tecman.pdf for details
          */
          
         DEBUGCOUT("Entering TotalEquation::AssJac()" << std::endl);
 
         FullSubMatrixHandler& WM = WorkMat.SetFull();
 
         /* Ridimensiona la sottomatrice in base alle esigenze */
         integer iNumRows = 0;
         integer iNumCols = 0;
         WorkSpaceDim(&iNumRows, &iNumCols);
         WM.ResizeReset(iNumRows, iNumCols);
 
         /* Recupera gli indici delle varie incognite */
         integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
         integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
         integer iFirstReactionIndex = iGetFirstIndex();
 
         /* Setta gli indici delle equazioni */
         for (int iCnt = 1; iCnt <= 6; iCnt++) {
                 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
                 WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
         }
 
         for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
                 WM.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
         }
 
         /* Recupera i dati che servono */
         Mat3x3 R1(pNode1->GetRRef()*R1h);
         Mat3x3 R1r(pNode1->GetRRef()*R1hr);
         Vec3 b2(pNode2->GetRCurr()*f2);
         Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
 
 /* Phi/q is the only contribution... */
 
         Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
         Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
 
         for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
                 Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
                 Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
                 Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
 
                 /* Constraint, node 1 */
                 WM.SubT(1 + iPosEqIndex[iCnt], 1, vR1);
                 WM.SubT(1 + iPosEqIndex[iCnt], 3 + 1, vb1Cross_R1);
 
                 /* Constraint, node 2 */
                 WM.AddT(1 + iPosEqIndex[iCnt], 6 + 1, vR1);
                 WM.AddT(1 + iPosEqIndex[iCnt], 9 + 1, vb2Cross_R1);
         }
         
         for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
                 Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
 
                 /* Constraint, node 1 */
                 WM.SubT(1 + iRotEqIndex[iCnt], 3 + 1, vR1);
 
                 /* Constraint, node 2 */
                 WM.AddT(1 + iRotEqIndex[iCnt], 9 + 1, vR1);
         }
 
         if (nVelConstraints > 0) {
                 Mat3x3 Omega1Cross_R1(pNode1->GetWCurr().Cross(R1));
                 Mat3x3  Tmp12 = (
                                 Mat3x3(MatCrossCross, pNode1->GetWCurr(), -b1)
                                 + Mat3x3(MatCrossCross, b2, pNode1->GetWCurr())
                                 - Mat3x3(MatCross, pNode2->GetVCurr())
                                 - Mat3x3(MatCrossCross, pNode2->GetWCurr(), b2)
                                 + Mat3x3(MatCross, pNode1->GetVCurr())
                                 ) * R1;
                 Mat3x3 Tmp22(MatCrossCross, pNode2->GetWCurr(), b2);
 
                 for (unsigned iCnt = 0 ; iCnt < nVelConstraints; iCnt++) {
                         Vec3 vR1(R1.GetVec(iVelIncid[iCnt]));
                         Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iVelIncid[iCnt]));
                         Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iVelIncid[iCnt]));
 
                         Vec3 vOmega1Cross_R1(Omega1Cross_R1.GetVec(iVelIncid[iCnt]));
                         Vec3 vTmp12(Tmp12.GetVec(iVelIncid[iCnt]));     
                         Vec3 vTmp22(Tmp22.GetVec(iVelIncid[iCnt]));     
                         
                         /* Constraint, node 1 */
                         /* The same as in position constraint*/
                         WM.SubT(1 + iVelEqIndex[iCnt], 1, vR1);                         // delta_v1
                         WM.SubT(1 + iVelEqIndex[iCnt], 3 + 1, vb1Cross_R1);             // delta_W1
                         /* New contributions, related to delta_x1 and delta_g1 */
                         WM.SubT(1 + iVelEqIndex[iCnt], 1, vOmega1Cross_R1 * dCoef);     // delta_x1
                         WM.AddT(1 + iVelEqIndex[iCnt], 3 + 1, vTmp12 * dCoef);          // delta_g1
                         
                         /* Constraint, node 2 */
                         /* The same as in position constraint*/
                         WM.AddT(1 + iVelEqIndex[iCnt], 6 + 1, vR1);                     // delta_v2
                         WM.AddT(1 + iVelEqIndex[iCnt], 9 + 1, vb2Cross_R1);             // delta_W2
                         /* New contributions, related to delta_x1 and delta_g1 */
                         WM.AddT(1 + iVelEqIndex[iCnt], 6 + 1, vOmega1Cross_R1 * dCoef); // delta_x2
                         WM.AddT(1 + iVelEqIndex[iCnt], 9 + 1, vTmp22 * dCoef);          // delta_g2
                 }
         }
         
         if(nAgvConstraints > 0) {
                 Mat3x3 DeltaWCross_R1((pNode1->GetWCurr() - pNode2->GetWCurr()).Cross(R1r));
                 for (unsigned iCnt = 0 ; iCnt < nAgvConstraints; iCnt++) {
                         Vec3 vR1(R1r.GetVec(iAgvIncid[iCnt]));
                         Vec3 vDeltaWCross_R1(DeltaWCross_R1.GetVec(iAgvIncid[iCnt]));
 
                         /* Constraint, node 1 */
                         WM.SubT(1 + iAgvEqIndex[iCnt], 3 + 1, vR1);     // delta_W1
                         /* New contribution, related to delta_g1 */
                         WM.SubT(1 + iAgvEqIndex[iCnt], 3 + 1, vDeltaWCross_R1 * dCoef); // delta_g1
         
                         /* Constraint, node 2 */
                         WM.AddT(1 + iAgvEqIndex[iCnt], 9 + 1, vR1);// delta_W2
                 }
         }
 
         return WorkMat;
 }

Here is the call graph for this function:

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

virtual

Implements Elem.

Definition at line 765 of file totalequation.cc.

References ASSERT, Vec3::Cross(), DEBUGCOUT, f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), iAgvEqIndex, iAgvIncid, DofOwnerOwner::iGetFirstIndex(), iPosEqIndex, iPosIncid, iRotEqIndex, iRotIncid, iVelEqIndex, iVelIncid, Mat3x3::MulTM(), Mat3x3::MulTV(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, nVelConstraints, pNode1, pNode2, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), R1h, R1hr, R2hr, VectorHandler::ResizeReset(), RotManip::Rot(), ThetaDelta, ThetaDrv, tilde_f1, RotManip::VecRot(), WorkSpaceDim(), and XDrv.

 {
         DEBUGCOUT("Entering TotalEquation::AssRes()" << std::endl);
 
         /* Dimensiona e resetta la matrice di lavoro */
         integer iNumRows = 0;
         integer iNumCols = 0;
         WorkSpaceDim(&iNumRows, &iNumCols);
         WorkVec.ResizeReset(iNumRows);
 
         /* Indici */
         integer iFirstReactionIndex = iGetFirstIndex();
 
         /* Indici del vincolo */
         for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++) {
                 WorkVec.PutRowIndex(iCnt, iFirstReactionIndex + iCnt);
         }
 
         Vec3 b2(pNode2->GetRCurr()*f2);
         Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
 
         Mat3x3 R1 = pNode1->GetRCurr()*R1h;
         Mat3x3 R1r = pNode1->GetRCurr()*R1hr;
         Mat3x3 R2r = pNode2->GetRCurr()*R2hr;
 
         Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
         Vec3 VDelta = R1.MulTV(
                         pNode2->GetVCurr()
                         - b2.Cross(pNode2->GetWCurr())
                         - pNode1->GetVCurr()
                         + b1.Cross(pNode1->GetWCurr())
                         ) 
                         - XDrv.Get();   
                         
         Mat3x3 R0T = RotManip::Rot(-ThetaDrv.Get());    // -Theta0 to get R0 transposed
         Mat3x3 RDelta = R1r.MulTM(R2r*R0T);
         ThetaDelta = RotManip::VecRot(RDelta);
         
         Vec3 WDelta = R1r.MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()) - ThetaDrv.Get();
 
         /* Constraint equations are divided by dCoef
          * ONLY if the constraint is on position */
         ASSERT(dCoef != 0.);
 
         /* Position constraint:  */
         for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
                 WorkVec.PutCoef(1 + iPosEqIndex[iCnt],
                         -(XDelta(iPosIncid[iCnt])/dCoef));
         }
 
         /* Rotation constraints: */
         for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
                 WorkVec.PutCoef(1 + iRotEqIndex[iCnt],
                         -(ThetaDelta(iRotIncid[iCnt])/dCoef));
         }
 
         /* Linear Velocity Constraint */
         for (unsigned iCnt = 0; iCnt < nVelConstraints; iCnt++) {
                 WorkVec.PutCoef(1 + iVelEqIndex[iCnt],
                         -(VDelta(iVelIncid[iCnt])));
         }
                 
         /* Angular Velocity Constraint */
         for (unsigned iCnt = 0; iCnt < nAgvConstraints; iCnt++) {
                 WorkVec.PutCoef(1 + iAgvEqIndex[iCnt],
                         -(WDelta(iAgvIncid[iCnt])));
         }
 
         return WorkVec;
 }

Here is the call graph for this function:

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

virtual

Reimplemented from Elem.

Definition at line 124 of file totalequation.cc.

References bAgvActive, bPosActive, bRotActive, bVelActive, idx2xyz, DofOwnerOwner::iGetFirstIndex(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

 {
         integer iIndex = iGetFirstIndex();
 
         if (nPosConstraints > 0 || nVelConstraints > 0) {
                 out << prefix << iIndex + 1;
                 if (nPosConstraints + nVelConstraints > 1) {
                         out << "->" << iIndex + nPosConstraints + nVelConstraints;
                 }
                 out << ": ";
                 out << "reaction force(s) [";
 
                 for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                         if (bPosActive[i] || bVelActive[i]) {
                                 cnt++;
                                 if (cnt > 1) {
                                         out << ",";
                                 }
                                 out << "F" << idx2xyz[i];
                         }
                 }
                 out << "]" << std::endl;
         }
 
         if (nRotConstraints > 0 || nAgvConstraints > 0) {
                 out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
                 if (nRotConstraints + nAgvConstraints > 1) {
                         out << "->" << iIndex + nConstraints ;
                 }
                 out << ": ";
                 out << "reaction couple(s) [";
 
                 for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                         if (bRotActive[i] || bAgvActive[i]) {
                                 cnt++;
                                 if (cnt > 1) {
                                         out << ",";
                                 }
                                 out << "m" << idx2xyz[i];
                         }
                 }
                 out << "]" << std::endl;
         }
 
         if (bInitial) {
                 iIndex += nConstraints;
 
                 if (nPosConstraints > 0) {
                         out << prefix << iIndex + 1;
                         if (nPosConstraints > 1) {
                                 out << "->" << iIndex + nPosConstraints;
                         }
                         out << ": ";
                         out << "reaction force(s) derivative(s) [";
 
                         for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                                 if (bPosActive[i]) {
                                         cnt++;
                                         if (cnt > 1) {
                                                 out << ",";
                                         }
                                         out << "FP" << idx2xyz[i];
                                 }
                         }
                         out << "]" << std::endl;
                 }
 
                 if (nRotConstraints > 0) {
                         out << prefix << iIndex + nPosConstraints + 1;
                         if (nRotConstraints > 1) {
                                 out << "->" << iIndex + nConstraints;
                         }
                         out << ": ";
                         out << "reaction couple(s) derivative(s) [";
 
                         for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                                 if (bRotActive[i]) {
                                         cnt++;
                                         if (cnt > 1) {
                                                 out << ",";
                                         }
                                         out << "mP" << idx2xyz[i];
                                 }
                         }
                         out << "]" << std::endl;
                 }
         }
 
         return out;
 }

Here is the call graph for this function:

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

virtual

Reimplemented from Elem.

Definition at line 217 of file totalequation.cc.

References WithLabel::GetLabel(), iGetInitialNumDof(), and iGetNumDof().

 {
         // FIXME: todo
         int ndof = 1;
         if (i == -1) {
                 if (bInitial) {
                         ndof = iGetNumDof();
 
                 } else {
                         ndof = iGetInitialNumDof();
                 }
         }
         desc.resize(ndof);
 
         std::ostringstream os;
         os << "TotalEquation(" << GetLabel() << ")";
 
         if (i == -1) {
                 std::string name(os.str());
 
                 for (i = 0; i < ndof; i++) {
                         os.str(name);
                         os.seekp(0, std::ios_base::end);
                         os << ": dof(" << i + 1 << ")";
                         desc[i] = os.str();
                 }
 
         } else {
                 os << ": dof(" << i + 1 << ")";
                 desc[0] = os.str();
         }
 }

Here is the call graph for this function:

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

virtual

Reimplemented from Elem.

Definition at line 253 of file totalequation.cc.

References bAgvActive, bPosActive, bRotActive, bVelActive, idx2xyz, DofOwnerOwner::iGetFirstIndex(), nAgvConstraints, nConstraints, nPosConstraints, nRotConstraints, and nVelConstraints.

 {
         integer iIndex = iGetFirstIndex();
 
         if (nPosConstraints > 0 || nVelConstraints > 0) {
                 out << prefix << iIndex + 1;
                 if (nPosConstraints + nVelConstraints > 1) {
                         out << "->" << iIndex + nPosConstraints + nVelConstraints;
                 }
                 out << ": ";
                 out << "position/velocity constraint(s) [";
         
                 for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                         if (bPosActive[i]) {
                                 cnt++;
                                 if (cnt > 1) {
                                         out << ",";
                                 }
                                 out << "P" << idx2xyz[i] << "1=P" << idx2xyz[i] << "2";
                         } else if (bVelActive[i]) {
                                 cnt++;
                                 if (cnt > 1) {
                                         out << ",";
                                 }
                                 out << "V" << idx2xyz[i] << "1=V" << idx2xyz[i] << "2";
                         }
                 }
         
                 out << "]" << std::endl;
         }
 
         if (nRotConstraints > 0 || nAgvConstraints > 0) {
                 out << prefix << iIndex + nPosConstraints + nVelConstraints + 1;
                 if (nRotConstraints + nAgvConstraints > 1) {
                         out << "->" << iIndex + nConstraints ;
                 }
                 out << ": ";
                 out << "orientation/angular velocity constraint(s) [";
         
                 for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                         if (bRotActive[i]) {
                                 cnt++;
                                 if (cnt > 1) {
                                         out << ",";
                                 }
                                 out << "g" << idx2xyz[i] << "1=g" << idx2xyz[i] << "2";
                         } else if (bAgvActive[i]) {
                                 cnt++;
                                 if (cnt > 1) {
                                         out << ",";
                                 }
                                 out << "W" << idx2xyz[i] << "1=W" << idx2xyz[i] << "2";
                         }
                 }
 
                 out << "]" << std::endl;
         }
 
         if (bInitial) {
                 iIndex += nConstraints;
 
                 if (nPosConstraints > 0) {
                         out << prefix << iIndex + 1;
                         if (nPosConstraints > 1) {
                                 out << "->" << iIndex + nPosConstraints;
                         }
                         out << ": ";
                         out << "velocity constraint(s) [";
                 
                         for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                                 if (bPosActive[i]) {
                                         cnt++;
                                         if (cnt > 1) {
                                                 out << ",";
                                         }
                                         out << "v" << idx2xyz[i] << "1=v" << idx2xyz[i] << "2";
                                 }
                         }
 
                         out << "]" << std::endl;
                 }
                 
                 if (nRotConstraints > 0) {
                         out << prefix << iIndex + nPosConstraints + 1;
                         if (nRotConstraints > 1) {
                                 out << "->" << iIndex + nConstraints ;
                         }
                         out << ": ";
                         out << "angular velocity constraint(s) [";
                 
                         for (unsigned int i = 0, cnt = 0; i < 3; i++) {
                                 if (bRotActive[i]) {
                                         cnt++;
                                         if (cnt > 1) {
                                                 out << ",";
                                         }
                                         out << "w" << idx2xyz[i] << "1=w" << idx2xyz[i] << "2";
                                 }
                         }
 
                         out << "]" << std::endl;
                 }       
         }
 
         return out;
 }

Here is the call graph for this function:

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

virtual

Reimplemented from Elem.

Definition at line 362 of file totalequation.cc.

References WithLabel::GetLabel(), iGetInitialNumDof(), and iGetNumDof().

 {
         // FIXME: todo
         int ndof = 1;
         if (i == -1) {
                 if (bInitial) {
                         ndof = iGetNumDof();
 
                 } else {
                         ndof = iGetInitialNumDof();
                 }
         }
         desc.resize(ndof);
 
         std::ostringstream os;
         os << "TotalEquation(" << GetLabel() << ")";
 
         if (i == -1) {
                 std::string name(os.str());
 
                 for (i = 0; i < ndof; i++) {
                         os.str(name);
                         os.seekp(0, std::ios_base::end);
                         os << ": equation(" << i + 1 << ")";
                         desc[i] = os.str();
                 }
 
         } else {
                 os << ": equation(" << i + 1 << ")";
                 desc[0] = os.str();
         }
 }

Here is the call graph for this function:

doublereal TotalEquation::dGetPrivData ( unsigned int i ) const

virtual

Reimplemented from SimulationEntity.

Definition at line 1147 of file totalequation.cc.

References ASSERT, bPosActive, bRotActive, Vec3::Cross(), f1, f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), Mat3x3::GetVec(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), Mat3x3::MulTV(), pNode1, pNode2, R1h, R1hr, ThetaDelta, ThetaDeltaPrev, ThetaDrv, Unwrap(), and XDrv.

 {
         switch (i) {
         case 1:
         case 2:
         case 3: {
                 Vec3 x(pNode1->GetRCurr().MulTV(
                         pNode2->GetXCurr() + pNode2->GetRCurr()*f2
                                 - pNode1->GetXCurr()) - f1);
                         return R1h.GetVec(i)*x;
                 }
 
         case 4:
         case 5:
         case 6: {
                 Vec3 Theta(Unwrap(ThetaDeltaPrev, ThetaDelta) + ThetaDrv.Get());
                 return Theta(i - 3);
                 }
 
         case 7:
         case 8:
         case 9:
                 return 0.;
 
         case 10:
         case 11:
         case 12:
                 return 0.;
 
         case 13:
         case 14:
         case 15:
                 if (!bPosActive[i - 13]) {
                         return 0.;
                 }
                 return XDrv.Get()(i - 12);
 
         case 16:
         case 17:
         case 18:
                 if (!bRotActive[i - 16]) {
                         return 0.;
                 }
                 return ThetaDrv.Get()(i - 15);
         case 19:
         case 20:
         case 21:
                 // FIXME: blind fix
                 {
                 Vec3 v(pNode1->GetRCurr().MulTV(
                         (pNode2->GetVCurr() + pNode2->GetWCurr().Cross(pNode2->GetRCurr()*f2)
                                 - pNode1->GetVCurr()) - 
                         pNode1->GetWCurr().Cross(pNode2->GetXCurr() + 
                                 pNode2->GetRCurr()*f2
                                 - pNode1->GetXCurr() - f1) 
                         )
                 );
                 
                         return R1h.GetVec(i-18)*v;
                 }
         case 22:
         case 23:
         case 24:
                 {
                 Vec3 W(pNode1->GetRCurr().MulTV(pNode2->GetWCurr() - pNode1->GetWCurr()))
                 ;
                 
                         return R1hr.GetVec(i-21)*W;
                 }
 
 
 
         default:
                 ASSERT(0);
         }
 
         return 0.;
 }

Here is the call graph for this function:

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

inlinevirtual

Reimplemented from Elem.

Definition at line 236 of file totalequation.h.

References pNode1, and pNode2.

                                                                          {
                 connectedNodes.resize(2);
                 connectedNodes[0] = pNode1;
                 connectedNodes[1] = pNode2;
         };

DofOrder::Order TotalEquation::GetDofType ( unsigned int i ) const

inlinevirtual

Reimplemented from Elem.

Definition at line 147 of file totalequation.h.

References DofOrder::ALGEBRAIC, ASSERT, and nConstraints.

                                          {
                 ASSERT(i >= 0 && i < nConstraints);
                 return DofOrder::ALGEBRAIC;
         };

virtual Joint::Type TotalEquation::GetJointType ( void ) const

inlinevirtual

Implements Joint.

Definition at line 117 of file totalequation.h.

References Joint::TOTALEQUATION.

                                  {
                 return Joint::TOTALEQUATION;
         };

virtual unsigned int TotalEquation::iGetInitialNumDof ( void ) const

inlinevirtual

Implements SubjectToInitialAssembly.

Definition at line 209 of file totalequation.h.

References nConstraints.

Referenced by DescribeDof(), and DescribeEq().

                                       {
                 return  2*nConstraints;
         };

virtual unsigned int TotalEquation::iGetNumDof ( void ) const

inlinevirtual

Reimplemented from Elem.

Definition at line 122 of file totalequation.h.

References nConstraints.

Referenced by DescribeDof(), and DescribeEq().

                                {
                 return nConstraints;
         };

unsigned int TotalEquation::iGetNumPrivData ( void ) const

virtual

Reimplemented from SimulationEntity.

Definition at line 1075 of file totalequation.cc.

 {
         return 24;
 }

unsigned int TotalEquation::iGetPrivDataIdx ( const char * s ) const

virtual

Reimplemented from SimulationEntity.

Definition at line 1081 of file totalequation.cc.

References ASSERT.

 {
         ASSERT(s != NULL);
 
         unsigned int off = 0;
 
         switch (s[0]) {
         case 'p':
                 /* relative position */
                 break;
 
         case 'r':
                 /* relative orientation */
                 off += 3;
                 break;
 
         case 'F':
                 /* force */
                 off += 6;
                 break;
 
         case 'M':
                 /* moment */
                 off += 9;
                 break;
 
         case 'd':
                 /* imposed relative position */
                 off += 12;
                 break;
 
         case 't':
                 /* imposed relative orientation */
                 off += 15;
                 break;
         
         case 'v':
                 /* relative linear velocity */
                 off += 18;
                 break;
 
         case 'w':
                 /* relative angular velocity */
                 off += 21;
                 break;
 
 
         default:
                 return 0;
         }
 
         switch (s[1]) {
         case 'x':
                 return off + 1;
 
         case 'y':
                 return off + 2;
 
         case 'z':
                 return off + 3;
         }
 
         return 0;
 }

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

virtual

Implements SubjectToInitialAssembly.

Definition at line 873 of file totalequation.cc.

References FullSubMatrixHandler::AddT(), Vec3::Cross(), f2, StructNode::GetRCurr(), StructNode::GetRRef(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), InitialWorkSpaceDim(), iPosIncid, iRotIncid, nConstraints, nPosConstraints, nRotConstraints, pNode1, pNode2, FullSubMatrixHandler::PutColIndex(), FullSubMatrixHandler::PutRowIndex(), R1h, R1hr, FullSubMatrixHandler::ResizeReset(), VariableSubMatrixHandler::SetFull(), and FullSubMatrixHandler::SubT().

 {
 
         /* Per ora usa la matrice piena; eventualmente si puo'
          * passare a quella sparsa quando si ottimizza */
         FullSubMatrixHandler& WM = WorkMat.SetFull();
 
         /* Dimensiona e resetta la matrice di lavoro */
         integer iNumRows = 0;
         integer iNumCols = 0;
         InitialWorkSpaceDim(&iNumRows, &iNumCols);
         WM.ResizeReset(iNumRows, iNumCols);
 
         /* Equazioni: vedi joints.dvi */
 
         /*       equazioni ed incognite
          * F1                                     Delta_x1      1          
          * M1                                     Delta_g1      3 + 1 
          * FP1                                    Delta_xP1     6 + 1
          * MP1                                    Delta_w1      9 + 1 
          * F2                                     Delta_x2      12 + 1
          * M2                                     Delta_g2      15 + 1
          * FP2                                    Delta_xP2     18 + 1  
          * MP2                                    Delta_w2      21 + 1 
          * vincolo spostamento                    Delta_F       24 + 1 
          * vincolo rotazione                      Delta_M       24 + nPosConstraints  
          * derivata vincolo spostamento           Delta_FP      24 + nConstraints  
          * derivata vincolo rotazione             Delta_MP      24 + nConstraints + nPosConstraints  
          */
         
         
         /* Indici */
         integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
         integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
         integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
         integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
         integer iFirstReactionIndex = iGetFirstIndex();
         integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
 
 
         /* Setta gli indici dei nodi */
         for (int iCnt = 1; iCnt <= 6; iCnt++) {
                 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
                 WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
                 WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
                 WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
         }
 
         /* Setta gli indici delle reazioni */
         for (unsigned int iCnt = 1; iCnt <=  nConstraints; iCnt++) {
                 WM.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
                 WM.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
         }
         
         /* Recupera i dati che servono */
         Mat3x3 R1(pNode1->GetRRef()*R1h);
         Mat3x3 R1r(pNode1->GetRRef()*R1hr);
         
         Vec3 b2(pNode2->GetRCurr()*f2);
         Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
         
         Vec3 Omega1(pNode1->GetWCurr());
         Vec3 Omega2(pNode2->GetWCurr());
         
         Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
         
         /* Constraints: Add only active rows/columns*/  
         
         /* Positions contribution:*/
 
         Mat3x3 b1Cross_R1(b1.Cross(R1)); // = [ b1 x ] * R1
         Mat3x3 b2Cross_R1(b2.Cross(R1)); // = [ b2 x ] * R1
 
         for (unsigned iCnt = 0 ; iCnt < nPosConstraints; iCnt++) {
                 Vec3 vR1(R1.GetVec(iPosIncid[iCnt]));
                 Vec3 vb1Cross_R1(b1Cross_R1.GetVec(iPosIncid[iCnt]));
                 Vec3 vb2Cross_R1(b2Cross_R1.GetVec(iPosIncid[iCnt]));
 
                 /* Constraint, node 1 */
                 WM.SubT(24 + 1 + iCnt, 1, vR1); // * Delta_x1
                 WM.SubT(24 + 1 + iCnt, 3 + 1, vb1Cross_R1);     // * Delta_g1
 
                 /* d/dt(Constraint), node 1 */
                 WM.SubT(24 + 1 + nConstraints + iCnt, 6 + 1, vR1);      // * Delta_xP1
                 WM.SubT(24 + 1 + nConstraints + iCnt, 9 + 1, vb1Cross_R1);      // * Delta_W1
                 
                 /* Constraint, node 2 */
                 WM.AddT(24 + 1 + iCnt, 12 + 1, vR1);    // * Delta_x2
                 WM.AddT(24 + 1 + iCnt, 15 + 1, vb2Cross_R1);    // * Delta_g2
         
                 /* d/dt(Constraint), node 2 */
                 WM.AddT(24 + 1 + nConstraints +  iCnt, 18 + 1, vR1);    // * Delta_xP2
                 WM.AddT(24 + 1 + nConstraints +  iCnt, 21 + 1, vb2Cross_R1);    // * Delta_W2
         }
 
         for (unsigned iCnt = 0 ; iCnt < nRotConstraints; iCnt++) {
                 Vec3 vR1(R1r.GetVec(iRotIncid[iCnt]));
 
                 /* Constraint, node 1 */
                 WM.SubT(24 + 1 + nPosConstraints + iCnt, 3 + 1, vR1);   // * Delta_g1
 
                 /* d/dt(Constraint), node 1 */
                 WM.SubT(24 + 1 + nConstraints + nPosConstraints + iCnt, 9 + 1, vR1);    // * Delta_W1
 
                 /* Constraint, node 2 */
                 WM.AddT(24 + 1 + nPosConstraints +  iCnt, 15 + 1, vR1); // * Delta_g2
 
                 /* d/dt(Constraint), node 2 */
                 WM.AddT(24 + 1 + nConstraints + nPosConstraints + iCnt, 21 + 1, vR1);   // * Delta_W2
         }
 
         return WorkMat;
 }

Here is the call graph for this function:

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

virtual

Implements SubjectToInitialAssembly.

Definition at line 991 of file totalequation.cc.

References TplDriveOwner< T >::bIsDifferentiable(), Vec3::Cross(), DEBUGCOUT, f2, TplDriveOwner< T >::Get(), TplDriveOwner< T >::GetP(), StructNode::GetRCurr(), StructNode::GetRRef(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), DofOwnerOwner::iGetFirstIndex(), StructDispNode::iGetFirstPositionIndex(), InitialWorkSpaceDim(), iPosIncid, iRotIncid, Mat3x3::MulTM(), nConstraints, nPosConstraints, nRotConstraints, pNode1, pNode2, VectorHandler::PutCoef(), SubVectorHandler::PutRowIndex(), R1h, R1hr, R2hr, VectorHandler::ResizeReset(), RotManip::Rot(), ThetaDelta, ThetaDrv, tilde_f1, RotManip::VecRot(), and XDrv.

 {
 
         DEBUGCOUT("Entering TotalEquation::InitialAssRes()" << std::endl);
 
         /* Dimensiona e resetta la matrice di lavoro */
         integer iNumRows = 0;
         integer iNumCols = 0;
         InitialWorkSpaceDim(&iNumRows, &iNumCols);
         WorkVec.ResizeReset(iNumRows);
 
         /* Indici */
         integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
         integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
         integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
         integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
         integer iFirstReactionIndex = iGetFirstIndex();
         integer iReactionPrimeIndex = iFirstReactionIndex + nConstraints;
 
         /* Setta gli indici */
         for (int iCnt = 1; iCnt <= 6; iCnt++) {
                 WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
                 WorkVec.PutRowIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
                 WorkVec.PutRowIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
                 WorkVec.PutRowIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
         }
 
         for (unsigned int iCnt = 1; iCnt <= nConstraints; iCnt++)       {
                 WorkVec.PutRowIndex(24 + iCnt, iFirstReactionIndex + iCnt);
                 WorkVec.PutRowIndex(24 + nConstraints + iCnt, iReactionPrimeIndex + iCnt);
         }
 
         /* Recupera i dati */
         /* Recupera i dati che servono */
         Mat3x3 R1(pNode1->GetRRef()*R1h);
         Mat3x3 R1r(pNode1->GetRRef()*R1hr);
         Mat3x3 R2r(pNode2->GetRCurr()*R2hr);
         
         Vec3 b2(pNode2->GetRCurr()*f2);
         Vec3 b1(pNode2->GetXCurr() + b2 - pNode1->GetXCurr());
         
         Vec3 Omega1(pNode1->GetWCurr());
         Vec3 Omega2(pNode2->GetWCurr());
         
         Vec3 b1Prime(pNode2->GetVCurr() + Omega2.Cross(b2) - pNode1->GetVCurr());
         
 
         /* Constraint Equations */
         
         Vec3 XDelta = R1.MulTV(b1) - tilde_f1 - XDrv.Get();
         Vec3 XDeltaPrime = R1.MulTV(b1Prime + b1.Cross(Omega1));
         
         if(XDrv.bIsDifferentiable())    {
                 XDeltaPrime -= XDrv.GetP();
         }
         
         Mat3x3 R0T = RotManip::Rot(-ThetaDrv.Get());    // -Theta0 to get R0 transposed
         Mat3x3 RDelta = R1r.MulTM(R2r*R0T);
         ThetaDelta = RotManip::VecRot(RDelta);
         Vec3 ThetaDeltaPrime = R1r.MulTV(Omega2 - Omega1);
 
         if(ThetaDrv.bIsDifferentiable())        {
                 ThetaDeltaPrime -= RDelta*ThetaDrv.GetP();
         }
 
 
         /* Position constraint:  */
         for (unsigned iCnt = 0; iCnt < nPosConstraints; iCnt++) {
                 WorkVec.PutCoef(24 + 1 + iCnt, -XDelta(iPosIncid[iCnt]));
                 WorkVec.PutCoef(24 + 1 + nConstraints + iCnt, -XDeltaPrime(iPosIncid[iCnt]));
         }
 
         /* Rotation constraints: */
         for (unsigned iCnt = 0; iCnt < nRotConstraints; iCnt++) {
                 WorkVec.PutCoef(24 + 1 + nPosConstraints + iCnt, -ThetaDelta(iRotIncid[iCnt]));
                 WorkVec.PutCoef(24 + 1 + nPosConstraints + nConstraints +  iCnt, -ThetaDeltaPrime(iRotIncid[iCnt]));
         }
 
 return WorkVec;
 }

Here is the call graph for this function:

virtual void TotalEquation::InitialWorkSpaceDim	(	integer *	piNumRows,
		integer *	piNumCols
	)		const

inlinevirtual

Implements SubjectToInitialAssembly.

Definition at line 213 of file totalequation.h.

References nConstraints.

Referenced by InitialAssJac(), and InitialAssRes().

                                                                           {
                 *piNumCols = *piNumRows = 24 + 2*nConstraints;
         };

void TotalEquation::Output ( OutputHandler & OH ) const

virtual

Reimplemented from ToBeOutput.

Definition at line 840 of file totalequation.cc.

References ToBeOutput::bToBeOutput(), f2, WithLabel::GetLabel(), StructNode::GetRCurr(), StructDispNode::GetVCurr(), StructNode::GetWCurr(), StructDispNode::GetXCurr(), OutputHandler::Joints(), Mat3x3::MulTM(), Mat3x3::MulTV(), Joint::Output(), pNode1, pNode2, R1h, R1hr, R2hr, tilde_f1, V1, V2, RotManip::VecRot(), and Zero3.

 {
         if (bToBeOutput()) {
                 const Vec3& X1(pNode1->GetXCurr());
                 const Vec3& X2(pNode2->GetXCurr());
                 const Mat3x3& R1(pNode1->GetRCurr());
                 const Mat3x3& R2(pNode2->GetRCurr());
                 const Vec3& V1(pNode1->GetVCurr());
                 const Vec3& V2(pNode2->GetVCurr());
                 const Vec3& Omega1(pNode1->GetWCurr());
                 const Vec3& Omega2(pNode2->GetWCurr());
 
                 Mat3x3 R1Tmp(R1*R1h);
                 Mat3x3 R1rTmp(R1*R1hr);
                 Mat3x3 R2rTmp(R2*R2hr);
                 Mat3x3 RTmp(R1rTmp.MulTM(R2rTmp));
                 Vec3 b2(R2*f2);
                 Vec3 b1(X2 + b2 - X1);
                 Vec3 X(R1Tmp.MulTV(b1) - tilde_f1);
 
                 Joint::Output(OH.Joints(), "TotalEquation", GetLabel(),
                         Zero3, Zero3, Zero3, Zero3)
                         << " " << X
                         << " " << RotManip::VecRot(RTmp)
                         << " " << R1Tmp.MulTV(V2 + Omega2.Cross(b2) - V1 - Omega1.Cross(b1))
                         << " " << R1rTmp.MulTV(Omega2 - Omega1)
                         << std::endl;
                         // accelerations?
         }
 }

Here is the call graph for this function:

Hint * TotalEquation::ParseHint	(	DataManager *	pDM,
		const char *	s
	)		const

virtual

Reimplemented from SimulationEntity.

Definition at line 487 of file totalequation.cc.

References STRLENOF.

 {
         if (strncasecmp(s, "offset{" /*}*/ , STRLENOF("offset{" /*}*/ )) == 0)
         {
                 s += STRLENOF("offset{" /*}*/ );
 
                 if (strcmp(&s[1], /*{*/ "}") != 0) {
                         return 0;
                 }
 
                 switch (s[0]) {
                 case '1':
                         return new Joint::OffsetHint<1>;
 
                 case '2':
                         return new Joint::OffsetHint<2>;
                 }
 
         } else if (strncasecmp(s, "position-hinge{" /*}*/, STRLENOF("position-hinge{" /*}*/)) == 0) {
                 s += STRLENOF("position-hinge{" /*}*/);
 
                 if (strcmp(&s[1], /*{*/ "}") != 0) {
                         return 0;
                 }
 
                 switch (s[0]) {
                 case '1':
                         return new Joint::HingeHint<1>;
 
                 case '2':
                         return new Joint::HingeHint<2>;
                 }
 
         } else if (strncasecmp(s, "position-drive3{" /*}*/, STRLENOF("position-drive3{" /*}*/)) == 0) {
                 s += STRLENOF("position-");
 
                 Hint *pH = ::ParseHint(pDM, s);
                 if (pH) {
                         TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
                         if (pTDH) {
                                 return new PositionDriveHint<Vec3>(pTDH);
                         }
                 }
                 return 0;
 
         } else if (strncasecmp(s, "orientation-hinge{" /*}*/, STRLENOF("orientation-hinge{" /*}*/)) == 0) {
                 s += STRLENOF("orientation-hinge{" /*}*/);
 
                 if (strcmp(&s[1], /*{*/ "}") != 0) {
                         return 0;
                 }
 
                 switch (s[0]) {
                 case '1':
                         return new Joint::HingeHint<1>;
 
                 case '2':
                         return new Joint::HingeHint<2>;
                 }
 
         } else if (strncasecmp(s, "orientation-drive3{" /*}*/, STRLENOF("orientation-drive3{" /*}*/)) == 0) {
                 s += STRLENOF("orientation-");
 
                 Hint *pH = ::ParseHint(pDM, s);
                 if (pH) {
                         TplDriveHint<Vec3> *pTDH = dynamic_cast<TplDriveHint<Vec3> *>(pH);
                         if (pTDH) {
                                 return new OrientationDriveHint<Vec3>(pTDH);
                         }
                 }
                 return 0;
         }
 
         return 0;
 }

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

virtual

Implements Elem.

Definition at line 580 of file totalequation.cc.

References bPosActive, bRotActive, f1, f2, WithLabel::GetLabel(), Mat3x3::GetVec(), pNode1, pNode2, R1h, R1hr, R2h, R2hr, Joint::Restart(), and Vec3::Write().

 {
         Joint::Restart(out) << ", total equation, "
                 << pNode1->GetLabel() << ", "
                         << "position, ";
                         f1.Write(out, ", ") << ", "
                         << "position orientation, "
                                 "1 , "; 
                                 R1h.GetVec(1).Write(out, ", ") << ", "
                                 "2 , "; 
                                 R1h.GetVec(2).Write(out, ", ") << ", "
                         << "rotation orientation, "
                                 "1 , "; 
                                 R1hr.GetVec(1).Write(out, ", ") << ", "
                                 "2 , ";
                                 R1hr.GetVec(2).Write(out, ", ") << ", "
                 << pNode2->GetLabel() << ", "
                         << "position, "; 
                         f2.Write(out, ", ") << ", "
                         << "position orientation, "
                                 "1 , ";
                                 R2h.GetVec(1).Write(out, ", ") << ", "
                                 "2 , ";
                                 R2h.GetVec(2).Write(out, ", ") << ", "
                         << "rotation orientation, "
                                 "1 , ";
                                 R2hr.GetVec(1).Write(out, ", ") << ", "
                                 "2 , ";
                                 R2hr.GetVec(2).Write(out, ", ");
 
         if (bPosActive[0] || bPosActive[1] || bPosActive[2]) {
                 out << ", position constraint";
                 for (unsigned i = 0; i < 3; i++) {
                         if (bPosActive[i]) {
                                 out << ", active";
                         } else {
                                 out << ", inactive";
                         }
                 }
         }
 
         if (bRotActive[0] || bRotActive[1] || bRotActive[2]) {
                 out << ", orientation constraint";
                 for (unsigned i = 0; i < 3; i++) {
                         if (bRotActive[i]) {
                                 out << ", active";
                         } else {
                                 out << ", inactive";
                         }
                 }
         }
 
         return out << std::endl;
 }

Here is the call graph for this function:

void TotalEquation::SetValue	(	DataManager *	pDM,
		VectorHandler &	X,
		VectorHandler &	XP,
		SimulationEntity::Hints *	ph = `0`
	)

virtual

Reimplemented from Joint.

Definition at line 397 of file totalequation.cc.

References f1, f2, TplDriveOwner< T >::Get(), StructNode::GetRCurr(), StructDispNode::GetXCurr(), MBDYN_EXCEPT_ARGS, Mat3x3::MulTM(), Mat3x3::MulTV(), OmegaDrv, OmegaPDrv, pNode1, pNode2, Joint::JointDriveHint< T >::pTDH, R1h, R1hr, R2h, R2hr, TplDriveOwner< T >::Set(), ThetaDrv, tilde_f1, WithLabel::uLabel, XDrv, XPDrv, and XPPDrv.

 {
         if (ph) {
                 for (unsigned int i = 0; i < ph->size(); i++) {
                         Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
 
                         if (pjh) {
 
                                 if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
                                         const Mat3x3& R1(pNode1->GetRCurr());
                                         Vec3 fTmp2(pNode2->GetRCurr()*f2);
 
                                         f1 = R1.MulTV(pNode2->GetXCurr() + fTmp2 - pNode1->GetXCurr());
                                         tilde_f1 = R1h.MulTV(f1) - XDrv.Get();
 
                                 } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
                                         const Mat3x3& R2(pNode2->GetRCurr());
                                         Mat3x3 R1(pNode1->GetRCurr()*R1h);
                                         Vec3 fTmp1(pNode1->GetRCurr()*f1);
 
                                         f2 = R2.MulTV(pNode1->GetXCurr() + fTmp1 - pNode2->GetXCurr() + R1*XDrv.Get());
 
                                 } else if (dynamic_cast<Joint::HingeHint<1> *>(pjh)) {
                                         if (dynamic_cast<Joint::PositionHingeHint<1> *>(pjh)) {
                                                 const Mat3x3& R1(pNode1->GetRCurr());
                                                 R1h = R1.MulTM(pNode2->GetRCurr()*R2h);
 
                                         } else if (dynamic_cast<Joint::OrientationHingeHint<1> *>(pjh)) {
                                                 const Mat3x3& R1(pNode1->GetRCurr());
                                                 R1hr = R1.MulTM(pNode2->GetRCurr()*R2hr);
                                         }
 
                                 } else if (dynamic_cast<Joint::HingeHint<2> *>(pjh)) {
                                         if (dynamic_cast<Joint::PositionHingeHint<2> *>(pjh)) {
                                                 const Mat3x3& R2(pNode2->GetRCurr());
                                                 R2h = R2.MulTM(pNode1->GetRCurr()*R1h);
 
                                         } else if (dynamic_cast<Joint::OrientationHingeHint<2> *>(pjh)) {
                                                 const Mat3x3& R2(pNode2->GetRCurr());
                                                 R2hr = R2.MulTM(pNode1->GetRCurr()*R1hr);
                                         }
 
                                 } else if (dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh)) {
                                         Joint::JointDriveHint<Vec3> *pjdh
                                                 = dynamic_cast<Joint::JointDriveHint<Vec3> *>(pjh);
                                         pedantic_cout("TotalEquation(" << uLabel << "): "
                                                 "creating drive from hint[" << i << "]..." << std::endl);
 
                                         TplDriveCaller<Vec3> *pDC = pjdh->pTDH->pCreateDrive(pDM);
                                         if (pDC == 0) {
                                                 silent_cerr("TotalEquation(" << uLabel << "): "
                                                         "unable to create drive "
                                                         "after hint #" << i << std::endl);
                                                 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
                                         }
 
                                         if (dynamic_cast<Joint::PositionDriveHint<Vec3> *>(pjdh)) {
                                                 XDrv.Set(pDC);
 
                                         } else if (dynamic_cast<Joint::VelocityDriveHint<Vec3> *>(pjdh)) {
                                                 XPDrv.Set(pDC);
 
                                         } else if (dynamic_cast<Joint::AccelerationDriveHint<Vec3> *>(pjdh)) {
                                                 XPPDrv.Set(pDC);
 
                                         } else if (dynamic_cast<Joint::OrientationDriveHint<Vec3> *>(pjdh)) {
                                                 ThetaDrv.Set(pDC);
 
                                         } else if (dynamic_cast<Joint::AngularVelocityDriveHint<Vec3> *>(pjdh)) {
                                                 OmegaDrv.Set(pDC);
 
                                         } else if (dynamic_cast<Joint::AngularAccelerationDriveHint<Vec3> *>(pjdh)) {
                                                 OmegaPDrv.Set(pDC);
 
                                         } else {
                                                 delete pDC;
                                         }
 
                                 } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
                                         /* TODO */
                                 }
                                 continue;
                         }
                 }
         }
 }

Here is the call graph for this function:

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

inlinevirtual

Implements Elem.

Definition at line 170 of file totalequation.h.

References nConstraints.

Referenced by AssJac(), and AssRes().

                                                                    {
                 *piNumRows = nConstraints ;
                 *piNumCols = 12;
         };