Introduction

Definition of the basic lineal algebra routines used in the CuikSuite.

These basic routines are implemented using BLAS, if available.

Functions
void	ClearVector (unsigned int n, double *v)
	Set a vector to zero. More...

void	VectorAnd (unsigned int s, boolean v1, boolean v2, boolean *vo)
	And of two vectors. More...

void	VectorOr (unsigned int s, boolean v1, boolean v2, boolean *vo)
	Or of two vectors. More...

CBLAS_INLINE double	GeneralDotProduct (unsigned int s, double v1, double v2)
	Computes the dot product of two general vectors. More...

CBLAS_INLINE double	MatrixWeigthedDotProduct (unsigned int s, double W, double v1, double *v2)
	Computes the weighted dot product of two general vectors. More...

CBLAS_INLINE void	ScaleVector (double f, unsigned int s, double *v)
	Scales a vector. More...

CBLAS_INLINE void	ScaleVector2 (double f, unsigned int s, double v, double vout)
	Scales a vector. More...

CBLAS_INLINE void	OffsetVector (double o, unsigned int s, double *v)
	Offsets a vector. More...

CBLAS_INLINE void	AccumulateVector (unsigned int s, double v1, double v2)
	Adds a vector to another vectors. More...

CBLAS_INLINE void	SumVector (unsigned int s, double v1, double v2, double *v)
	Adds two vectors. More...

CBLAS_INLINE void	SumVectorScale (unsigned int s, double v1, double w, double v2, double *v)
	Adds two vectors with a scale. More...

void	CosVector (unsigned int s, double v, double co)
	Cosine on a vector. More...

void	SinVector (unsigned int s, double v, double si)
	Sine on a vector. More...

double	AddVectorElements (unsigned int m, double *v)
	Adds the elements of a vector. More...

double	AddVectorPositiveElements (unsigned int m, double *v)
	Adds the positive elements of a vector. More...

unsigned int	MaxVectorElement (unsigned int m, double *v)
	Index of the maximum element of a vector. More...

double	MaxVector (unsigned int m, double *v)
	Value of the maximum element of a vector. More...

unsigned int	MinVectorElement (unsigned int m, double *v)
	Index of the minimum element of a vector. More...

double	MinVector (unsigned int m, double *v)
	Value of the minimum element of a vector. More...

void	Vector2Range (double l, double u, double mode, unsigned int m, double *v)
	Scales a vector to a given range. More...

CBLAS_INLINE void	SubtractVector (unsigned int s, double v1, double v2)
	Subtracts a vector from another vector. More...

void	DifferenceVector (unsigned int s, double v1, double v2, double *v)
	Subtracts two vectors. More...

void	DifferenceVectorSubset (unsigned int s, boolean subset, double v1, double v2, double v)
	Subtracts two vectors along some of its components. More...

void	DifferenceVectorTopology (unsigned int s, unsigned int tp, double v1, double v2, double v)
	Subtracts two vectors. More...

void	DifferenceVectorTopologySubset (unsigned int s, unsigned int tp, boolean subset, double v1, double v2, double *v)
	Subtracts two vectors along some of its components. More...

CBLAS_INLINE double	Norm (unsigned int s, double *v)
	Computes the norm of a vector. More...

CBLAS_INLINE double	WeightedNorm (unsigned int s, double w, double v)
	Computes the weighted norm of a vector. More...

CBLAS_INLINE double	MatrixWeightedNorm (unsigned int s, double W, double v)
	Computes the weighted norm of a vector. More...

CBLAS_INLINE double	NormWithStride (unsigned int s, unsigned int st, double *v)
	Computes the norm of a vector. More...

double	SquaredDistance (unsigned int s, double v1, double v2)
	Computes the squared distance of two points. More...

double	WeightedSquaredDistance (unsigned int s, double w, double v1, double *v2)
	Computes the weighted squared distance of two points. More...

double	MatrixWeightedSquaredDistance (unsigned int s, double W, double v1, double *v2)
	Computes the weighted squared distance of two points. More...

double	SquaredDistanceSubset (unsigned int s, boolean subset, double v1, double *v2)
	Computes the squared distance of two points along selected dimensions. More...

double	WeightedSquaredDistanceSubset (unsigned int s, boolean subset, double w, double v1, double v2)
	Computes the weighted squared distance of two points along selected dimensions. More...

double	MatrixWeightedSquaredDistanceSubset (unsigned int s, boolean subset, double W, double v1, double v2)
	Computes the weighted squared distance of two points along selected dimensions. More...

double	Distance (unsigned int s, double v1, double v2)
	Computes the distance of two points. More...

double	WeightedDistance (unsigned int s, double w, double v1, double *v2)
	Computes the weighted distance of two points. More...

double	MatrixWeightedDistance (unsigned int s, double W, double v1, double *v2)
	Computes the weighted distance of two points. More...

double	SquaredDistanceTopology (unsigned int s, unsigned int tp, double v1, double *v2)
	Computes the squared distance of two points. More...

double	WeightedSquaredDistanceTopology (unsigned int s, unsigned int tp, double w, double v1, double v2)
	Computes the squared distance of two points. More...

double	SquaredDistanceTopologyMin (double t2, unsigned int s, unsigned int tp, double v1, double *v2)
	Computes the squared distance of two points. More...

double	WeightedSquaredDistanceTopologyMin (double t2, unsigned int s, unsigned int tp, double w, double v1, double v2)
	Computes the squared distance of two points. More...

double	DistanceTopology (unsigned int s, unsigned int tp, double v1, double *v2)
	Computes the distance of two points. More...

double	WeightedDistanceTopology (unsigned int s, unsigned int tp, double w, double v1, double v2)
	Computes the distance of two points. More...

double	SquaredDistanceTopologySubset (unsigned int s, unsigned int tp, boolean subset, double v1, double v2)
	Computes the square distance of two points along a subset of components. More...

double	WeightedSquaredDistanceTopologySubset (unsigned int s, unsigned int tp, boolean subset, double w, double v1, double *v2)
	Computes the square distance of two points along a subset of components. More...

double	DistanceTopologySubset (unsigned int s, unsigned int tp, boolean subset, double v1, double v2)
	Computes the distance of two points along a subset of components. More...

double	WeightedDistanceTopologySubset (unsigned int s, unsigned int tp, boolean subset, double w, double v1, double *v2)
	Computes the distance of two points along a subset of components. More...

double	DistanceTopologyMin (double t, unsigned int s, unsigned int tp, double v1, double *v2)
	Computes the distance of two points, if it is below a given threshold. More...

double	WeightedDistanceTopologyMin (double t, unsigned int s, unsigned int tp, double w, double v1, double v2)
	Computes the weighted distance of two points, if it is below a given threshold. More...

boolean	CrossTopologyBorder (unsigned int s, unsigned int tp, double v1, double *v2)
	Determines if the line between two points crosses the topology boder. More...

CBLAS_INLINE void	Normalize (unsigned int s, double *v)
	Normalizes a vector. More...

double	Mean (unsigned int s, double *v)
	Computes the mean. More...

double	StdDev (unsigned int s, double m, double *v)
	Computes the standard deviation. More...

void	ArrayPi2Pi (unsigned int n, unsigned int t, double a)
	Applies PI2PI to an array. More...

CBLAS_INLINE void	GetRow (unsigned int k, unsigned int r, unsigned int c, double m, double v)
	Gets a row from a matrix. More...

CBLAS_INLINE void	GetColumn (unsigned int k, unsigned int r, unsigned int c, double m, double v)
	Gets a column from a matrix. More...

CBLAS_INLINE void	SetRow (double v, unsigned int k, unsigned int r, unsigned int c, double m)
	Sets a row of a matrix. More...

CBLAS_INLINE void	SetColumn (double v, unsigned int k, unsigned int r, unsigned int c, double m)
	Sets a column of a matrix. More...

CBLAS_INLINE void	ScaleRow (unsigned int k, double sc, unsigned int r, unsigned int c, double *m)
	Scales a row of a matrix. More...

CBLAS_INLINE void	ScaledAddRows (unsigned int r, unsigned int c, double m, unsigned int n, unsigned int sr, double sc, unsigned int sv, double v)
	Defines a vector as a scaled sum of rows of a matrix. More...

CBLAS_INLINE void	ScaleColumn (unsigned int k, double sc, unsigned int r, unsigned int c, double *m)
	Scales a column of a matrix. More...

CBLAS_INLINE double	RowsDotProduct (unsigned int i, unsigned int j, unsigned int r, unsigned int c, double *m)
	Dot product between two rows of a matrix. More...

CBLAS_INLINE double	ColumnsDotProduct (unsigned int i, unsigned int j, unsigned int r, unsigned int c, double *m)
	Dot product between two columns of a matrix. More...

CBLAS_INLINE double	RowSquaredNorm (unsigned int k, unsigned int r, unsigned int c, double *m)
	Computes the squared norm of a row of a matrix. More...

CBLAS_INLINE double	ColumnSquaredNorm (unsigned int k, unsigned int r, unsigned int c, double *m)
	Computes the squared norm of a column of a matrix. More...

CBLAS_INLINE void	SumRowsScale (unsigned int i, double sc, unsigned int j, unsigned int r, unsigned int c, double *m)
	Accumulates in a given row another row, with a scale factor. More...

CBLAS_INLINE void	SumColumnsScale (unsigned int i, double sc, unsigned int j, unsigned int r, unsigned int c, double *m)
	Accumulates in a given column another column, with a scale factor. More...

CBLAS_INLINE void	CopyRow (unsigned int i, unsigned int j, unsigned int r, unsigned int c, double *m)
	Copies a row into another row. More...

CBLAS_INLINE void	CopyColumn (unsigned int i, unsigned int j, unsigned int r, unsigned int c, double *m)
	Copies a column into another column. More...

CBLAS_INLINE void	IdentityMatrix (unsigned int n, double *M)
	Initializes an identiy matrix. More...

CBLAS_INLINE void	DiagonalMatrix (unsigned int n, double v, double M)
	Initializes a diagonal matrix. More...

void	SymmetrizeMatrix (unsigned int n, double *M)
	Symmetrizes a matrix. More...

CBLAS_INLINE void	MatrixVectorProduct (unsigned int r, unsigned int c, double A, double b, double *o)
	Product of a matrix and a vector. More...

