link.c File Reference

Implementaton of the function operating on Tlink. More...

#include "link.h"
#include "cpolyhedron.h"
#include "varnames.h"
#include <math.h>
#include <string.h>

Functions
void	InitLink (char name, Tlink l)
	Constructor.
void	CopyLink (Tlink l_dst, Tlink l_src)
	Copy constructor.
void	AddBody2Link (Tcpolyhedron b, Tlink l)
	Adds a body to the link.
void	ChangeLinkReferenceFrame (double p1, double p2, Tlink *l)
	Sets a new reference frame for the link.
unsigned int	LinkNBodies (Tlink *l)
	Gets the number of convex parts of the link.
Tcpolyhedron *	GetLinkBody (unsigned int i, Tlink *l)
	Gets one of the convex parts of the link.
char *	GetLinkName (Tlink *l)
	Gets the name of a link.
void	GenerateLinkRot (Tparameters p, unsigned int lID, TCuikSystem cs, Tlink *l)
	Generate the variables and constraints related with the rotation of the link reference frame.
void	ApplyLinkRot (double sf, unsigned int sv, double p, Tequation eq, TCuikSystem cs, boolean groundLink, Tlink l)
	Transforms a vector from the local reference frame to the global one and accumulates the resulting expression into the given equations.
void	RegenerateLinkSolution (TCuikSystem cs, double sol, boolean groundLink, Tlink *l)
	Computes the values for the dummy variables used to represent the rotation matrices for a given link.
void	GetTransform2Link (TCuikSystem cs, double sol, boolean groundLink, double r, THTransform t, Tlink *l)
	Gets the homogeneous transform associated with a link for a given solution point.
double	GetLinkMaxCoordinate (Tlink *l)
	Returns the sum of the maximum coordinate value for all the convex polyhedrons in the link.
void	PlotLink (Tplot3d pt, double axesLength, Tlink l)
	Adds a link to a 3d scene.
void	MoveLink (Tplot3d pt, TCuikSystem cs, double sol, double r, Tlink *l)
	Displaces a link in a 3d scene.
void	DeleteLink (Tlink *l)
	Destructor.

Detailed Description

Implementaton of the function operating on Tlink.

See also:: Tlink, link.h.

Definition in file link.c.

Function Documentation

void InitLink	(	char *	name,
		Tlink *	l
	)

Initializes a link structure with no bodies.

Parameters:

	name	The name to give to the link.
	l	The links to initialize.

Definition at line 16 of file link.c.

References Tlink::axisID, Tlink::bodies, Tlink::c, CopyVoidPtr(), DeleteVoidPtr(), HTransformIdentity(), INIT_NUM_SHAPES, InitVector(), Tlink::maxCoord, Tlink::name, NEW, NO_UINT, Tlink::R, and Tlink::s.

Here is the call graph for this function:

void CopyLink	(	Tlink *	l_dst,
		Tlink *	l_src
	)

Initializes a link copying the information from another link.

Parameters:

	l_dst	The link to initialize.
	l_src	The link from where to copy.

Definition at line 42 of file link.c.

References AddBody2Link(), Tlink::axisID, Tlink::bodies, Tlink::c, CopyVoidPtr(), DeleteVoidPtr(), GetLinkBody(), HTransformCopy(), INIT_NUM_SHAPES, InitVector(), LinkNBodies(), Tlink::maxCoord, Tlink::name, NEW, Tlink::R, and Tlink::s.

Referenced by AddLink2Mechanism().

Here is the call graph for this function:

Here is the caller graph for this function:

void AddBody2Link	(	Tcpolyhedron *	b,
		Tlink *	l
	)

Adds a body to the link.

Parameters:

	b	The convex polyhedron to add to the link
	l	The link where to add the new polyhedron.

Definition at line 70 of file link.c.

References Tlink::bodies, CopyCPolyhedron(), GetCPolyhedronMaxCoordinate(), Tlink::maxCoord, NEW, and NewVectorElement().

Referenced by AddBody2Mechanism(), and CopyLink().

Here is the call graph for this function:

Here is the caller graph for this function:

void ChangeLinkReferenceFrame	(	double **	p1,
		double **	p2,
		Tlink *	l
	)

In order to get the simplest possible set of solutions we can select a reference frame for the link different from that used to defined the bodies. This internal reference frame is defined from two (not necessarily orthogonal) vectors. Therefore, we store the sin and cos between the two vectors defining the internal reference frame and the rotation from the frame where bodies are defined and the internal reference frame.

This function does not change the points defining the bodies. (they remain in the original frame), but it changes the way they are interpreted changing the workings of functions GenerateLinkRot and ApplyLinkRot.

