Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -38,103 +38,109 @@ subject to the following restrictions:
#ifdef _DYNAMIC
const double BASEMAXDIST = 0.02;
#else
const double MAXDIST = 0.08; // optimal value for double Y shape : 0.08
const double MAXDIST = 0.08; // optimal value for double Y shape : 0.08
#endif
const double DELTA = 0.4;
const long double LAMBDA = 2.0; // only for DLS. optimal : 0.24
const long double LAMBDA = 2.0; // only for DLS. optimal : 0.24
const double NEARZERO = 0.0000000001;
enum UpdateMode {
enum UpdateMode
{
JACOB_Undefined = 0,
JACOB_JacobianTranspose = 1,
JACOB_PseudoInverse = 2,
JACOB_DLS = 3,
JACOB_SDLS = 4 };
JACOB_SDLS = 4
};
class Jacobian {
class Jacobian
{
public:
Jacobian(Tree*);
Jacobian(bool useAngularJacobian, int nDof);
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; }
void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void ZeroDeltaThetas();
void CalcDeltaThetasTranspose();
void CalcDeltaThetasPseudoinverse();
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLS2(const VectorRn& dVec);
void CalcDeltaThetasDLS2(const VectorRn& dVec);
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV);
void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV);
void UpdateThetas();
void UpdateThetaDot();
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
void UpdateThetaDot();
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
const VectorRn& GetErrorArray() const { return errorArray; }
void UpdatedSClampValue(VectorR3* targets);
void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; }
void SetCurrentMode(UpdateMode mode) { CurrentUpdateMode = mode; }
UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }
void SetDampingDLS( double lambda ) { DampingLambda = lambda; DampingLambdaSq = Square(lambda); }
void SetDampingDLS(double lambda)
{
DampingLambda = lambda;
DampingLambdaSq = Square(lambda);
}
void Reset();
static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 );
static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies );
static void CompareErrors(const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2);
static void CountErrors(const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies);
int GetNumRows() { return nRow; }
int GetNumCols() { return nCol; }
int GetNumRows() {return nRow;}
int GetNumCols() {return nCol;}
public:
Tree* m_tree; // tree associated with this Jacobian matrix
int m_nEffector; // Number of end effectors
int nJoint; // Number of joints
int nRow; // Total number of rows the real J (= 3*number of end effectors for now)
int nCol; // Total number of columns in the real J (= number of joints for now)
Tree* m_tree; // tree associated with this Jacobian matrix
int m_nEffector; // Number of end effectors
int nJoint; // Number of joints
int nRow; // Total number of rows the real J (= 3*number of end effectors for now)
int nCol; // Total number of columns in the real J (= number of joints for now)
MatrixRmn Jend; // Jacobian matrix based on end effector positions
MatrixRmn Jtarget; // Jacobian matrix based on target positions
MatrixRmn Jnorms; // Norms of 3-vectors in active Jacobian (SDLS only)
MatrixRmn Jend; // Jacobian matrix based on end effector positions
MatrixRmn Jtarget; // Jacobian matrix based on target positions
MatrixRmn Jnorms; // Norms of 3-vectors in active Jacobian (SDLS only)
MatrixRmn U; // J = U * Diag(w) * V^T (Singular Value Decomposition)
VectorRn w;
MatrixRmn U; // J = U * Diag(w) * V^T (Singular Value Decomposition)
VectorRn w;
MatrixRmn V;
UpdateMode CurrentUpdateMode;
VectorRn dS; // delta s
VectorRn dT1; // delta t -- these are delta S values clamped to smaller magnitude
VectorRn dSclamp; // Value to clamp magnitude of dT at.
VectorRn dTheta; // delta theta
VectorRn dPreTheta; // delta theta for single eigenvalue (SDLS only)
VectorRn dS; // delta s
VectorRn dT1; // delta t -- these are delta S values clamped to smaller magnitude
VectorRn dSclamp; // Value to clamp magnitude of dT at.
VectorRn dTheta; // delta theta
VectorRn dPreTheta; // delta theta for single eigenvalue (SDLS only)
VectorRn errorArray; // Distance of end effectors from target after updating
VectorRn errorArray; // Distance of end effectors from target after updating
// Parameters for pseudoinverses
static const double PseudoInverseThresholdFactor; // Threshold for treating eigenvalue as zero (fraction of largest eigenvalue)
static const double PseudoInverseThresholdFactor; // Threshold for treating eigenvalue as zero (fraction of largest eigenvalue)
// Parameters for damped least squares
static const double DefaultDampingLambda;
double DampingLambda;
double DampingLambdaSq;
//double DampingLambdaSDLS;
// Cap on max. value of changes in angles in single update step
static const double MaxAngleJtranspose;
static const double MaxAnglePseudoinverse;
static const double MaxAngleDLS;
static const double MaxAngleSDLS;
static const double MaxAngleDLS;
static const double MaxAngleSDLS;
MatrixRmn* Jactive;
void CalcdTClampedFromdS();
static const double BaseMaxTargetDist;
};
#endif