CBLAS_INLINE void	MatrixVectorProductAccum (unsigned int r, unsigned int c, double A, double b, double *o)
	Accumulate the product of a matrix and a vector. More...

CBLAS_INLINE void	TMatrixVectorProduct (unsigned int r, unsigned int c, double A, double b, double *o)
	Product of a transposed matrix and a vector. More...

CBLAS_INLINE void	TMatrixVectorProductAccum (unsigned int r, unsigned int c, double A, double b, double *o)
	Accumulates the product of a transposed matrix and a vector. More...

CBLAS_INLINE void	TMatrixVectorStrideProduct (unsigned int r, unsigned int c, double A, unsigned int s, double b, double *o)
	Product of a transposed matrix and a vector. More...

CBLAS_INLINE void	AccumulateMatrix (unsigned int nr, unsigned int nc, double B, double A)
	A=A+B. More...

CBLAS_INLINE void	AccumulateTMatrix (unsigned int nr, double B, double A)
	A=A+B^t. More...

CBLAS_INLINE void	SubtractMatrix (unsigned int nr, unsigned int nc, double A, double B)
	A=A-B. More...

CBLAS_INLINE void	MatrixMatrixProduct (unsigned int ra, unsigned int ca, double A, unsigned int cb, double B, double *C)
	C = A * B. More...

CBLAS_INLINE void	MatrixMatrixProductAccum (unsigned int ra, unsigned int ca, double A, unsigned int cb, double B, double *C)
	C = C + A * B. More...

CBLAS_INLINE void	MatrixTMatrixProduct (unsigned int ra, unsigned int ca, double A, unsigned int rb, double B, double *C)
	C = A * B^t. More...

CBLAS_INLINE void	TMatrixMatrixProduct (unsigned int ra, unsigned int ca, double A, unsigned int cb, double B, double *C)
	C = A^t * B. More...

double	MinCosinusBetweenSubSpaces (unsigned int m, unsigned int k, double T1, double T2)
	Computes the cosinus of the maximum angle between two lineal sub-spaces. More...

unsigned int	OrthonormalizeRows (unsigned int r, unsigned int c, double *m)
	Orthonormalize the rows of a matrix. More...

unsigned int	OrthonormalizeColumns (unsigned int r, unsigned int c, double *m)
	Orthonormalize the columns of a matrix. More...

void	ProjectToColumnSubSpace (unsigned int rs, unsigned int cs, double ms, unsigned int c, double m)
	Projects to the subpace spanned by the columns of a matrix. More...

CBLAS_INLINE void	RankOneUpdate (double sc, unsigned int nr, double vr, unsigned int nc, double vc, double *m)
	Rank-one update of a matrix. More...

CBLAS_INLINE void	SubMatrixFromMatrix (unsigned int nr1, unsigned int nc1, double m1, unsigned int nri, unsigned int nci, unsigned int nr, unsigned int nc, double m)
	Defines a submatrix in a matrix. More...

CBLAS_INLINE void	AddSubMatrix (double sc, unsigned int nr1, unsigned int nc1, double m1, unsigned int nri, unsigned int nci, unsigned int nr, unsigned int nc, double m)
	Accumulates a submatrix. More...

CBLAS_INLINE void	SubMatrixFromSubMatrix (unsigned int nrs, unsigned int ncs, unsigned int nrc, unsigned int ncc, unsigned int nr1, unsigned int nc1, double m1, unsigned int nri, unsigned int nci, boolean trans, unsigned int nr, unsigned int nc, double m)
	Defines a submatrix in a matrix. More...

CBLAS_INLINE void	MatrixFromSubMatrix (unsigned int r_src, unsigned int c_src, double m_src, unsigned int rs, unsigned int cs, unsigned int r_dst, unsigned int c_dst, double m_dst)
	Initializes a matrix with a block from another matrix. More...

CBLAS_INLINE void	SubMatrixFromTMatrix (unsigned int nr1, unsigned int nc1, double m1, unsigned int nri, unsigned int nci, unsigned int nr, unsigned int nc, double m)
	Defines a submatrix in a matrix. More...

CBLAS_INLINE void	SelectMatrixColumns (boolean sc, unsigned int nr, unsigned int nc, double m)
	Selects some of the columns of a matrix. More...

CBLAS_INLINE void	SelectMatrixRows (boolean sr, unsigned int nr, unsigned int nc, double m)
	Selects some of the rows of a matrix. More...

void	PrintVector (FILE f, char label, unsigned int n, double *v)
	Prints a vector. More...

void	SaveVector (FILE f, unsigned int n, double v)
	Saves a vector. More...

void	PrintMatrix (FILE f, char label, unsigned int r, unsigned int c, double *m)
	Prints a matrix. More...

void	PrintTMatrix (FILE f, char label, unsigned int r, unsigned int c, double *m)
	Prints a transposed matrix. More...

Function Documentation

◆ ClearVector()

void ClearVector	(	unsigned int	n,
		double *	v
	)

inline

Sets a vector to 0.

Parameters

n	The size of the vector.
v	The vector.

Definition at line 16 of file basic_algebra.c.

◆ VectorAnd()

void VectorAnd	(	unsigned int	s,
		boolean *	v1,
		boolean *	v2,
		boolean *	vo
	)

Generates a vector with the 'and' of two vectors of flags.

Parameters

s	Size of the vectors.
v1	The first vector to operate.
v2	The second vector to operate.
vo	The output vector (and of v1, v2).

Definition at line 21 of file basic_algebra.c.

Referenced by DynamicStepResidue(), and GetPositionJacobian().

◆ VectorOr()

void VectorOr	(	unsigned int	s,
		boolean *	v1,
		boolean *	v2,
		boolean *	vo
	)

Generates a vector with the 'or' of two vectors of flags.

Parameters

s	Size of the vectors.
v1	The first vector to operate.
v2	The second vector to operate.
vo	The output vector (or of v1, v2).

Definition at line 29 of file basic_algebra.c.

◆ GeneralDotProduct()

CBLAS_INLINE double GeneralDotProduct	(	unsigned int	s,
		double *	v1,
		double *	v2
	)

Computes the dot product of two general vectors.

Parameters

s	The dimensionality of the vectors.
v1	First vector.
v2	Second vector.

Returns: The dot produt.

Definition at line 37 of file basic_algebra.c.

Referenced by AtlasRRTValidateSample(), BroydenUpdateJacobian(), CutPolytopeWithFace(), DetermineSPolytopeNeighbour(), EnlargeSPolytope(), GradientSmooth(), InsidePolytope(), InsideSPolytope(), LQRComputePolicy(), LQRComputePolicy_t(), MatrixWeigthedDotProduct(), MechEnergy(), PolytopeBoundaryPointFromExternalCornerInt(), RefineSingularPoint(), SAVxSAV(), StepEffort(), StepEffortGradient(), and Time2Go().

◆ MatrixWeigthedDotProduct()

CBLAS_INLINE double MatrixWeigthedDotProduct	(	unsigned int	s,
		double *	W,
		double *	v1,
		double *	v2
	)

Computes weighted the dot product of two general vectors.

Parameters

s	The dimensionality of the vectors.
W	The matrix of weights.
v1	First vector.
v2	Second vector.

Returns: The dot produt.

Definition at line 52 of file basic_algebra.c.

References GeneralDotProduct(), and RC2INDEX.

◆ ScaleVector()

CBLAS_INLINE void ScaleVector	(	double	f,
		unsigned int	s,
		double *	v
	)

Multiplies all the elements of a vector by a given factor.

Parameters

f	The factor.
s	Number of elements in the vector.
v	The vector.

Definition at line 72 of file basic_algebra.c.

Referenced by AddBranchToRRT(), CDCallBackInfo(), ComputeAcceleration(), ComputeC(), ConnectDynamicStatesID(), ConnectSamplesChart(), DetermineSPolytopeNeighbour(), DistanceOnChart(), EvaluateMEquationXVectors(), ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), FocusedPointOnBoundary(), GetConnectionLinkWrench(), GradientSmooth(), HandC(), InitHandC(), LinearizeDynamics(), MechEnergy(), MinimizeOnAtlas(), NewChartFromPoint(), PathInChart(), PlotForceField(), PolytopeBoundaryPointFromExternalCornerInt(), rbi2mci(), scaleSAM(), scaleSAV(), StepDispersionGradient(), StepEffort(), StepEffortGradient(), StepLengthGradient(), and TriangulateAtlas().

◆ ScaleVector2()

CBLAS_INLINE void ScaleVector2	(	double	f,
		unsigned int	s,
		double *	v,
		double *	vout
	)

Multiplies all the elements of a vector by a given factor and stores the result in a new vector.

Parameters

f	The factor.
s	Number of elements in the vector.
v	The vector.
vout	The output vector.

Definition at line 84 of file basic_algebra.c.

Referenced by EvaluateTrans().

◆ OffsetVector()

CBLAS_INLINE void OffsetVector	(	double	o,
		unsigned int	s,
		double *	v
	)

Adds a constant to all the elements of a vector.

Parameters

o	The constant (or offset) to add.
s	The size of the vector.
v	The vector.

Definition at line 97 of file basic_algebra.c.

◆ AccumulateVector()

CBLAS_INLINE void AccumulateVector	(	unsigned int	s,
		double *	v1,
		double *	v2
	)

Adds a vector to another vector

Parameters

s	Number of elements in the vector.
v1	The vector to add.
v2	The vector where to add v1.

Definition at line 105 of file basic_algebra.c.

Referenced by accumSAM(), accumSAV(), AccumulateMatrix(), AddBranchToRRT(), BroydenUpdateJacobian(), ConnectSamplesChart(), dG(), dr(), EvaluateMEquationXVectors(), EvaluateTrans(), InitPatchTrans(), Local2Global(), NextDynamicState(), PolytopeBoundaryPointFromExternalCornerInt(), and PrintTransSeq().

◆ SumVector()

CBLAS_INLINE void SumVector	(	unsigned int	s,
		double *	v1,
		double *	v2,
		double *	v
	)

Adds two vectors and stores the result in another vector..

Parameters

s	Number of elements in the vector.
v1	The first vector to add.
v2	The second vector to add.
v	The vector where to store the result.