If the two given vectors are (almost) aligned, the internal reference frame is not changed.

When using a rotation representation based on quaternions it is not possible to change the internal link reference frame.

Parameters:

	p1	Two 3d points defining the first vector for the new internal reference frame.
	p2	Two 3d points defining the second vector for the new internal reference frame.
	l	The link whose reference frame we want to change.

Definition at line 82 of file link.c.

References Tlink::c, HTransformSetElement(), Tlink::R, and Tlink::s.

Referenced by InitWorldKinCS().

Here is the call graph for this function:

Here is the caller graph for this function:

unsigned int LinkNBodies ( Tlink * l )

Gets the number of convex parts of the link.

Parameters:

l

The link to query.

Returns:: The number of convex parts of the link.

Definition at line 148 of file link.c.

References Tlink::bodies, and VectorSize().

Referenced by AddLink2World(), CopyLink(), DeleteLink(), MoveLink(), and PlotLink().

Here is the call graph for this function:

Here is the caller graph for this function:

Tcpolyhedron* GetLinkBody	(	unsigned int	i,
		Tlink *	l
	)

Gets one of the convex parts of the link.

Parameters:

	i	The identifier of the convex part to retrive. The identifier of a part is given by the order at which they are added to the link.
	l	The link to query.

Returns:: A pointer to the requested convex part or NULL if the link does not have any part with the given identifier.

Definition at line 153 of file link.c.

References Tlink::bodies, and GetVectorElement().

Referenced by CopyLink(), DeleteLink(), GetMechanismDefiningPoint(), MoveLink(), and PlotLink().

Here is the call graph for this function:

Here is the caller graph for this function:

char* GetLinkName ( Tlink * l )

Gets the name of a link.

Parameters:

l

The link to query.

Returns:: A pointer to a string with the name of the link.

Definition at line 164 of file link.c.

References Tlink::name.

Referenced by GenerateEquationsFromBranch(), GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetLinkID(), and RegenerateJointSolution().

Here is the caller graph for this function:

void GenerateLinkRot	(	Tparameters *	p,
		unsigned int	lID,
		TCuikSystem *	cs,
		Tlink *	l
	)

Each link has a 3D reference frame formed by three vectors (in general orthonormal vectors unless ChangeLinkReferenceFrame is used). This functions add the variales representing the rotation of this reference frame with respect to the gobal reference frame. The translation from this global reference frame to the origin of the frame attached to the link is given by equations involving only rotation variables (and fixed vectors) and can only be computed at the level where we have together links (mechanisms) and cuiksystems, that is, at the world.h level.

Parameters:

	p	A set of parameters. They include, for instance the dummification level.
	lID	The identifier of the link in the mechanism. Used to generate unique names for the new variables.
	cs	The cuiksystem where to add the variables and equations.
	l	The link from where to generate the variables and equation.

Definition at line 170 of file link.c.

References AddEquation2CS(), AddVariable2CS(), Tlink::c, DeleteEquation(), DeleteVariable(), GenerateCrossProductEquations(), GenerateDotProductEquation(), GenerateNormEquation(), GetCSVariableID(), IsGroundLink, LINK_ROT, Tlink::name, NEW, NewInterval(), NewVariable(), NO_UINT, ROT_REDUNDANCY, Tlink::s, SetVariableInterval(), and SYSTEM_VAR.

Referenced by InitWorldKinCS().

Here is the call graph for this function:

Here is the caller graph for this function:

void ApplyLinkRot	(	double	sf,
		unsigned int	sv,
		double *	p,
		Tequation *	eq,
		TCuikSystem *	cs,
		boolean	groundLink,
		Tlink *	l
	)

Apply the changes of basis given by the link reference frame to a vector. This results in an expression that transforms the vector from the local reference frame to the global one. These expressions are added to the given equations (one expression for X, another for Y, and another for Z).

Parameters:

	sf	Constant scale factor to apply to the input vector.
	sv	Variable scale factor given as the identifier of the range in the given box whose center have to be used as scale factor. If no variable scale factor is to be used this parameter should be NO_UINT.
	p	The 3D vector in the link reference frame that we want to transform to the global one and to add to trans.
	eq	Set of 3 equations where to add the 3 expressions resulting from this function (one for the X component of the vector in the globla frame, another for the Y expression and, finally the expression for the Z component).
	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers. The numerical identifier of the variable is used to generate a unique name for it.
	groundLink	TRUE if the given link is the ground link.
	l	The link to use to rotate the given input vector p.

Definition at line 285 of file link.c.

