void Gradient::viterbiDecoding(Beliefs bel, iVector& ystar, dMatrix& pystar)
{
int nbNodes, nbStates, xi, yi;
double max_val;
nbNodes = (int)bel.belStates.size();
nbStates = 0;
if(nbNodes > 0)
nbStates = bel.belStates[0].getLength();
ystar.create(nbNodes);
pystar.create(nbNodes,nbStates);
// Viterbi decoding
for( xi=0; xi<nbNodes; xi++) {
ystar.setValue(xi, 0);
max_val = bel.belStates[xi][0];
pystar.setValue(0, xi, bel.belStates[xi][0]);
for( yi=1; yi<nbStates; yi++) {
pystar.setValue(yi, xi, bel.belStates[xi][yi]);
if(max_val < bel.belStates[xi][yi]) {
ystar.setValue(xi,yi);
max_val = bel.belStates[xi][yi];
}
}
}
}
void BLASInterface::Mul(const dMatrix& A,const dMatrix& B,dMatrix& X)
{
X.resize(A.m,B.n);
X.setZero();
if(X.isRowMajor()) std::swap(X.istride,X.jstride);
Madd(A,B,X);
}
void dRigidbodyNodeInfo::BakeTransform (const dMatrix& transform)
{
// SetTransform (transform.Inverse4x4() * GetTransform() * transform);
dNodeInfo::BakeTransform (transform);
m_centerOfMass = transform.UnrotateVector(m_centerOfMass);
m_massMatrix = transform.UnrotateVector(m_massMatrix);
m_velocity = transform.RotateVector(m_velocity);
}
dMatrix transpose(dMatrix m){
dMatrix t(m[0].size(), dVector(m.size()));
for (int i = 0; i < m.size(); i++){
for( int j = 0; j < m[0].size(); j++){
t[i][j] = m[j][i];
}
}
return t;
}
void MSNewton::Servo::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
dMatrix matrix;
dVector centre;
NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
NewtonBodyGetCentreOfMass(joint_data->child, ¢re[0]);
centre = matrix.TransformVector(centre);
centre = pin_matrix.UntransformVector(centre);
dVector point(0.0f, 0.0f, centre.m_z);
pin_matrix.m_posit = pin_matrix.TransformVector(point);
}
dMatrix clSpline::GetCoordinates(const dMatrix &uSpec)
{
dMatrix coord;
if(uSpec.GetNumberColumns() != 1 || uSpec.GetNumberRows() == 0)
{
throw clException("clSpline", "GetCoordinates", "Invalid dimensions of matrix uSpec.");
}
else
{
coord.SetNumberRows(uSpec.GetNumberRows());
coord.SetNumberColumns(3);
if(!initialised)
{
Initialise();
}
// Do for all specified u's
for(int i=1; i<=uSpec.GetNumberRows(); i++)
{
if(uSpec(i, 1) < 0 || uSpec(i, 1) > 1)
{
throw clException("clSpline", "GetCoordinates", "Invalid value for uSpec.");
}
else
{
// Now find the position of uSpec
double uu = uSpec(i, 1)*GetSplineLength();
int j = 1;
while((uu - u(j+1, 1) > 0) && (j<u.GetNumberRows()-1))
{
j++;
}
// Now calculate the coefficients
double A = (u(j+1, 1)-uu)/(u(j+1, 1)-u(j,1));
double B = 1-A;
double C = (A*A*A-A)/6 * (u(j+1, 1)-u(j, 1))*(u(j+1, 1)-u(j, 1));
double D = (B*B*B-B)/6 * (u(j+1, 1)-u(j, 1))*(u(j+1, 1)-u(j, 1));
// Finally calculate the coordinates
coord.SetElement(i, 1, A*X(j, 1) + B*X(j+1, 1) + C*X2(j, 1) + D*X2(j+1, 1));
coord.SetElement(i, 2, A*Y(j, 1) + B*Y(j+1, 1) + C*Y2(j, 1) + D*Y2(j+1, 1));
coord.SetElement(i, 3, A*Z(j, 1) + B*Z(j+1, 1) + C*Z2(j, 1) + D*Z2(j+1, 1));
}
}
}
return (coord);
}
dMatrix multiplicacaoNN(const dMatrix m1, const dMatrix m2)
{
int m1l = m1.size(), m1c = m1[0].size(), m2l=m2.size(), m2c = m2[0].size();
dMatrix matrix(m1l, dVector(m2c, 0));
for (int l = 0; l < m1l; l++){
for (int c = 0; c < m1c; c++){
for (int k = 0; k < m1c; k++){
matrix[l][c] += m1[l][k] * m2[k][c];
}
}
}
return matrix;
}
void dMeshNodeInfo::BakeTransform (const dMatrix& transform)
{
dVector scale;
dMatrix stretchMatrix;
// dMatrix matrix (m_matrix * transform);
// matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
// matrix = dMatrix (GetIdentityMatrix(), scale, stretchMatrix);
dMatrix tmp (m_matrix);
dMatrix matrix (transform.Inverse4x4() * m_matrix * transform);
matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
matrix = transform * dMatrix (GetIdentityMatrix(), scale, stretchMatrix);
int pointCount = NewtonMeshGetPointCount (m_mesh);
int pointStride = NewtonMeshGetPointStrideInByte (m_mesh) / sizeof (dFloat);
dFloat* const points = NewtonMeshGetPointArray (m_mesh);
matrix.TransformTriplex(points, pointStride * sizeof (dFloat), points, pointStride * sizeof (dFloat), pointCount);
dFloat* const normals = NewtonMeshGetNormalArray(m_mesh);
dMatrix rotation (matrix.Inverse4x4().Transpose() * matrix);
rotation.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
rotation.TransformTriplex(normals, pointStride * sizeof (dFloat), normals, pointStride * sizeof (dFloat), pointCount);
int vertexCount = NewtonMeshGetVertexCount (m_mesh);
int vertexStride = NewtonMeshGetVertexStrideInByte (m_mesh) / sizeof (dFloat);
dFloat* const vertex = NewtonMeshGetVertexArray (m_mesh);
matrix.TransformTriplex(vertex, vertexStride * sizeof (dFloat), vertex, vertexStride * sizeof (dFloat), vertexCount);
}
double segment::diff_unary(const dMatrix M, int c1, int c2)
{
double val = 0;
for( int r=0; r<M.getHeight(); r++ )
val += fabs( M(r,c1) - M(r,c2) );
return val;
}
void ConvexApproximationObject::AddPolygonFromObject(INode* const node, ObjectState* const os, NewtonMesh* const meshOut, const dMatrix& matrix)
{
BOOL needDel = FALSE;
PolyObject* const poly = GetPolyObject (os, needDel);
if (poly) {
float polygon[32][12];
memset (polygon, 0, sizeof (polygon));
MNMesh& maxMesh = poly->GetMesh();
int facesCount = maxMesh.FNum();
int vertexCount = maxMesh.VNum();
if (facesCount && vertexCount) {
for (int i = 0; i < facesCount; i ++) {
MNFace* const face = maxMesh.F(i);
for (int j = 0; j < face->deg; j ++) {
int index = face->vtx[j];
Point3 p (maxMesh.P(index));
dVector v (matrix.TransformVector(dVector (p.x, p.y, p.z, 0.0f)));
polygon[j][0] = v.m_x;
polygon[j][1] = v.m_y;
polygon[j][2] = v.m_z;
}
NewtonMeshAddFace(meshOut, face->deg, &polygon[0][0], 12 * sizeof (float), 0);
}
}
}
if (needDel) {
delete poly;
}
}
dVector multiplicacaoN1(const dMatrix &m1, const dVector &v2)
{
dVector vetor (m1[0].size());
for (int i = 0; i < m1.size(); ++i)
for (int j = 0; j < m1[0].size(); ++j)
vetor[i] += m1[i][j] * v2[j];
return vetor;
}
void dCustomJoint::dDebugDisplay::DrawFrame(const dMatrix& matrix)
{
dVector o0(matrix.m_posit);
dFloat size = 0.25f;
dVector x(matrix.m_posit + matrix.RotateVector(dVector(size, 0.0f, 0.0f, 0.0f)));
SetColor(dVector (1.0f, 0.0f, 0.0f));
DrawLine (matrix.m_posit, x);
dVector y(matrix.m_posit + matrix.RotateVector(dVector(0.0f, size, 0.0f, 0.0f)));
SetColor(dVector (0.0f, 1.0f, 0.0f));
DrawLine (matrix.m_posit, y);
dVector z(matrix.m_posit + matrix.RotateVector(dVector(0.0f, 0.0f, size, 0.0f)));
SetColor(dVector (0.0f, 0.0f, 1.0f));
DrawLine (matrix.m_posit, z);
}
dBoundingBox ParticlePrimitive::GetBoundingBox(const dMatrix &space)
{
dBoundingBox box;
for (vector<dVector,FLX_ALLOC(dVector) >::iterator i=m_VertData->begin(); i!=m_VertData->end(); ++i)
{
box.expand(space.transform(*i));
}
return box;
}
dQuaternion::dQuaternion (const dMatrix &matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);
if (trace > dFloat(0.0f)) {
trace = dSqrt (trace + dFloat(1.0f));
m_q0 = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dSqrt (trace);
dFloat* const ptr = &m_q1;
ptr[i] = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#if _DEBUG
dMatrix tmp (*this, matrix.m_posit);
dMatrix unitMatrix (tmp * matrix.Inverse());
for (int i = 0; i < 4; i ++) {
dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
dAssert (err < dFloat (1.0e-3f));
}
dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
dAssert (err < dFloat(1.0e-3f));
#endif
}
void GLSLShader::SetMatrix(const string &name, dMatrix &m)
{
#ifdef GLSL
if (!m_Enabled) return;
GLuint param = glGetUniformLocation(m_Program, name.c_str());
glUniformMatrix4fv(param, 1, GL_FALSE, m.arr());
#endif
}
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
for (int i = 0; i < 3; i ++) {
dVector support(0.0f);
dVector dir (0.0f);
dir[i] = 1.0f;
dVector localDir (matrix.UnrotateVector (dir));
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
maxP[i] = support[i];
localDir = localDir.Scale (-1.0f);
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
minP[i] = support[i];
}
}
CustomLimitBallAndSocket::CustomLimitBallAndSocket(const dMatrix& childPinAndPivotFrame, NewtonBody* const child, const dMatrix& parentPinAndPivotFrame, NewtonBody* const parent)
:CustomBallAndSocket(childPinAndPivotFrame, child, parent)
,m_rotationOffset(childPinAndPivotFrame * parentPinAndPivotFrame.Inverse())
{
SetConeAngle (0.0f);
SetTwistAngle (0.0f, 0.0f);
dMatrix matrix;
CalculateLocalMatrix (parentPinAndPivotFrame, matrix, m_localMatrix1);
}
dBoundingBox PixelPrimitive::GetBoundingBox(const dMatrix &space)
{
dBoundingBox box;
for (vector<dVector,FLX_ALLOC(dVector) >::iterator i=m_Points.begin(); i!=m_Points.end(); ++i)
{
box.expand(space.transform(*i));
}
return box;
}
dMatrix::dMatrix (const dMatrix & transformMatrix, const dVector & scale, const dMatrix & stretchAxis)
{
dMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale (scale[0]);
scaledAxis[1] = stretchAxis[1].Scale (scale[1]);
scaledAxis[2] = stretchAxis[2].Scale (scale[2]);
scaledAxis[3] = stretchAxis[3];
*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
}
dBoundingBox NURBSPrimitive::GetBoundingBox(const dMatrix &space)
{
dBoundingBox box;
for (vector<dVector>::iterator i=m_CVVec->begin(); i!=m_CVVec->end(); ++i)
{
box.expand(space.transform(*i));
}
return box;
}
void segment::segment_sequence(const dMatrix src_M, dMatrix& dst_M,
Beliefs E, double c, int *num_ccs, int **sizes)
{
int i, j, t, seqLength, dimH;
seqLength = src_M.getWidth();
dimH = E.belStates[0].getLength();
// Build a graph
std::vector<edge> edges;
for( t=0; t<seqLength-1; t++ ) {
edge e(t,t+1,diff_unary(E,t,t+1,dimH));
edges.push_back(e);
}
// segment
universe u(seqLength); // make a disjoint-set forest
segment_graph(u, seqLength, seqLength-1, edges, c);
*num_ccs = u.num_sets();
int *elt_labels, *seg_sizes, **seg_elts;
elt_labels = new int[seqLength];
seg_sizes = new int[u.num_sets()];
seg_elts = new int*[u.num_sets()];
u.get_segments(seqLength, elt_labels, seg_sizes, seg_elts);
for(i=0; i<u.num_sets(); i++) printf("[%d]", seg_sizes[i]); printf("\n"); //getchar();
if( sizes ) {
delete[] *sizes; *sizes = new int[u.num_sets()];
memcpy(*sizes,seg_sizes,u.num_sets()*sizeof(int));
}
// Generate observations for the new layer
dst_M.resize(u.num_sets(), src_M.getHeight());
for( i=0; i<src_M.getHeight(); i++ ) for( j=0; j<src_M.getWidth(); j++ )
dst_M.addValue(i,elt_labels[j],src_M.getValue(i,j));
for( i=0; i<dst_M.getHeight(); i++ ) for( j=0; j<dst_M.getWidth(); j++ )
dst_M(i,j) = dst_M(i,j) / seg_sizes[j];
// Clean up and return
delete[] elt_labels; elt_labels = 0;
delete[] seg_sizes; seg_sizes = 0;
for(i=0; i<u.num_sets(); i++) {
delete[] seg_elts[i];
seg_elts[i] = 0;
}
delete[] seg_elts; seg_elts = 0;
}
void dCollisionBoxNodeInfo::BakeTransform (const dMatrix& transform)
{
dCollisionNodeInfo::BakeTransform (transform);
dVector scale;
dMatrix stretchAxis;
dMatrix transformMatrix;
transform.PolarDecomposition (transformMatrix, scale, stretchAxis);
m_size = m_size * scale;
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
rollPin = matrix1.RotateVector(rollPin);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(euler0.m_x);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
dFloat skinThickness = NewtonCollisionGetSkinThickness (collision) * 0.125f;
for (int i = 0; i < 3; i ++) {
dVector support;
dVector dir (0.0f, 0.0f, 0.0f, 0.0f);
dir[i] = 1.0f;
dVector localDir (matrix.UnrotateVector (dir));
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
maxP[i] = support[i] - skinThickness;
localDir = localDir.Scale (-1.0f);
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
minP[i] = support[i] + skinThickness;
}
}
void segment::segment_sequence(const dMatrix src_M, dMatrix& dst_M,
std::vector<Beliefs> E, double c, int *num_ccs, int**labels)
{
int i, j, t, seqLength, dimH, dimY;
seqLength = src_M.getWidth();
dimH = E[0].belStates[0].getLength();
dimY = (int)E.size();
std::vector<Beliefs> logE; getLogE(E, logE);
// Build a graph
std::vector<edge> edges;
for( t=0; t<seqLength-1; t++ ) {
edge e(t,t+1,diff_unary(E,t,t+1,dimH,dimY));
edges.push_back(e);
}
// segment
universe u(seqLength); // make a disjoint-set forest
segment_graph(u, seqLength, seqLength-1, edges, c);
*num_ccs = u.num_sets();
int *elt_labels, *seg_sizes, **seg_elts;
elt_labels = new int[seqLength];
seg_sizes = new int[u.num_sets()];
seg_elts = new int*[u.num_sets()];
u.get_segments(seqLength, elt_labels, seg_sizes, seg_elts);
// Optionally, save elt_labels to labels
if( labels!=0 )
memcpy(*labels, elt_labels, sizeof(int)*seqLength);
// Generate observations for the new layer
dst_M.resize(u.num_sets(), src_M.getHeight());
for( i=0; i<src_M.getHeight(); i++ ) for( j=0; j<src_M.getWidth(); j++ )
dst_M.addValue(i,elt_labels[j],src_M.getValue(i,j));
for( i=0; i<dst_M.getHeight(); i++ ) for( j=0; j<dst_M.getWidth(); j++ )
dst_M(i,j) = dst_M(i,j) / seg_sizes[j];
// Clean up and return
delete[] elt_labels; elt_labels = 0;
delete[] seg_sizes; seg_sizes = 0;
for(i=0; i<u.num_sets(); i++) {
delete[] seg_elts[i];
seg_elts[i] = 0;
}
delete[] seg_elts; seg_elts = 0;
}
void dCollisionConeNodeInfo::BakeTransform (const dMatrix& transform)
{
dCollisionNodeInfo::BakeTransform (transform);
dVector scale;
dMatrix stretchAxis;
dMatrix transformMatrix;
transform.PolarDecomposition (transformMatrix, scale, stretchAxis);
m_radius *= scale.m_y;
m_height *= scale.m_x;
}
void dLineNodeInfo::BakeTransform (const dMatrix& transform)
{
dVector scale;
dMatrix stretchMatrix;
dMatrix matrix (transform.Inverse4x4() * m_matrix * transform);
matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
matrix = transform * dMatrix (dGetIdentityMatrix(), scale, stretchMatrix);
matrix.TransformTriplex (&m_curve.GetControlPointArray()[0].m_x, sizeof (dVector), &m_curve.GetControlPointArray()[0].m_x, sizeof (dVector), m_curve.GetControlPointCount());
}
//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const
void dMatrix::PolarDecomposition (dMatrix & transformMatrix, dVector & scale, dMatrix & stretchAxis, const dMatrix & initialStretchAxis) const
{
// a polar decomposition decompose matrix A = O * S
// where S = sqrt (transpose (L) * L)
// calculate transpose (L) * L
dMatrix LL ((*this) * Transpose());
// check is this si a pure uniformScale * rotation * translation
dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f);
dFloat invdet2 = 1.0f / det2;
dMatrix pureRotation (LL);
pureRotation[0] = pureRotation[0].Scale (invdet2);
pureRotation[1] = pureRotation[1].Scale (invdet2);
pureRotation[2] = pureRotation[2].Scale (invdet2);
//dFloat soureSign = ((*this)[0] * (*this)[1]) % (*this)[2];
dFloat sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dFloat det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
if (dAbs (det - 1.0f) < 1.e-5f)
{
// this is a pure scale * rotation * translation
det = sign * dSqrt (det2);
scale[0] = det;
scale[1] = det;
scale[2] = det;
scale[3] = 1.0f;
det = 1.0f / det;
transformMatrix.m_front = m_front.Scale (det);
transformMatrix.m_up = m_up.Scale (det);
transformMatrix.m_right = m_right.Scale (det);
transformMatrix[0][3] = 0.0f;
transformMatrix[1][3] = 0.0f;
transformMatrix[2][3] = 0.0f;
transformMatrix.m_posit = m_posit;
stretchAxis = dGetIdentityMatrix();
}
else
{
stretchAxis = LL.JacobiDiagonalization (scale, initialStretchAxis);
// I need to deal with buy seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dSqrt (scale[0]);
scale[1] = sign * dSqrt (scale[1]);
scale[2] = sign * dSqrt (scale[2]);
scale[3] = 1.0f;
dMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]);
scaledAxis[3] = stretchAxis[3];
dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
}
void dMeshNodeInfo::BakeTransform (const dMatrix& transform)
{
dVector scale;
dMatrix stretchMatrix;
//dMatrix tmp (m_matrix);
dMatrix matrix (transform.Inverse4x4() * m_matrix * transform);
matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
matrix = transform * dMatrix (dGetIdentityMatrix(), scale, stretchMatrix);
NewtonMeshApplyTransform (m_mesh, &matrix[0][0]);
}
void Madd(const dMatrix& A,const dVector& x,dVector& y,double alpha,double beta,bool transpose)
{
if(A.isRowMajor()) {
Assert(A.jstride == 1);
transpose = !transpose;
}
else {
Assert(A.istride == 1);
//Assert(IsCompliant(A));
}
//Assert(IsCompliant(A));
Assert(x.n == A.n);
Assert(y.n == A.m);
integer m=A.m;
integer n=A.n;
integer lda=A.m;
integer xinc=x.stride;
integer yinc=y.stride;
char trans = (transpose?'T':'N');
dgemv_(&trans,&m,&n,&alpha,A.getStart(),&lda,x.getStart(),&xinc,&beta,y.getStart(),&yinc);
}