Definition at line 117 of file basic_algebra.c.

Referenced by body2SDF(), Chart2Manifold(), CombineGradients(), DynamicStepResidue(), GetJointTransSeq(), Local2Global(), LQRComputePolicy_t(), main(), NextDynamicStateLocalRK4(), NextDynamicStateRK4(), PrintJoint(), PrintTransSeq(), sumSAM(), and sumSAV().

◆ SumVectorScale()

CBLAS_INLINE void SumVectorScale	(	unsigned int	s,
		double *	v1,
		double	w,
		double *	v2,
		double *	v
	)

Adds two vectors /scaling the second vector) and stores the result in another vector.. This is basically used in Gram-Smidth orthogonalization.

Parameters

s	Number of elements in the vector.
v1	The first vector to add.
w	The weight to scale the second vector.
v2	The second vector to scale and add.
v	The vector where to store the result.

Definition at line 136 of file basic_algebra.c.

Referenced by AddBranchToRRT(), ConnectSamples(), ConnectSamplesChart(), CuikGradientInBox(), DynamicStepResidue(), EvaluateTrans(), EvaluateVectorsPATrans(), FindPointInOtherBranch(), FindSingularPoint(), GeodesicDistance(), GetJointBasicTransform(), GetJointDOFValues(), GetJointTransform(), HandC(), InitWorldFromMolecule(), LQRComputePolicy(), LQRComputePolicy_t(), NewTemptativeSample(), NewTriangularPrism(), NextDynamicState(), NextDynamicStateEuler(), NextDynamicStateLocalEuler(), NextDynamicStateLocalRK4(), NextDynamicStateRK4(), PointTowardRandSample(), StepDispersionGradient(), StepEffortGradient(), and StepLengthGradient().

◆ CosVector()

void CosVector	(	unsigned int	s,
		double *	v,
		double *	co
	)

Compputes the cosine for all the elements of a vector.

Parameters

s	Number of elements in the vector.
v	The vector.
co	The vector with the cosines (output, allocated externally).

Definition at line 160 of file basic_algebra.c.

◆ SinVector()

void SinVector	(	unsigned int	s,
		double *	v,
		double *	si
	)

Compputes the sine for all the elements of a vector.

Parameters

s	Number of elements in the vector.
v	The vector.
si	The vector with the sine (output, allocated externally).

Definition at line 168 of file basic_algebra.c.

◆ AddVectorElements()

double AddVectorElements	(	unsigned int	m,
		double *	v
	)

Adds all the elements of a vector.

Parameters

m	The size of the vector.
v	The vector.

Returns: The sum of the elements of the vector.

Definition at line 176 of file basic_algebra.c.

◆ AddVectorPositiveElements()

double AddVectorPositiveElements	(	unsigned int	m,
		double *	v
	)

Adds all the positive elements of a vector.

Parameters

m	The size of the vector.
v	The vector.

Returns: The sum of the positive elements of the vector.

Definition at line 189 of file basic_algebra.c.

Referenced by main().

◆ MaxVectorElement()

unsigned int MaxVectorElement	(	unsigned int	m,
		double *	v
	)

Returns the index of the maximum element in the given vector.

Parameters

m	The size of the vector.
v	The vector.

Returns: The index for the maximum element or NO_UINT if the vector is empty.

Definition at line 202 of file basic_algebra.c.

References NO_UINT.

◆ MaxVector()

double MaxVector	(	unsigned int	m,
		double *	v
	)

Returns the value of the maximum element in the given vector.

Parameters

m	The size of the vector.
v	The vector.

Returns: The value for the maximum element or 0 if the vector is empty.

Definition at line 226 of file basic_algebra.c.

Referenced by ErrorInInequalities(), ErrorInSimpInequalitiesOnPoint(), InequalitiesHoldOnPoint(), InitBTree(), and SimpInequalitiesHoldOnPoint().

◆ MinVectorElement()

unsigned int MinVectorElement	(	unsigned int	m,
		double *	v
	)

Returns the index of the minimum element in the given vector.

Parameters

m	The size of the vector.
v	The vector.

Returns: The index for the minimum element or NO_UINT if the vector is empty.

Definition at line 245 of file basic_algebra.c.

References NO_UINT.

◆ MinVector()

double MinVector	(	unsigned int	m,
		double *	v
	)

Returns the value of the minimum element in the given vector.

Parameters

m	The size of the vector.
v	The vector.

Returns: The value for the minimum element or 0 if the vector is empty.

Definition at line 269 of file basic_algebra.c.

◆ Vector2Range()

void Vector2Range	(	double	l,
		double	u,
		double	mode,
		unsigned int	m,
		double *	v
	)

Scales a vector so that their values span a given range.

Parameters

l	The lower value of the range.
u	The upper value of the range.
mode	Exponential used in the transform. If positive we increase the resolution in the upper levels. If negative, the difference in lower levels are more remarked.
m	The size of the vector.
v	The vector.

Definition at line 288 of file basic_algebra.c.

References Error(), and ZERO.

Referenced by main().

◆ SubtractVector()

CBLAS_INLINE void SubtractVector	(	unsigned int	s,
		double *	v1,
		double *	v2
	)

Subtracts a vector from another vector: v1=v1-v2

Parameters

s	Number of elements in the vector.
v1	The vector from where to subtract.
v2	The vector to subtract.

Definition at line 329 of file basic_algebra.c.

Referenced by AtlasRRTValidateSample(), ComputeHandC_FJH(), ComputeInverseDynamics(), LinearizeDynamics(), LQRComputePolicy_t(), PolytopeBoundaryPointFromExternalCornerInt(), and SubtractMatrix().

◆ DifferenceVector()

void DifferenceVector	(	unsigned int	s,
		double *	v1,
		double *	v2,
		double *	v
	)

inline

Subtracts two vectors and stores the result in another vector: v=v1-v2

Parameters

s	Number of elements in the vector.
v1	The vector from where to subtract.
v2	The vector to subtract.
v	The vector where to store the result.

Definition at line 342 of file basic_algebra.c.

Referenced by AdjustBioWorldGeometry(), Atoms2Transforms(), axis2SDF(), body2SDF(), BroydenUpdateJacobian(), ConnectSamplesChart(), DifferenceVectorSubset(), DifferenceVectorTopology(), DynamicStepResidue(), FindSingularPoint(), GetConnectionLinkPotentialEnergy(), GetConnectionLinkWrench(), GetJointDOFValues(), GetTransform2ConnectionLink(), InitPatchTrans(), InitWorldFromMolecule(), Manifold2Chart(), NewTriangularPrism(), PointTowardRandSample(), StepEffort(), StepEffortGradient(), Time2Go(), and TriangulateAtlas().

◆ DifferenceVectorSubset()

void DifferenceVectorSubset	(	unsigned int	s,
		boolean *	subset,
		double *	v1,
		double *	v2,
		double *	v
	)

Subtracts two vectors and stores the result in another vector: v=v1-v2 Only the selected components are operated. The rest are set to 0.

Parameters

s	Number of elements in the vector.
subset	The selected components.
v1	The vector from where to subtract.
v2	The vector to subtract.
v	The vector where to store the result.

Definition at line 364 of file basic_algebra.c.

References DifferenceVector().

Referenced by DifferenceVectorTopologySubset().

◆ DifferenceVectorTopology()

void DifferenceVectorTopology	(	unsigned int	s,
		unsigned int *	tp,
		double *	v1,
		double *	v2,
		double *	v
	)

Subtracts two vectors and stores the result in another vector considering the topology for each component.

Parameters

s	Number of elements in the vector.
tp	The topology for each variable.
v1	The vector from where to subtract.
v2	The vector to subtract.
v	The vector where to store the result.

Definition at line 384 of file basic_algebra.c.

References DifferenceVector(), PI2PI, and TOPOLOGY_S.

Referenced by AddBranchToRRT(), Chart2Manifold(), ConnectSamples(), ConnectSamplesChart(), DifferenceVectorTopologySubset(), GeodesicDistance(), and Manifold2Chart().

◆ DifferenceVectorTopologySubset()

void DifferenceVectorTopologySubset	(	unsigned int	s,
		unsigned int *	tp,
		boolean *	subset,
		double *	v1,
		double *	v2,
		double *	v
	)

Subtracts two vectors and stores the result in another vector considering the topology for the selected components.

The output for the non-selected components are set to 0.

Parameters

s	Number of elements in the vector.
tp	The topology for each variable. If NULL this function is the same as DifferenceVector.
subset	The selected dimensions. If NULL this function is the same as DifferenceVectorTopology.
v1	The vector from where to subtract.
v2	The vector to subtract.
v	The vector where to store the result.

Definition at line 403 of file basic_algebra.c.

References DifferenceVectorSubset(), DifferenceVectorTopology(), PI2PI, and TOPOLOGY_S.

Referenced by StepDispersionGradient(), StepEffort(), StepEffortGradient(), and StepLengthGradient().

◆ Norm()

CBLAS_INLINE double Norm	(	unsigned int	s,
		double *	v
	)

Computes the norm of a vector with the given size.

Parameters

s	Number of elements in the vector.
v	The vector.

Returns: The norm of the vector.

Definition at line 434 of file basic_algebra.c.

Referenced by AddBorderConstraint(), AddBranchToAtlasRRT(), AddBranchToRRT(), ApplyLinkRot(), ApplyLinkRotAxisX(), ApplyLinkRotDeformX(), AtlasRRTSample(), AtlasRRTValidateSample(), body2SDF(), BroydenStep(), CDCallBackInfo(), Chart2Manifold(), ConnectSamples(), ConnectSamplesChart(), CuikGradientInBox(), CuikNewtonInBox(), CutPolytope(), CutPolytopeWithFace(), CutSPolytope(), DetermineSPolytopeNeighbour(), DistanceOnChart(), DynamicStepResidue(), ErrorInCSEquations(), ErrorInSimpCSEquations(), ErrorInSolution(), EvaluateTrans(), ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), GetConnectionLinkPotentialEnergy(), GetConnectionLinkWrench(), GetJointDOFValues(), GetPolytopeInteriorPoint(), GetTransform2ConnectionLink(), InsideSPolytope(), LQRComputePolicy_t(), main(), Manifold2Chart(), MatrixWeightedNorm(), MinCosinusBetweenSubSpaces(), MinimizeOnAtlas(), NewChartFromPoint(), NewFixJoint(), NewInPatchJoint(), NewPrismaticJoint(), NewRevoluteJoint(), NewSphericalJoint(), NewTemptativeSample(), Newton2ManifoldPlane(), NewUniversalJoint(), NextDynamicState(), NextDynamicStateEuler(), NextDynamicStateLocalEuler(), NextDynamicStateLocalRK4(), NextDynamicStateRK4(), Normalize(), PathInChart(), PlotForceField(), PointTowardRandSample(), PolytopeBoundaryPointFromExternalCornerInt(), ReconstructAtlasPath(), RefineSingularPoint(), StepDispersionGradient(), StepEffort(), StepEffortGradient(), StepLengthGradient(), Time2Go(), and WeightedNorm().