References AddCt2Monomial(), AddMonomial(), AddVariable2Monomial(), DeleteMonomial(), Error(), GetCSVariableID(), HTransformApply(), InitMonomial(), LINK_ROT, Tlink::name, NEW, NO_UINT, Tlink::R, ResetMonomial(), and ZERO.

Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), and GenerateJointRangeEquations().

Here is the call graph for this function:

Here is the caller graph for this function:

void RegenerateLinkSolution	(	TCuikSystem *	cs,
		double *	sol,
		boolean	groundLink,
		Tlink *	l
	)

Solution points only include values for the system (and secondary) variables. However, in some formulations, the frame of reference for each link is represented using not only system variables, but dummy variable too. This function computes the values for the link-related dummy variables form the system ones for a given link.

Parameters:

	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers, i.e., the number of range of the box to use to instantiate each variable.
	sol	The solution point with the values for the system variables. At the end of the function, this vector also has values for the link-related dummy variables.
	groundLink	TRUE if the given link is the ground link. The rotation matrix for the ground link is fixed (the identity) and, its rotation matrix generates no variables (nor system nor dummy).
	l	The link.

Definition at line 351 of file link.c.

Referenced by RegenerateMechanismSolution().

Here is the caller graph for this function:

void GetTransform2Link	(	TCuikSystem *	cs,
		double *	sol,
		boolean	groundLink,
		double *	r,
		THTransform *	t,
		Tlink *	l
	)

Returns the homogeneous transform with the position and orientation of the link for a given solution point.

Parameters:

	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers, i.e., the number of range of the box to use to instantiate each variable.
	sol	The solution point.
	groundLink	TRUE if the given link is the ground link.
	r	3d vector with the translation from the ground link to this link. The variables for these translations are removed from the cuiksystems and computed from rotations and fixed vectors. Consequently they can only be computed at the level where we have cuiksystems and links (i.e. in world.h)
	t	The returned homogeneous transform.
	l	The link whose reference frame we want to retrive.

Definition at line 358 of file link.c.

References AXIS_H, Error(), GetCSVariableID(), HTransformIdentity(), HTransformOrthonormalize(), HTransformProduct(), HTransformSetElement(), LINK_ROT, Tlink::name, NEW, NO_UINT, and Tlink::R.

Referenced by MoveJoint(), and MoveLink().

Here is the call graph for this function:

Here is the caller graph for this function:

double GetLinkMaxCoordinate ( Tlink * l )

Returns the sum of the maximum coordinate value (either for X,Y or Z) for all the convex polyhedrons in the link. This is used in higher levels to define an over-estimate bounding-box of the world. This bounding box is used to define the ranges for some of the coordinate and separating planes variables.

Parameters:

l

The link to query.

Returns:: The sum of the maximum coordinate values for all bodies in the link.

Definition at line 975 of file link.c.

References Tlink::maxCoord.

Referenced by AddBody2Mechanism(), and AddLink2Mechanism().

Here is the caller graph for this function:

void PlotLink	(	Tplot3d *	pt,
		double	axesLength,
		Tlink *	l
	)

Adds a link to a 3d scene. It adds the diffent parts of the link as if they are expressed in the global frame. To displace the use MoveLink.

Parameters:

	pt	The 3d geometry where to add the link.
	axesLength	>0 to display the link axes. The value is the length of the lines representing the axes. The reference axes for the link are represeted as red for X, green for Y and blue for Z.
	l	The link to add to the scene.

Definition at line 980 of file link.c.

References Tlink::axisID, Close3dObject(), DeleteColor(), GetLinkBody(), LinkNBodies(), NewColor(), NO_UINT, PlotCPolyhedron(), PlotVect3d(), and StartNew3dObject().

Referenced by PlotMechanism().

Here is the call graph for this function:

Here is the caller graph for this function:

void MoveLink	(	Tplot3d *	pt,
		TCuikSystem *	cs,
		double *	sol,
		double *	r,
		Tlink *	l
	)

Displaces a link previously added to a 3d scene.

The parameters for the rotation to apply are taken from the central points of interval of the the given box corresponding to the variables expressing the reference frame for the link.
The parameters for the translation are externally given.

This function triggers an error (actually the error is triggered in GetTransform2Link) if applied to the groundlink. Note that the groundLink is not suposed to move.

Parameters:

	pt	The 3d scene where the apply the displacement.
	cs	The cuik system on which the boxes are defiend. It is used to, given the name of the variables corresponding the reference frame of the given link, retrive its numerical identifiers, i.e., the number of range of the box to use to instantiate each variable.
	sol	The solution point. Only values for system variables are used.
	r	A 3d vector with the translation from the ground link to the reference frame of the link.
	l	The link to move.