◆ WeightedNorm()

CBLAS_INLINE double WeightedNorm	(	unsigned int	s,
		double *	w,
		double *	v
	)

Computes the weighted norm of a vector with the given size.

Parameters

s	Number of elements in the vector.
w	The weigths.
v	The vector.

Returns: The weighted norm of the vector.

Definition at line 449 of file basic_algebra.c.

References Norm().

◆ MatrixWeightedNorm()

CBLAS_INLINE double MatrixWeightedNorm	(	unsigned int	s,
		double *	W,
		double *	v
	)

The same as WeightedNorm but here the weights is given in the form of a matrix. Thus, were we compute v^T*W*v.

Parameters

s	Number of elements in the vector.
W	The (symmetric) matrix of weigths.
v	The vector.

Returns: The weighted norm of the vector.

Definition at line 465 of file basic_algebra.c.

References Norm(), and RC2INDEX.

◆ NormWithStride()

CBLAS_INLINE double NormWithStride	(	unsigned int	s,
		unsigned int	st,
		double *	v
	)

Computes the norm of a vector with the given size and whose elements might not be continuous in memory but separated by a given stride.

Parameters

s	Number of elements in the vector.
st	The stride (separation between elements of v).
v	The vector.

Returns: The norm of the vector.

Definition at line 485 of file basic_algebra.c.

◆ SquaredDistance()

double SquaredDistance	(	unsigned int	s,
		double *	v1,
		double *	v2
	)

inline

Computes the distance between two points given as a vector with a given size.

Parameters

s	Number of elements in the vectors.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 502 of file basic_algebra.c.

Referenced by Distance(), MatrixWeightedSquaredDistance(), SquaredDistanceSubset(), SquaredDistanceTopology(), and WeightedSquaredDistance().

◆ WeightedSquaredDistance()

double WeightedSquaredDistance	(	unsigned int	s,
		double *	w,
		double *	v1,
		double *	v2
	)

inline

Computes the weighted distance between two points given as a vector with a given size.

Parameters

s	Number of elements in the vectors.
w	Weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The weighted distance between the points.

Definition at line 518 of file basic_algebra.c.

References SquaredDistance().

Referenced by WeightedDistance(), WeightedSquaredDistanceSubset(), and WeightedSquaredDistanceTopology().

◆ MatrixWeightedSquaredDistance()

double MatrixWeightedSquaredDistance	(	unsigned int	s,
		double *	W,
		double *	v1,
		double *	v2
	)

inline

The same as WeightedSquaredDistance, but the weights are given in the form of a matrix. Thus, we compute (v2-v1)^t * W * (v2-v1). WeightedSquaredDistance can be seen as a particular case of this function where matrix W is diagonal.

Parameters

s	Number of elements in the vectors.
W	Weight in the form of a (symmetric) matrix.
v1	The first point.
v2	The second point.

Returns: The weighted distance between the points.

Definition at line 539 of file basic_algebra.c.

References RC2INDEX, and SquaredDistance().

Referenced by MatrixWeightedDistance(), and MatrixWeightedSquaredDistanceSubset().

◆ SquaredDistanceSubset()

double SquaredDistanceSubset	(	unsigned int	s,
		boolean *	subset,
		double *	v1,
		double *	v2
	)

Computes the distance between two points given as a vector with a given size along some of its components.

Parameters

s	Number of elements in the vectors.
subset	The selected components.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 565 of file basic_algebra.c.

References SquaredDistance().

Referenced by MatrixWeightedSquaredDistanceSubset(), SquaredDistanceTopologySubset(), and WeightedSquaredDistanceSubset().

◆ WeightedSquaredDistanceSubset()

double WeightedSquaredDistanceSubset	(	unsigned int	s,
		boolean *	subset,
		double *	w,
		double *	v1,
		double *	v2
	)

Computes the weighted distance between two points given as a vector with a given size along some of its components.

Parameters

s	Number of elements in the vectors.
subset	The selected components.
w	Weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 589 of file basic_algebra.c.

References SquaredDistanceSubset(), and WeightedSquaredDistance().

Referenced by WeightedSquaredDistanceTopologySubset().

◆ MatrixWeightedSquaredDistanceSubset()

double MatrixWeightedSquaredDistanceSubset	(	unsigned int	s,
		boolean *	subset,
		double *	W,
		double *	v1,
		double *	v2
	)

Computes the weighted distance between two points given as a vector with a given size along some of its components.

Parameters

s	Number of elements in the vectors.
subset	The selected components.
W	The (symmetric) matrix of weights. This is of the size of the vectors but only the selected rows/colums are used.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 618 of file basic_algebra.c.

References MatrixWeightedSquaredDistance(), RC2INDEX, and SquaredDistanceSubset().

◆ Distance()

double Distance	(	unsigned int	s,
		double *	v1,
		double *	v2
	)

inline

Computes the distance between two points given as a vector with a given size.

Parameters

s	Number of elements in the vectors.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 656 of file basic_algebra.c.

References SquaredDistance().

Referenced by AddBranchToAtlasRRT(), AdjustBioWorldGeometry(), BioWorldRMSE(), CrossTopologyBorder(), DetermineChartNeighbours(), DistanceOnChart(), HTransformIsIdentity(), HTransformIsZero(), main(), NewTemptativeSample(), PathInChart(), ReadSTL(), ReadTwoSamples(), SimplifyPolyhedron(), and UpdateLQRPolicy().

◆ WeightedDistance()

double WeightedDistance	(	unsigned int	s,
		double *	w,
		double *	v1,
		double *	v2
	)

inline

Computes the weighted distance between two points given as a vector with a given size.

Parameters

s	Number of elements in the vectors.
w	Weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 661 of file basic_algebra.c.

References WeightedSquaredDistance().

◆ MatrixWeightedDistance()

double MatrixWeightedDistance	(	unsigned int	s,
		double *	W,
		double *	v1,
		double *	v2
	)

inline

The same as WeightedDistance but the weights are given in matrix form. Thus, the computation is: (v2-v1)^t * W * (v2-v1)

Parameters

s	Number of elements in the vectors.
W	The (symmetric) matrix of weight.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 666 of file basic_algebra.c.

References MatrixWeightedSquaredDistance().

◆ SquaredDistanceTopology()

double SquaredDistanceTopology	(	unsigned int	s,
		unsigned int *	tp,
		double *	v1,
		double *	v2
	)

Computes the squared distance between two points given as a vector with a given size and considering the topology for each dimension.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
v1	The first point.
v2	The second point.

Returns: The squared distance between the points.

Definition at line 671 of file basic_algebra.c.

References M_PI, PI2PI, SquaredDistance(), and TOPOLOGY_S.

Referenced by DistanceTopology(), SquaredDistanceTopologySubset(), and WeightedSquaredDistanceTopology().

◆ WeightedSquaredDistanceTopology()

double WeightedSquaredDistanceTopology	(	unsigned int	s,
		unsigned int *	tp,
		double *	w,
		double *	v1,
		double *	v2
	)

Computes the squared distance between two points given as a vector with a given size and considering the topology for each dimension.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
w	Weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The squared distance between the points.

Definition at line 696 of file basic_algebra.c.

References M_PI, PI2PI, SquaredDistanceTopology(), TOPOLOGY_S, and WeightedSquaredDistance().

Referenced by WeightedDistanceTopology(), and WeightedSquaredDistanceTopologySubset().

◆ SquaredDistanceTopologyMin()

double SquaredDistanceTopologyMin	(	double	t2,
		unsigned int	s,
		unsigned int *	tp,
		double *	v1,
		double *	v2
	)

Computes the squared distance between two points given as a vector with a given size and considering the topology for each dimension.

The squared distance is only fully computed for points closer than a given threshold. This accelerates the distance computations.

Parameters

t2	The threshold (a square distance!).
s	Number of elements in the vectors.
tp	The topology for each dimension.
v1	The first point.
v2	The second point.

Returns: The squared distance between the points.

Definition at line 727 of file basic_algebra.c.

References M_PI, PI2PI, and TOPOLOGY_S.

Referenced by DistanceTopologyMin(), HaveChartAtPoint(), and WeightedSquaredDistanceTopologyMin().

◆ WeightedSquaredDistanceTopologyMin()

double WeightedSquaredDistanceTopologyMin	(	double	t2,
		unsigned int	s,
		unsigned int *	tp,
		double *	w,
		double *	v1,
		double *	v2
	)

Computes the squared distance between two points given as a vector with a given size and considering the topology for each dimension.

The squared distance is only fully computed for points closer than a given threshold. This accelerates the distance computations.

Parameters

t2	The threshold (a square distance!).
s	Number of elements in the vectors.
tp	The topology for each dimension.
w	Weight in each dimension.
v1	The first point.
v2	The second point.

Returns: The squared distance between the points.

Definition at line 759 of file basic_algebra.c.

References M_PI, PI2PI, SquaredDistanceTopologyMin(), and TOPOLOGY_S.

Referenced by WeightedDistanceTopologyMin().

◆ DistanceTopology()

double DistanceTopology	(	unsigned int	s,
		unsigned int *	tp,
		double *	v1,
		double *	v2
	)

Computes the distance between two points given as a vector with a given size and considering the topology for each components.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 795 of file basic_algebra.c.

References SquaredDistanceTopology().

Referenced by AddBranchToAtlasDynamicRRT(), AddBranchToAtlasRRT(), AddBranchToRRT(), AddNodeToRRT(), AddSample2AtlasRRT(), AddStepToRRTstar(), ApproachState(), AtlasAStar(), AtlasBiRRTstar(), AtlasGBF(), AtlasRRT(), AtlasRRTstar(), AtlasRRTValidateSample(), BiRRTstar(), BuildAtlasFromPoint(), cBiRRT(), ccRRT(), ccTRRT(), Chart2Manifold(), ChartErrorFromParameters(), ClassifyPointInChart(), ConnectDynamicStates(), ConnectDynamicStatesID(), ConnectSamples(), ConnectSamplesChart(), CrossTopologyBorder(), DistanceOnChart(), FindPointInOtherBranch(), GeodesicDistance(), GradientSmooth(), InDynamicDomain(), InitBranchState(), IntersectChartsInt(), kinobiRRT(), kinoEST(), kinoRRT(), NewTemptativeSample(), NumericIntegration(), PathInChart(), PathStart2GoalInRRT(), PointOnChart(), RecursiveReWireRRTstar(), RRTPathLength(), RRTstar(), RRTValidateSample(), and Steps2PathinRRT().

◆ WeightedDistanceTopology()

double WeightedDistanceTopology	(	unsigned int	s,
		unsigned int *	tp,
		double *	w,
		double *	v1,
		double *	v2
	)

Computes the distance between two points given as a vector with a given size and considering the topology for each components.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
w	Weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 801 of file basic_algebra.c.

References WeightedSquaredDistanceTopology().

Referenced by AddBranchToAtlasDynamicRRT(), AddBranchToAtlasRRT(), AddBranchToRRT(), InitBranchState(), and PathStart2GoalInRRT().

◆ SquaredDistanceTopologySubset()

double SquaredDistanceTopologySubset	(	unsigned int	s,
		unsigned int *	tp,
		boolean *	subset,
		double *	v1,
		double *	v2
	)

Computes the squared distance between two points given as a vector with a given size and considering the topology for the selected components.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
subset	The components to use in the distance computation. If NULL, this function is the same as SquaredDistanceTopology.
v1	The first point.
v2	The second point.

Returns: The squared distance between the points in the selected dimensions.

Definition at line 807 of file basic_algebra.c.

References M_PI, PI2PI, SquaredDistanceSubset(), SquaredDistanceTopology(), and TOPOLOGY_S.

Referenced by DistanceTopologySubset(), and WeightedSquaredDistanceTopologySubset().

◆ WeightedSquaredDistanceTopologySubset()

double WeightedSquaredDistanceTopologySubset	(	unsigned int	s,
		unsigned int *	tp,
		boolean *	subset,
		double *	w,
		double *	v1,
		double *	v2
	)

Computes the squared weighted distance between two points given as a vector with a given size and considering the topology for the selected components.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
subset	The components to use in the distance computation. If NULL, this function is the same as SquaredDistanceTopology.
w	The weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The squared weighted distance between the points in the selected dimensions.

Definition at line 840 of file basic_algebra.c.

References M_PI, PI2PI, SquaredDistanceTopologySubset(), TOPOLOGY_S, WeightedSquaredDistanceSubset(), and WeightedSquaredDistanceTopology().

Referenced by WeightedDistanceTopologySubset().

◆ DistanceTopologySubset()

double DistanceTopologySubset	(	unsigned int	s,
		unsigned int *	tp,
		boolean *	subset,
		double *	v1,
		double *	v2
	)

Computes the distance between two points given as a vector with a given size and considering the topology for the selected components.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
subset	The components to use in the distance computation. If NULL, this function is the same as DistanceTopology.
v1	The first point.
v2	The second point.

Returns: The distance between the points in the selected dimensions.

Definition at line 878 of file basic_algebra.c.

References SquaredDistanceTopologySubset().

Referenced by ConnectSamples(), ConnectSamplesChart(), PathLength(), StepDispersion(), StepDispersionGradient(), and StepLength().

◆ WeightedDistanceTopologySubset()

double WeightedDistanceTopologySubset	(	unsigned int	s,
		unsigned int *	tp,
		boolean *	subset,
		double *	w,
		double *	v1,
		double *	v2
	)

Computes the distance between two points given as a vector with a given size and considering the topology for the selected components.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
subset	The components to use in the distance computation. If NULL, this function is the same as DistanceTopology.
w	Weight for each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points in the selected dimensions.

Definition at line 885 of file basic_algebra.c.

References WeightedSquaredDistanceTopologySubset().

◆ DistanceTopologyMin()

double DistanceTopologyMin	(	double	t,
		unsigned int	s,
		unsigned int *	tp,
		double *	v1,
		double *	v2
	)

This is like DistanceTopology, but it only computes the distance as far it is below a given threshold. This is usefuls to rapidly abort computing distances for points that are further away than other points when searching for nearest neighbours.

Note that if the output is above the given threshold, the distance is not actually fully computed (the output is not the true distance between the given points).

Parameters

t	The threshold.
s	Number of elements in the vectors.
tp	The topology for each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 892 of file basic_algebra.c.

References SquaredDistanceTopologyMin().

Referenced by GetRRTNNInChart().

◆ WeightedDistanceTopologyMin()

double WeightedDistanceTopologyMin	(	double	t,
		unsigned int	s,
		unsigned int *	tp,
		double *	w,
		double *	v1,
		double *	v2
	)

This is like DistanceTopologyMin, but including weights in each dimension.

Parameters

t	The threshold.
s	Number of elements in the vectors.
tp	The topology for each dimension.
w	Weight in each dimension.
v1	The first point.
v2	The second point.

Returns: The distance between the points.

Definition at line 898 of file basic_algebra.c.

References WeightedSquaredDistanceTopologyMin().

Referenced by GetRRTNN(), GetRRTNNInBall(), GetRRTNNInBranch(), and PointInRRT().

◆ CrossTopologyBorder()

boolean CrossTopologyBorder	(	unsigned int	s,
		unsigned int *	tp,
		double *	v1,
		double *	v2
	)

Checks if the shortest line connecting two points crosses the borders imposed by the topology of each variable, i.e., if it goes across the pi,-pi discontinuity.

Parameters

s	Number of elements in the vectors.
tp	The topology for each dimension.
v1	The first point.
v2	The second point.

Returns: TRUE if the line crosses the topology border.

Definition at line 904 of file basic_algebra.c.

References Distance(), and DistanceTopology().

Referenced by TriangulateAtlas().

◆ Normalize()

CBLAS_INLINE void Normalize	(	unsigned int	s,
		double *	v
	)

Normalizes a generic vector with the given size.

Parameters

s	The size of the vector.
v	The vector to normalize. We output overwrite the input.

Definition at line 909 of file basic_algebra.c.

References Error(), and Norm().

Referenced by Atoms2Transforms(), axis2SDF(), body2SDF(), EvaluatePATrans(), GetJointBasicTransform(), GetJointDOFValues(), GetJointTransform(), GetTransform2ConnectionLink(), GetTransform2LinkAxisX(), NewTriangularPrism(), and PointTowardRandSample().

◆ Mean()

double Mean	(	unsigned int	s,
		double *	v
	)

Computes the mean of an array of doubles.

Parameters

s	The size of the vector.
v	The vector with the data.

Returns: The mean of the elements in the vector.

Definition at line 933 of file basic_algebra.c.

Referenced by PrintAverages().

◆ StdDev()

double StdDev	(	unsigned int	s,
		double	m,
		double *	v
	)

Computes the sample standard deviation of an array of doubles.

Parameters

s	The size of the vector.
m	The mean of the data (see Mean).
v	The vector with the data.

Returns: The mean of the elements in the vector.

Definition at line 945 of file basic_algebra.c.

Referenced by PrintAverages().

◆ ArrayPi2Pi()

void ArrayPi2Pi	(	unsigned int	n,
		unsigned int *	t,
		double *	a
	)

Applies PI2PI to an array but only to those elements that have sphere topology.

Parameters

n	Size of the array.
t	The topology for each entry in the array.
a	The array to adjust.

Definition at line 963 of file basic_algebra.c.

References PI2PI, and TOPOLOGY_S.

Referenced by AddChart2Btree(), AddNodeToRRT(), Chart2Manifold(), ConnectSamples(), ConnectSamplesChart(), CuikGradientInBox(), CuikNewtonInBox(), CuikNewtonSimp(), InitChartInt(), Local2Global(), Manifold2Chart(), Newton2ManifoldPlane(), RefineSingularPoint(), and SearchInBtree().

◆ GetRow()

CBLAS_INLINE void GetRow	(	unsigned int	k,
		unsigned int	r,
		unsigned int	c,
		double *	m,
		double *	v
	)

Defines a vector taking the values from the row of a matrix.

Parameters

k	The row to use.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix.
v	The vector to define.

Definition at line 976 of file basic_algebra.c.

References RC2INDEX.

◆ GetColumn()

CBLAS_INLINE void GetColumn	(	unsigned int	k,
		unsigned int	r,
		unsigned int	c,
		double *	m,
		double *	v
	)

Defines a vector taking the values from the column of a matrix.

Parameters

k	The column to use.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix.
v	The vector to define.

Definition at line 988 of file basic_algebra.c.

References RC2INDEX.

Referenced by FindRightNullVector(), HandC(), and RefineSingularPoint().

◆ SetRow()

CBLAS_INLINE void SetRow	(	double *	v,
		unsigned int	k,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Defines a row of a matrix taking the values from a vector.

Parameters

v	The vector with the values to use.
k	The row to replace.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1000 of file basic_algebra.c.

References RC2INDEX.

Referenced by Newton2ManifoldPlane().

◆ SetColumn()

CBLAS_INLINE void SetColumn	(	double *	v,
		unsigned int	k,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Defines a column of a matrix taking the values from a vector.

Parameters

v	The vector with the values to use.
k	The column to replace.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1012 of file basic_algebra.c.

References RC2INDEX.

Referenced by EvaluateHessianVector(), EvaluateNHessianVector2(), HandC(), and LinearizeDynamics().

◆ ScaleRow()

CBLAS_INLINE void ScaleRow	(	unsigned int	k,
		double	sc,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Multiples a row of a matrix by a given factor.

Parameters

k	The row to scale.
sc	The scale factor.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1025 of file basic_algebra.c.

References RC2INDEX.

Referenced by OrthonormalizeRows(), and SumRowsScale().

◆ ScaledAddRows()

CBLAS_INLINE void ScaledAddRows	(	unsigned int	r,
		unsigned int	c,
		double *	m,
		unsigned int	n,
		unsigned int *	sr,
		double *	sc,
		unsigned int	sv,
		double *	v
	)

Defines a vector as a scaled sum of rows of a matrix.

Parameters

r	Number of rows.
c	Number of columns.
m	The matrix.
n	The number of selected rows.
sr	The selection of rows.
sc	The scale factor for each row.
sv	Stride in the 'v' vector.
v	The vector to define.

Definition at line 1037 of file basic_algebra.c.

References RC2INDEX.

Referenced by CRFxSAM().

◆ ScaleColumn()

CBLAS_INLINE void ScaleColumn	(	unsigned int	k,
		double	sc,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Multiples a column of a matrix by a given factor.

Parameters

k	The column to scale.
sc	The scale factor.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1071 of file basic_algebra.c.

References RC2INDEX.

Referenced by OrthonormalizeColumns(), and SumColumnsScale().

◆ RowsDotProduct()

CBLAS_INLINE double RowsDotProduct	(	unsigned int	i,
		unsigned int	j,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Dot product between two rows of a matrix.

Parameters

i	The first row to operate.
j	The second row to operate.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix.

Definition at line 1083 of file basic_algebra.c.

References RC2INDEX.

Referenced by ComputeHandC_FJH(), and OrthonormalizeRows().

◆ ColumnsDotProduct()

CBLAS_INLINE double ColumnsDotProduct	(	unsigned int	i,
		unsigned int	j,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Dot product between two columns of a matrix.

Parameters

i	The first column to operate.
j	The second column to operate.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix.

Definition at line 1108 of file basic_algebra.c.

References RC2INDEX.

Referenced by OrthonormalizeColumns().

◆ RowSquaredNorm()

CBLAS_INLINE double RowSquaredNorm	(	unsigned int	k,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Computes the squared norm of a row of a matrix.

Parameters

k	The row to use.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Returns: The squared norm of the selected row.

Definition at line 1133 of file basic_algebra.c.

References RC2INDEX.

Referenced by OrthonormalizeRows().

◆ ColumnSquaredNorm()

CBLAS_INLINE double ColumnSquaredNorm	(	unsigned int	k,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Computes the squared norm of a column of a matrix.

Parameters

k	The column to use.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Returns: The squared norm of the selected column.

Definition at line 1154 of file basic_algebra.c.

References RC2INDEX.

Referenced by FindRightNullVector(), and OrthonormalizeColumns().

◆ SumRowsScale()

CBLAS_INLINE void SumRowsScale	(	unsigned int	i,
		double	sc,
		unsigned int	j,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Accumulates in row 'i' the values in row 'j' scaled by factor 'sc'.

Parameters

i	Index of the row where to accumulate.
sc	The scale factor.
j	Index of the row to scale and to accumulate in x.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1175 of file basic_algebra.c.

References RC2INDEX, and ScaleRow().

Referenced by OrthonormalizeRows().

◆ SumColumnsScale()

CBLAS_INLINE void SumColumnsScale	(	unsigned int	i,
		double	sc,
		unsigned int	j,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Accumulates in column 'i' the values in column 'j' scaled by factor 'sc'.

Parameters

i	Index of the column where to accumulate.
sc	The scale factor.
j	Index of the column to scale and accumulated.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1200 of file basic_algebra.c.

References RC2INDEX, and ScaleColumn().

Referenced by OrthonormalizeColumns().

◆ CopyRow()

CBLAS_INLINE void CopyRow	(	unsigned int	i,
		unsigned int	j,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Row copy.

Parameters

i	The index of the row where to copy.
j	The index of the row from where to copy.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1225 of file basic_algebra.c.

References RC2INDEX.

Referenced by OrthonormalizeRows().

◆ CopyColumn()

CBLAS_INLINE void CopyColumn	(	unsigned int	i,
		unsigned int	j,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Column copy.

Parameters

i	The index of the column where to copy.
j	The index of the column from where to copy.
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix to update.

Definition at line 1237 of file basic_algebra.c.

References RC2INDEX.

Referenced by OrthonormalizeColumns().

◆ IdentityMatrix()

CBLAS_INLINE void IdentityMatrix	(	unsigned int	n,
		double *	M
	)

Creates an identity matrix.

Parameters

n	The number of rows/columns of the matrix.
M	The matrix.

Definition at line 1249 of file basic_algebra.c.

References RC2INDEX.

Referenced by InitDynamicSpace(), MatrixExponential(), and NextDynamicState().

◆ DiagonalMatrix()

CBLAS_INLINE void DiagonalMatrix	(	unsigned int	n,
		double *	v,
		double *	M
	)

Creates a diagonal matrix.

Parameters

n	The number of rows/columns of the matrix.
v	The values for the diagonal.
M	The matrix.

Definition at line 1258 of file basic_algebra.c.

References RC2INDEX.

◆ SymmetrizeMatrix()

void SymmetrizeMatrix	(	unsigned int	n,
		double *	M
	)

Enforces the symmetry of a squared matrix.

Parameters

n	The size of the matrix.
M	The matrix.

Definition at line 1267 of file basic_algebra.c.

References RC2INDEX.

Referenced by dGt(), and SetLinearizedDynamics().

◆ MatrixVectorProduct()

CBLAS_INLINE void MatrixVectorProduct	(	unsigned int	r,
		unsigned int	c,
		double *	A,
		double *	b,
		double *	o
	)

Product of a matrix with r rows and c columns by a vector with c elements. The output is a vector of r entries.

The input and output vectors must be allocated externally. Here we assume they have the correct size.

Parameters

r	Number of rows of the matrix.
c	Number of columns of the matrix.
A	The matrix (stored as a vector row/column major).
b	The vector.
o	The resulting vector.

Definition at line 1287 of file basic_algebra.c.

References NEW, and RC2INDEX.

Referenced by BroydenUpdateJacobian(), ComputeAcceleration(), ComputeHandC_FJH(), ComputeInverseDynamics(), dr(), EvaluateHessianVector(), EvaluateHessianVector2(), EvaluateNHessianVector2(), GetJointBasicTransform(), HandC(), Local2Global(), LQRComputePolicy_t(), LQRPolicy(), MechEnergy(), RefineSingularPoint(), SAMxSAV(), and StepEffortGradient().

◆ MatrixVectorProductAccum()

CBLAS_INLINE void MatrixVectorProductAccum	(	unsigned int	r,
		unsigned int	c,
		double *	A,
		double *	b,
		double *	o
	)

Accumulates in 'o' the product of a matrix with r rows and c columns by a vector with c elements.

Note that 'o' must be different of A and 'b'. Otherwise the result would not be correct (and eventually an error is triggered).

Parameters

r	Number of rows of the matrix.
c	Number of columns of the matrix.
A	The matrix (stored as a vector row/column major).
b	The vector.
o	The resulting vector. o=o+A*b

Definition at line 1318 of file basic_algebra.c.

References Error(), and RC2INDEX.

Referenced by SAMxSAVAccum().

◆ TMatrixVectorProduct()

CBLAS_INLINE void TMatrixVectorProduct	(	unsigned int	r,
		unsigned int	c,
		double *	A,
		double *	b,
		double *	o
	)

Product of a transposed matrix with r rows and c columns by a vector with r elements. The output is a vector of c entries.

The input and output vectors must be allocated externally. Here we assume they have the correct size.

Parameters

r	Number of rows of the matrix (before transposing).
c	Number of columns of the matrix (before transposing).
A	The matrix (before transposing) stored as a vector (row or column major).
b	The vector (of size r).
o	The resulting vector.

Definition at line 1338 of file basic_algebra.c.

References NEW, and RC2INDEX.

Referenced by AtlasRRTValidateSample(), Chart2Manifold(), ComputeC(), CuikGradientInBox(), EvaluateSystemFromNJacobian(), LinearizeDynamics(), Manifold2Chart(), MinCosinusBetweenSubSpaces(), MinimizeOnAtlas(), NextDynamicState(), NextDynamicStateLocalEuler(), NextDynamicStateLocalRK4(), RefineSingularPoint(), StepDispersionGradient(), StepEffort(), StepEffortGradient(), StepLengthGradient(), Time2Go(), and TSAMxSAV().

◆ TMatrixVectorProductAccum()

CBLAS_INLINE void TMatrixVectorProductAccum	(	unsigned int	r,
		unsigned int	c,
		double *	A,
		double *	b,
		double *	o
	)

Accumulates the product of a transposed matrix with r rows and c columns by a vector with r elements. The output is a vector of c entries.

Note that 'o' must be different of A and 'b'. Otherwise the result would not be correct (and eventually an error is triggered).

Parameters

r	Number of rows of the matrix (before transposing).
c	Number of columns of the matrix (before transposing).
A	The matrix (before transposing) stored as a vector (row or column major).
b	The vector (of size r).
o	The resulting vector: o = o + A * b

Definition at line 1369 of file basic_algebra.c.

References Error(), and RC2INDEX.

Referenced by TSAMxSAVAccum().

◆ TMatrixVectorStrideProduct()

CBLAS_INLINE void TMatrixVectorStrideProduct	(	unsigned int	r,
		unsigned int	c,
		double *	A,
		unsigned int	s,
		double *	b,
		double *	o
	)

Product of a transposed matrix with r rows and c columns by a vector with r elements where the elements are separeted by stride positions. The output is a vector of c entries.

The input and output vectors must be allocated externally. Here we assume they have the correct size.

Parameters

r	Number of rows of the matrix (before transposing).
c	Number of columns of the matrix (before transposing).
A	The matrix (before transposing) stored as a vector (row or column major).
s	The stride between elements in b.
b	The vector (of size r).
o	The resulting vector.

Definition at line 1389 of file basic_algebra.c.

References NEW, and RC2INDEX.

Referenced by MinCosinusBetweenSubSpaces().

◆ AccumulateMatrix()

CBLAS_INLINE void AccumulateMatrix	(	unsigned int	nr,
		unsigned int	nc,
		double *	B,
		double *	A
	)

Compute the sum of two matrices.

Parameters

nr	Number of rows of the matrices.
nc	Number of columns of the matrices.
B	The matrix to add.
A	The matrix where to add.

Definition at line 1420 of file basic_algebra.c.

References AccumulateVector().

◆ AccumulateTMatrix()

CBLAS_INLINE void AccumulateTMatrix	(	unsigned int	nr,
		double *	B,
		double *	A
	)

Compute the sum of a matrix and the transposed of another matrix. Both matrices are squared (otherwise the sizes would not match).

Parameters

nr	Number of rows (and columns) of the matrices.
B	The matrix to transpose and add.
A	The matrix where to add.

Definition at line 1425 of file basic_algebra.c.

References RC2INDEX.

Referenced by dG().

◆ SubtractMatrix()

CBLAS_INLINE void SubtractMatrix	(	unsigned int	nr,
		unsigned int	nc,
		double *	A,
		double *	B
	)

Compute the subtraction of two matrices.

Parameters

nr	Number of rows of the matrices.
nc	Number of columns of the matrices.
A	The matrix from where to subtract.
B	The matrix to subtract.

Definition at line 1453 of file basic_algebra.c.

References SubtractVector().

Referenced by rbi2mci().

◆ MatrixMatrixProduct()

CBLAS_INLINE void MatrixMatrixProduct	(	unsigned int	ra,
		unsigned int	ca,
		double *	A,
		unsigned int	cb,
		double *	B,
		double *	C
	)

Computes the product of a matrix by another matrix.

Parameters

ra	Number of rows of A.
ca	Numer of columns of A. This also gives the number of rows of B.
A	The first matrix (ra x ca).
cb	Number of columns of B.
B	The second matrix (ca x cb)
C	The resulting matrix (ra x cb). The space for this matrix must be allocated (deallocated) externally.

Definition at line 1458 of file basic_algebra.c.

References NEW, and RC2INDEX.

Referenced by ComputeH(), ComputeHandC_FJH(), dG(), dGt(), HandC(), InitHandC(), MechEnergy(), ProjectToColumnSubSpace(), SAMxSAM(), SetLinearizedDynamics(), and TSAVxSAM().

◆ MatrixMatrixProductAccum()

CBLAS_INLINE void MatrixMatrixProductAccum	(	unsigned int	ra,
		unsigned int	ca,
		double *	A,
		unsigned int	cb,
		double *	B,
		double *	C
	)

Accumulates the product of a matrix by another matrix.

Note that C must be different of A and B. Otherwise the result would not be correct (and eventually an error is triggered).

Parameters

ra	Number of rows of A.
ca	Numer of columns of A. This also gives the number of rows of B.
A	The first matrix (ra x ca).
cb	Number of columns of B.
B	The second matrix (ca x cb)
C	The resulting matrix (ra x cb). The space for this matrix must be allocated (deallocated) externally.

Definition at line 1498 of file basic_algebra.c.

References Error(), and RC2INDEX.

Referenced by SAMxSAMAccum().

◆ MatrixTMatrixProduct()

CBLAS_INLINE void MatrixTMatrixProduct	(	unsigned int	ra,
		unsigned int	ca,
		double *	A,
		unsigned int	rb,
		double *	B,
		double *	C
	)

Computes the product of a matrix by another matrix transposed.

Parameters

ra	Number of rows of A (without transposing). This also gives the number of rows of B.
ca	Numer of columns of A (without transposing).
A	The first matrix (ra x ca).
rb	Number of rows of B.
B	The second matrix (rb x ca)
C	The resulting matrix (ca x rb). The space for this matrix must be allocated (deallocated) externally.

Definition at line 1527 of file basic_algebra.c.

References NEW, and RC2INDEX.

Referenced by dGt(), InitHandC(), rbi2mci(), and SetLinearizedDynamics().

◆ TMatrixMatrixProduct()

CBLAS_INLINE void TMatrixMatrixProduct	(	unsigned int	ra,
		unsigned int	ca,
		double *	A,
		unsigned int	cb,
		double *	B,
		double *	C
	)

Computes the product of a matrix transposed by another matrix.

Parameters

ra	Number of rows of A (without transposing). This also gives the number of rows of B.
ca	Numer of columns of A (without transposing).
A	The first matrix (ra x ca).
cb	Number of columns of B.
B	The second matrix (ra x cb)
C	The resulting matrix (ca x cb). The space for this matrix must be allocated (deallocated) externally.

Definition at line 1567 of file basic_algebra.c.

References NEW, and RC2INDEX.

Referenced by CompareTangentSpaces(), ComputeH(), FindRightNullVector(), ProjectToColumnSubSpace(), and TSAMxSAM().

◆ MinCosinusBetweenSubSpaces()

double MinCosinusBetweenSubSpaces	(	unsigned int	m,
		unsigned int	k,
		double *	T1,
		double *	T2
	)

We determine the cosinus of the maximum angle between tangent spaces.

Note that we actually return the minimum of the absolute values of the cosinus. This is so since aligned tangent spaces form an angle of 0 or of pi (with cosinus 1 or -1 respectively).

In other words, for subspaces to be equal this function should return 1. If the output is 0, the subspaces are orthogonal.

In any case, we assume that the matrices defining the subspaces are orthonormal.

Parameters

m	Rows of the matrices defining the sub-spaces.
k	Columns of the matrices defining the sub-spaces.
T1	The matrix with the first sub-space.
T2	The matrix with the second sub-space.

Returns: The cosinus of the maximum angle (i.e., the minimum cosinus) between the sub-spaces.

Definition at line 1607 of file basic_algebra.c.

References NEW, Norm(), RC2INDEX, TMatrixVectorProduct(), and TMatrixVectorStrideProduct().

Referenced by MinCosinusBetweenCharts().

◆ OrthonormalizeRows()

unsigned int OrthonormalizeRows	(	unsigned int	r,
		unsigned int	c,
		double *	m
	)

Applies the Gram-Schmidt process to the rows of a matrix. Note that the input matrix might not have independent rows. The output may have only few non-null rows (as many as those indicated by the return value of this function). The orthonormalized rows are returned in the first rows of the output matrix.

Parameters

r	The number of rows of the matrix.
c	The number of columns of the matrix.
m	The matrix.

Returns: The number of indepentdent columns in the input matrix.

Definition at line 1651 of file basic_algebra.c.

References CopyRow(), RowsDotProduct(), RowSquaredNorm(), ScaleRow(), and SumRowsScale().

◆ OrthonormalizeColumns()

unsigned int OrthonormalizeColumns	(	unsigned int	r,
		unsigned int	c,
		double *	m
	)

The same as OrthonormalizeRows but operating on columns.

Parameters

r	The number of rows of the matrix.
c	The number of columns of the matrix.
m	The matrix.

Returns: The number of independent columns in the input matrix.

Definition at line 1677 of file basic_algebra.c.

References ColumnsDotProduct(), ColumnSquaredNorm(), CopyColumn(), ScaleColumn(), and SumColumnsScale().

◆ ProjectToColumnSubSpace()

void ProjectToColumnSubSpace	(	unsigned int	rs,
		unsigned int	cs,
		double *	ms,
		unsigned int	c,
		double *	m
	)

Projects a set of vectors into the sub-space spanned by the columns of a matrix.

Note that despite projecting, the returnted vectors are in the original space (i.e., not given as components in the sub-space of the columns of me).

We assume that the sub-space is given by a set of normalized vectors.

Parameters

rs	Number of rows of the matrix spanning the sub-space of interest. Number of rows of the matrix to project.
cs	Number of columns of the matrix spanning the sub-space of interest.
ms	Matrix spanning the sub-space of interest. Each column must be a normal vector.
c	The number of columns of the matrix to project. Number of vectors to project into the sub-space spanned by ms.
m	The matrix to project. The vectors in the columns of this matrix are the ones projected. At the output, this containts the ambient space coordinates of the vectors inside the given subspace.

Definition at line 1710 of file basic_algebra.c.

References MatrixMatrixProduct(), NEW, and TMatrixMatrixProduct().

◆ RankOneUpdate()

CBLAS_INLINE void RankOneUpdate	(	double	sc,
		unsigned int	nr,
		double *	vr,
		unsigned int	nc,
		double *	vc,
		double *	m
	)

Updates a matrix with a rank-one update.

Parameters

sc	Scale factor.
nr	Number of rows of the matrix. Number of elements in the first vector.
vr	The first vector.
nc	Number of columns of the matrix. Number of elements in the second vector.
vc	The second vector.
m	The matrix.

Definition at line 1723 of file basic_algebra.c.

References RC2INDEX.

Referenced by BroydenUpdateJacobian().

◆ SubMatrixFromMatrix()

CBLAS_INLINE void SubMatrixFromMatrix	(	unsigned int	nr1,
		unsigned int	nc1,
		double *	m1,
		unsigned int	nri,
		unsigned int	nci,
		unsigned int	nr,
		unsigned int	nc,
		double *	m
	)

Inserts a matrix as a submatrix of a larger matrix.

Parameters

nr1	Number of rows of the matrix to insert.
nc1	Number of columns of the matrix to insert.
m1	The matrix to insert.
nri	Row defining the position where to insert the matrix.
nci	Column defining the positon where to insert the matrix.
nr	Number of rows of the matrix to modify.
nc	Number of columns of the matrix to modify.
m	The matrix to modify.

Definition at line 1740 of file basic_algebra.c.

References RC2INDEX.

Referenced by ComputeAcceleration(), ComputeH(), ComputeInverseDynamics(), EvaluateJacobianInVector(), EvaluateJacobianSubSetInVector(), EvaluateTransposedJacobianInVector(), EvaluateTransposedJacobianSubSetInVector(), NextDynamicState(), and RefineSingularPoint().

◆ AddSubMatrix()

CBLAS_INLINE void AddSubMatrix	(	double	sc,
		unsigned int	nr1,
		unsigned int	nc1,
		double *	m1,
		unsigned int	nri,
		unsigned int	nci,
		unsigned int	nr,
		unsigned int	nc,
		double *	m
	)

Adds a submatrix into a particular position of a larger matrix.

Parameters

sc	Scale factor.
nr1	Number of rows of the matrix to add.
nc1	Number of columns of the matrix to add.
m1	The matrix to add.
nri	Row defining the position where to add the matrix.
nci	Column defining the positon where to add the matrix.
nr	Number of rows of the matrix to modify.
nc	Number of columns of the matrix to modify.
m	The matrix to modify.

Definition at line 1784 of file basic_algebra.c.

References RC2INDEX.

Referenced by ComputeHandC_FJH().

◆ SubMatrixFromSubMatrix()

CBLAS_INLINE void SubMatrixFromSubMatrix	(	unsigned int	nrs,
		unsigned int	ncs,
		unsigned int	nrc,
		unsigned int	ncc,
		unsigned int	nr1,
		unsigned int	nc1,
		double *	m1,
		unsigned int	nri,
		unsigned int	nci,
		boolean	trans,
		unsigned int	nr,
		unsigned int	nc,
		double *	m
	)

Inserts a submatrix taken from a matrix as a submatrix of a second matrix.

Parameters

nrs	Row defining the position from where to take the submatrix to copy.
ncs	Column defining the position from where to take the submatrix to copy.
nrc	Number of rows of the submatrix to copy.
ncc	Number of columns of the submatrix to copy.
nr1	Total number of rows of the matrix from where to take the submatrix to insert.
nc1	Total number of columns of the matrix from where to take the submatrix to insert.
m1	The matrix from where to take the submatrix to insert.
nri	Row defining the position where to insert the matrix.
nci	Column defining the positon where to insert the matrix.
trans	TRUE if the input is transposed in the copy.
nr	Number of rows of the matrix to modify.
nc	Number of columns of the matrix to modify.
m	The matrix to modify.

Definition at line 1829 of file basic_algebra.c.

References RC2INDEX.

◆ MatrixFromSubMatrix()

CBLAS_INLINE void MatrixFromSubMatrix	(	unsigned int	r_src,
		unsigned int	c_src,
		double *	m_src,
		unsigned int	rs,
		unsigned int	cs,
		unsigned int	r_dst,
		unsigned int	c_dst,
		double *	m_dst
	)

Copies a sub-matrix into a matrix.

Note that rs+r_dst should be lower or equal than r_src and the same for the columns, but no error check is performed.

Parameters

r_src	Number of rows of the matrix from where to copy.
c_src	Number of columns of the matrix from where to copy.
m_src	The matrix from where to copy.
rs	Row of the position from where to take the block to copy.
cs	Column of the position from where to take the block to copy.
r_dst	Number of rows of the matrix where to copy. Size in rows of the block to take from m_src.
c_dst	Number of columns of the matrix where to copy. Size in columns of the block to take from m_src.
m_dst	The matrix to initialize.

Definition at line 1926 of file basic_algebra.c.

References RC2INDEX.

Referenced by ComputeHandC_FJH(), and rbi2mci().

◆ SubMatrixFromTMatrix()

CBLAS_INLINE void SubMatrixFromTMatrix	(	unsigned int	nr1,
		unsigned int	nc1,
		double *	m1,
		unsigned int	nri,
		unsigned int	nci,
		unsigned int	nr,
		unsigned int	nc,
		double *	m
	)

Inserts a transposed matrix as a submatrix of a larger matrix.

Parameters

nr1	Number of rows of the matrix to insert (before transposing it).
nc1	Number of columns of the matrix to insert (before transposing it).
m1	The matrix to insert.
nri	Row defining the position where to insert the matrix.
nci	Column defining the positon where to insert the matrix.
nr	Number of rows of the matrix to modify.
nc	Number of columns of the matrix to modify.
m	The matrix to modify.

Definition at line 1970 of file basic_algebra.c.

References RC2INDEX.

Referenced by Chart2Manifold(), ComputeAcceleration(), ComputeH(), ComputeInverseDynamics(), GetChartDegree(), LQRComputePolicy(), LQRComputePolicy_t(), NextDynamicState(), and RefineSingularPoint().

◆ SelectMatrixColumns()

CBLAS_INLINE void SelectMatrixColumns	(	boolean *	sc,
		unsigned int	nr,
		unsigned int	nc,
		double *	m
	)

Reduces the size of a matrix selecting some of their columns, for instance, the linearly independent ones.

Parameters

sc	The selected columns (boolean array).
nr	Number of rows in the matrix.
nc	Number of columns in the matrix.
m	The matrix (stored as a matrix).

Definition at line 2014 of file basic_algebra.c.

References RC2INDEX.

Referenced by GetPositionJacobian().

◆ SelectMatrixRows()

CBLAS_INLINE void SelectMatrixRows	(	boolean *	sr,
		unsigned int	nr,
		unsigned int	nc,
		double *	m
	)

Reduces the size of a matrix selecting some of their rows, for instance, the linearly independent ones.

Parameters

sr	The selected rows (boolean array).
nr	Number of rows in the matrix.
nc	Number of columns in the matrix.
m	The matrix (stored as a matrix).

Definition at line 2055 of file basic_algebra.c.

References RC2INDEX.

Referenced by ComputeHandC_FJH(), and GetPositionJacobian().

◆ PrintVector()

void PrintVector	(	FILE *	f,
		char *	label,
		unsigned int	n,
		double *	v
	)

Prints a vector into a file.

Parameters

f	The file where to store the vector.
label	The name of the vector (possibly NULL).
n	The size of the vector.
v	The vector.

Definition at line 2096 of file basic_algebra.c.

Referenced by AddBranchToAtlasDynamicRRT(), AddBranchToAtlasRRT(), AddChart2AtlasRRT(), ApplyExternalForces(), AtlasRRT(), AtlasRRTValidateSample(), BroydenStep(), ComputeAcceleration(), ComputeC(), ComputeH(), ComputeJacobianKernelBasis(), DynamicStepResidue(), GetChartDegree(), GetRRTNNInChart(), HandC(), InitChartInt(), LinearizeDynamics(), LQRComputePolicy(), LQRComputePolicy_t(), main(), MechEnergy(), NewTemptativeSample(), NextDynamicState(), PointTowardRandSample(), and UpdateLQRPolicy().

◆ SaveVector()

void SaveVector	(	FILE *	f,
		unsigned int	n,
		double *	v
	)

Saves a vector into a file.

Parameters

f	The file where to store the vector.
n	The size of the vector.
v	The vector.

Definition at line 2108 of file basic_algebra.c.

Referenced by main().

◆ PrintMatrix()

void PrintMatrix	(	FILE *	f,
		char *	label,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Prints a matrix into a file.

Parameters

f	The file where to print the matrix.
label	Tne name of the matrix (possibly NULL)
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix.

Definition at line 2117 of file basic_algebra.c.

References RC2INDEX.

Referenced by ApplyExternalForces(), BroydenStep(), ComputeAcceleration(), ComputeC(), ComputeH(), FindRightNullVector(), GetChartDegree(), HandC(), InitHandC(), LinearizeDynamics(), LQRComputePolicy(), LQRComputePolicy_t(), MechEnergy(), NextDynamicState(), PointTowardRandSample(), and UpdateLQRPolicy().

◆ PrintTMatrix()

void PrintTMatrix	(	FILE *	f,
		char *	label,
		unsigned int	r,
		unsigned int	c,
		double *	m
	)

Prints a transposed matrix into a file.

Parameters

f	The file where to print the matrix.
label	Tne name of the matrix (possibly NULL)
r	Number of rows of the matrix.
c	Number of columns of the matrix.
m	The matrix.

Definition at line 2140 of file basic_algebra.c.

References RC2INDEX.

Referenced by ComputeAcceleration(), ComputeJacobianKernelBasis(), FindRightNullVector(), GetPositionJacobian(), and NextDynamicState().

Institut de Robòtica i Informàtica Industrial

Introduction

Functions

Function Documentation

◆ ClearVector()

◆ VectorAnd()

◆ VectorOr()

◆ GeneralDotProduct()

◆ MatrixWeigthedDotProduct()

◆ ScaleVector()

◆ ScaleVector2()

◆ OffsetVector()

◆ AccumulateVector()

◆ SumVector()

◆ SumVectorScale()

◆ CosVector()

◆ SinVector()

◆ AddVectorElements()

◆ AddVectorPositiveElements()

◆ MaxVectorElement()

◆ MaxVector()

◆ MinVectorElement()

◆ MinVector()

◆ Vector2Range()

◆ SubtractVector()

◆ DifferenceVector()

◆ DifferenceVectorSubset()

◆ DifferenceVectorTopology()

◆ DifferenceVectorTopologySubset()

◆ Norm()

◆ WeightedNorm()

◆ MatrixWeightedNorm()

◆ NormWithStride()

◆ SquaredDistance()

◆ WeightedSquaredDistance()

◆ MatrixWeightedSquaredDistance()

◆ SquaredDistanceSubset()

◆ WeightedSquaredDistanceSubset()

◆ MatrixWeightedSquaredDistanceSubset()

◆ Distance()

◆ WeightedDistance()

◆ MatrixWeightedDistance()

◆ SquaredDistanceTopology()

◆ WeightedSquaredDistanceTopology()

◆ SquaredDistanceTopologyMin()

◆ WeightedSquaredDistanceTopologyMin()

◆ DistanceTopology()

◆ WeightedDistanceTopology()

◆ SquaredDistanceTopologySubset()

◆ WeightedSquaredDistanceTopologySubset()

◆ DistanceTopologySubset()

◆ WeightedDistanceTopologySubset()

◆ DistanceTopologyMin()

◆ WeightedDistanceTopologyMin()

◆ CrossTopologyBorder()

◆ Normalize()

◆ Mean()

◆ StdDev()

◆ ArrayPi2Pi()

◆ GetRow()

◆ GetColumn()

◆ SetRow()

◆ SetColumn()

◆ ScaleRow()

◆ ScaledAddRows()

◆ ScaleColumn()

◆ RowsDotProduct()

◆ ColumnsDotProduct()

◆ RowSquaredNorm()

◆ ColumnSquaredNorm()

◆ SumRowsScale()

◆ SumColumnsScale()

◆ CopyRow()

◆ CopyColumn()

◆ IdentityMatrix()

◆ DiagonalMatrix()

◆ SymmetrizeMatrix()

◆ MatrixVectorProduct()

◆ MatrixVectorProductAccum()

◆ TMatrixVectorProduct()

Institut de Robòtica
i Informàtica Industrial