bool EmbeddedGMap2::flipBackEdge(Dart d)
{
if(GMap2::flipBackEdge(d))
{
Dart e = phi2(d) ;
if (isOrbitEmbedded<VERTEX>())
{
unsigned int v1Emb = getEmbedding<VERTEX>(beta1(d)) ;
setDartEmbedding<VERTEX>(d, v1Emb) ;
setDartEmbedding<VERTEX>(beta2(d), v1Emb) ;
unsigned int v2Emb = getEmbedding<VERTEX>(beta1(e)) ;
setDartEmbedding<VERTEX>(e, v2Emb) ;
setDartEmbedding<VERTEX>(beta2(e), v2Emb) ;
}
if (isOrbitEmbedded<FACE>())
{
unsigned int f1Emb = getEmbedding<FACE>(d) ;
setDartEmbedding<FACE>(phi1(d), f1Emb) ;
setDartEmbedding<FACE>(beta0(phi1(d)), f1Emb) ;
unsigned int f2Emb = getEmbedding<FACE>(e) ;
setDartEmbedding<FACE>(phi1(e), f2Emb) ;
setDartEmbedding<FACE>(beta0(phi1(e)), f2Emb) ;
}
return true ;
}
return false ;
}
inline void GMap1<MAP_IMPL>::collapseEdge(Dart d)
{
Dart d1 = beta1(d) ;
Dart dd = this->beta0(d) ;
Dart dd1 = beta1(dd) ;
beta1unsew(d) ;
beta1unsew(dd) ;
beta1sew(d1, dd1) ;
this->deleteEdge(d) ;
}
inline void GMap1<MAP_IMPL>::mergeCycles(Dart d, Dart e)
{
assert(!sameCycle(d, e)) ;
Dart d1 = beta1(d) ;
Dart e1 = beta1(e) ;
beta1unsew(d) ;
beta1unsew(e) ;
beta1sew(d, e1) ;
beta1sew(e, d1) ;
}
inline void GMap1<MAP_IMPL>::linkCycles(Dart d, Dart e)
{
assert(d != e && !sameCycle(d, e)) ;
Dart d1 = beta1(d) ;
Dart e1 = beta1(e) ;
Dart dd = this->newEdge() ;
Dart ee = this->newEdge() ;
beta1unsew(d) ;
beta1unsew(e) ;
beta1sew(d, dd) ;
beta1sew(e1, this->beta0(dd)) ;
beta1sew(e, ee) ;
beta1sew(d1, this->beta0(ee)) ;
}
inline void GMap1<MAP_IMPL>::splitCycle(Dart d, Dart e)
{
assert(d != e && sameCycle(d, e)) ;
if(!sameOrientedCycle(d, e))
e = beta1(e) ;
Dart d1 = beta1(d) ;
Dart e1 = beta1(e) ;
beta1unsew(d) ;
beta1unsew(e) ;
beta1sew(d, e1) ;
beta1sew(e, d1) ;
}
Dart EmbeddedGMap3::cutEdge(Dart d)
{
Dart nd = GMap3::cutEdge(d);
if(isOrbitEmbedded<EDGE>())
{
// embed the new darts created in the cut edge
unsigned int eEmb = getEmbedding<EDGE>(d) ;
Dart e = d ;
do
{
setDartEmbedding<EDGE>(beta0(e), eEmb) ;
e = alpha2(e) ;
} while(e != d) ;
// embed a new cell for the new edge and copy the attributes' line (c) Lionel
setOrbitEmbeddingOnNewCell<EDGE>(phi1(d)) ;
copyCell<EDGE>(phi1(d), d) ;
}
if(isOrbitEmbedded<FACE>())
{
Dart f = d;
do
{
unsigned int fEmb = getEmbedding<FACE>(f) ;
setDartEmbedding<FACE>(beta0(f), fEmb);
setDartEmbedding<FACE>(phi1(f), fEmb);
setDartEmbedding<FACE>(phi3(f), fEmb);
setDartEmbedding<FACE>(beta1(phi3(f)), fEmb);
f = alpha2(f);
} while(f != d);
}
if(isOrbitEmbedded<VOLUME>())
{
Dart f = d;
do
{
unsigned int vEmb = getEmbedding<VOLUME>(f) ;
setDartEmbedding<VOLUME>(beta0(f), vEmb);
setDartEmbedding<VOLUME>(phi1(f), vEmb);
setDartEmbedding<VOLUME>(phi2(f), vEmb);
setDartEmbedding<VOLUME>(beta1(phi2(f)), vEmb);
f = alpha2(f);
} while(f != d);
}
return nd ;
}
inline void GMap1<MAP_IMPL>::foreach_dart_of_vertex(Dart d, FUNC& f) const
{
f(d);
Dart d1 = beta1(d);
if (d1 != d)
f(d1);
}
void EmbeddedGMap3::splitVolume(std::vector<Dart>& vd)
{
GMap3::splitVolume(vd);
// follow the edge path a second time to embed the vertex, edge and volume orbits
for(std::vector<Dart>::iterator it = vd.begin() ; it != vd.end() ; ++it)
{
Dart dit = *it;
// embed the vertex embedded from the origin volume to the new darts
if(isOrbitEmbedded<VERTEX>())
{
unsigned int vEmb = getEmbedding<VERTEX>(dit) ;
setDartEmbedding<VERTEX>(beta2(dit), vEmb);
setDartEmbedding<VERTEX>(beta3(beta2(dit)), vEmb);
setDartEmbedding<VERTEX>(beta1(beta2(dit)), vEmb);
setDartEmbedding<VERTEX>(beta3(beta1(beta2(dit))), vEmb);
}
// embed the edge embedded from the origin volume to the new darts
if(isOrbitEmbedded<EDGE>())
{
unsigned int eEmb = getEmbedding<EDGE>(dit) ;
setDartEmbedding<EDGE>(beta2(dit), eEmb);
setDartEmbedding<EDGE>(beta3(beta2(dit)), eEmb);
setDartEmbedding<EDGE>(beta0(beta2(dit)), eEmb);
setDartEmbedding<EDGE>(beta0(beta3(beta2(dit))), eEmb);
}
// embed the volume embedded from the origin volume to the new darts
if(isOrbitEmbedded<VOLUME>())
{
unsigned int vEmb = getEmbedding<VOLUME>(dit) ;
setDartEmbedding<VOLUME>(beta2(dit), vEmb);
setDartEmbedding<VOLUME>(beta0(beta2(dit)), vEmb);
}
}
if(isOrbitEmbedded<VOLUME>())
{
Dart v = vd.front() ;
Dart v23 = alpha2(v) ;
setOrbitEmbeddingOnNewCell<VOLUME>(v23) ;
copyCell<VOLUME>(v23, v) ;
}
}
inline Dart GMap1<MAP_IMPL>::beta(const Dart d) const
{
assert( (N > 0) || !"negative parameters not allowed in template multi-beta");
if (N<10)
{
switch(N)
{
case 0 : return this->beta0(d) ;
case 1 : return beta1(d) ;
default : assert(!"Wrong multi-beta relation value") ;
}
}
switch(N%10)
{
case 0 : return beta0(beta<N/10>(d)) ;
case 1 : return beta1(beta<N/10>(d)) ;
default : assert(!"Wrong multi-beta relation value") ;
}
}
void EmbeddedGMap2::splitFace(Dart d, Dart e)
{
GMap2::splitFace(d, e) ;
if (isOrbitEmbedded<VERTEX>())
{
unsigned int v1Emb = getEmbedding<VERTEX>(d) ;
setDartEmbedding<VERTEX>(phi_1(e), v1Emb) ;
setDartEmbedding<VERTEX>(beta1(d), v1Emb) ;
setDartEmbedding<VERTEX>(beta1(phi_1(e)), v1Emb) ;
unsigned int v2Emb = getEmbedding<VERTEX>(e) ;
setDartEmbedding<VERTEX>(phi_1(d), v2Emb) ;
setDartEmbedding<VERTEX>(beta1(e), v2Emb) ;
setDartEmbedding<VERTEX>(beta1(phi_1(d)), v2Emb) ;
}
if(isOrbitEmbedded<EDGE>())
{
Algo::Topo::initOrbitEmbeddingOnNewCell<EDGE>(*this, beta1(d)) ;
}
if (isOrbitEmbedded<FACE>())
{
unsigned int fEmb = getEmbedding<FACE>(d) ;
setDartEmbedding<FACE>(phi_1(d), fEmb) ;
setDartEmbedding<FACE>(beta1(phi_1(d)), fEmb) ;
Algo::Topo::setOrbitEmbeddingOnNewCell<FACE>(*this, e) ;
Algo::Topo::copyCellAttributes<FACE>(*this, e, d) ;
}
}
void EmbeddedGMap2::splitFace(Dart d, Dart e)
{
GMap2::splitFace(d, e) ;
if (isOrbitEmbedded<VERTEX>())
{
unsigned int v1Emb = getEmbedding<VERTEX>(d) ;
setDartEmbedding<VERTEX>(phi_1(e), v1Emb) ;
setDartEmbedding<VERTEX>(beta1(d), v1Emb) ;
setDartEmbedding<VERTEX>(beta1(phi_1(e)), v1Emb) ;
unsigned int v2Emb = getEmbedding<VERTEX>(e) ;
setDartEmbedding<VERTEX>(phi_1(d), v2Emb) ;
setDartEmbedding<VERTEX>(beta1(e), v2Emb) ;
setDartEmbedding<VERTEX>(beta1(phi_1(d)), v2Emb) ;
}
if(isOrbitEmbedded<EDGE>())
{
initOrbitEmbeddingNewCell<EDGE>(beta1(d)) ;
}
if (isOrbitEmbedded<FACE>())
{
unsigned int fEmb = getEmbedding<FACE>(d) ;
setDartEmbedding<FACE>(phi_1(d), fEmb) ;
setDartEmbedding<FACE>(beta1(phi_1(d)), fEmb) ;
setOrbitEmbeddingOnNewCell<FACE>(e) ;
copyCell<FACE>(e, d) ;
}
}
inline bool GMap1<MAP_IMPL>::sameCycle(Dart d, Dart e) const
{
Dart it = d ;
do
{
if (it == e)
return true ;
it = this->beta0(it);
if (it == e)
return true ;
it = beta1(it) ;
} while (it != d) ;
return false ;
}
unsigned int EmbeddedGMap3::closeHole(Dart d, bool forboundary)
{
unsigned int nbF = GMap3::closeHole(d, forboundary) ;
DartMarkerStore mark(*this); // Lock a marker
std::vector<Dart> visitedFaces; // Faces that are traversed
visitedFaces.reserve(1024) ;
visitedFaces.push_back(beta3(d));// Start with the face of d
mark.markOrbit<FACE>(beta3(d)) ;
// For every face added to the list
for(unsigned int i = 0; i < visitedFaces.size(); ++i)
{
Dart f = visitedFaces[i] ;
do
{
if(isOrbitEmbedded<VERTEX>())
{
unsigned int vEmb = getEmbedding<VERTEX>(beta3(f)) ;
setDartEmbedding<VERTEX>(f, vEmb) ;
setDartEmbedding<VERTEX>(beta1(f), vEmb) ;
}
if(isOrbitEmbedded<EDGE>())
{
unsigned int eEmb = getEmbedding<EDGE>(beta3(f)) ;
setDartEmbedding<EDGE>(f, eEmb) ;
setDartEmbedding<EDGE>(beta0(f), eEmb) ;
}
if(isOrbitEmbedded<FACE>())
{
unsigned int fEmb = getEmbedding<FACE>(beta3(f)) ;
setDartEmbedding<FACE>(f, fEmb) ;
setDartEmbedding<FACE>(beta0(f), fEmb) ;
}
Dart adj = beta2(f); // Get adjacent face
if (!mark.isMarked(adj))
{
visitedFaces.push_back(adj); // Add it
mark.markOrbit<FACE>(adj) ;
}
f = phi1(f) ;
} while(f != visitedFaces[i]) ;
}
return nbF ;
}
RcppExport SEXP nsem3b(SEXP data,
SEXP theta,
SEXP Sigma,
SEXP modelpar,
SEXP control
) {
// srand ( time(NULL) ); /* initialize random seed: */
Rcpp::NumericVector Theta(theta);
Rcpp::NumericMatrix D(data);
unsigned nobs = D.nrow(), k = D.ncol();
mat Data(D.begin(), nobs, k, false); // Avoid copying
Rcpp::NumericMatrix V(Sigma);
mat S(V.begin(), V.nrow(), V.ncol());
S(0,0) = 1;
mat iS = inv(S);
double detS = det(S);
Rcpp::List Modelpar(modelpar);
// Rcpp::IntegerVector _nlatent = Modelpar["nlatent"]; unsigned nlatent = _nlatent[0];
Rcpp::IntegerVector _ny0 = Modelpar["nvar0"]; unsigned ny0 = _ny0[0];
Rcpp::IntegerVector _ny1 = Modelpar["nvar1"]; unsigned ny1 = _ny1[0];
Rcpp::IntegerVector _ny2 = Modelpar["nvar2"]; unsigned ny2 = _ny2[0];
Rcpp::IntegerVector _npred0 = Modelpar["npred0"]; unsigned npred0 = _npred0[0];
Rcpp::IntegerVector _npred1 = Modelpar["npred1"]; unsigned npred1 = _npred1[0];
Rcpp::IntegerVector _npred2 = Modelpar["npred2"]; unsigned npred2 = _npred2[0];
Rcpp::List Control(control);
Rcpp::NumericVector _lambda = Control["lambda"]; double lambda = _lambda[0];
Rcpp::NumericVector _niter = Control["niter"]; double niter = _niter[0];
Rcpp::NumericVector _Dtol = Control["Dtol"]; double Dtol = _Dtol[0];
rowvec mu0(ny0), lambda0(ny0);
rowvec mu1(ny1), lambda1(ny1);
rowvec mu2(ny2), lambda2(ny2);
rowvec beta0(npred0);
rowvec beta1(npred1);
rowvec beta2(npred2);
rowvec gamma(2);
rowvec gamma2(2);
unsigned pos=0;
for (unsigned i=0; i<ny0; i++) {
mu0(i) = Theta[pos];
pos++;
}
for (unsigned i=0; i<ny1; i++) {
mu1(i) = Theta[pos];
pos++;
}
for (unsigned i=0; i<ny2; i++) {
mu2(i) = Theta[pos];
pos++;
}
for (unsigned i=0; i<ny0; i++) {
lambda0(i) = Theta[pos];
pos++;
}
lambda1(0) = 1;
for (unsigned i=1; i<ny1; i++) {
lambda1(i) = Theta[pos];
pos++;
}
lambda2(0) = 1;
for (unsigned i=1; i<ny2; i++) {
lambda2(i) = Theta[pos];
pos++;
}
for (unsigned i=0; i<npred0; i++) {
beta0(i) = Theta[pos];
pos++;
}
for (unsigned i=0; i<npred1; i++) {
beta1(i) = Theta[pos];
pos++;
}
for (unsigned i=0; i<npred2; i++) {
beta2(i) = Theta[pos];
pos++;
}
gamma(0) = Theta[pos]; gamma(1) = Theta[pos+1];
gamma2(0) = Theta[pos+2]; gamma2(1) = Theta[pos+3];
// cerr << "mu0=" << mu0 << endl;
// cerr << "mu1=" << mu1 << endl;
// cerr << "mu2=" << mu2 << endl;
// cerr << "lambda0=" << lambda0 << endl;
// cerr << "lambda1=" << lambda1 << endl;
// cerr << "lambda2=" << lambda2 << endl;
// cerr << "beta0=" << beta0 << endl;
// cerr << "beta1=" << beta1 << endl;
// cerr << "beta2=" << beta2 << endl;
// cerr << "gamma=" << gamma << endl;
// cerr << "gamma2=" << gamma2 << endl;
mat lap(nobs,4);
for (unsigned i=0; i<nobs; i++) {
rowvec newlap = laNRb(Data.row(i), iS, detS,
mu0, mu1, mu2,
lambda0, lambda1, lambda2,
beta0,beta1, beta2, gamma, gamma2,
Dtol,niter,lambda);
lap.row(i) = newlap;
}
List res;
res["indiv"] = lap;
res["logLik"] = sum(lap.col(0)) + (3-V.nrow())*log(2.0*datum::pi)*nobs/2;
res["norm0"] = (3-V.nrow())*log(2*datum::pi)/2;
return res;
}
inline Dart GMap1<MAP_IMPL>::alpha1(Dart d) const
{
return beta1(this->beta0(d)) ;
}
bool EmbeddedGMap3::check()
{
bool topo = GMap3::check() ;
if (!topo)
return false ;
CGoGNout << "Check: embedding begin" << CGoGNendl ;
for(Dart d = begin(); d != end(); next(d))
{
if(isOrbitEmbedded<VERTEX>())
{
if( getEmbedding<VERTEX>(d) != getEmbedding<VERTEX>(beta1(d)) ||
getEmbedding<VERTEX>(d) != getEmbedding<VERTEX>(beta2(d)) ||
getEmbedding<VERTEX>(d) != getEmbedding<VERTEX>(beta3(d)) )
{
std::cout << "Embedding Check : different embeddings on vertex" << std::endl ;
return false ;
}
}
if(isOrbitEmbedded<EDGE>())
{
if( getEmbedding<EDGE>(d) != getEmbedding<EDGE>(beta0(d)) ||
getEmbedding<EDGE>(d) != getEmbedding<EDGE>(beta2(d)) ||
getEmbedding<EDGE>(d) != getEmbedding<EDGE>(beta3(d)) )
{
std::cout << "Embedding Check : different embeddings on edge" << std::endl ;
return false ;
}
}
if (isOrbitEmbedded<FACE>())
{
if( getEmbedding<FACE>(d) != getEmbedding<FACE>(beta0(d)) ||
getEmbedding<FACE>(d) != getEmbedding<FACE>(beta1(d)) ||
getEmbedding<FACE>(d) != getEmbedding<FACE>(beta3(d)) )
{
CGoGNout << "Check: different embeddings on face" << CGoGNendl ;
return false ;
}
}
if (isOrbitEmbedded<VOLUME>())
{
if( getEmbedding<VOLUME>(d) != getEmbedding<VOLUME>(beta0(d)) ||
getEmbedding<VOLUME>(d) != getEmbedding<VOLUME>(beta1(d)) ||
getEmbedding<VOLUME>(d) != getEmbedding<VOLUME>(beta2(d)) )
{
CGoGNout << "Check: different embeddings on volume" << CGoGNendl ;
return false ;
}
}
}
CGoGNout << "Check: embedding ok" << CGoGNendl ;
std::cout << "nb vertex orbits : " << getNbOrbits<VERTEX>() << std::endl ;
std::cout << "nb vertex cells : " << m_attribs[VERTEX].size() << std::endl ;
std::cout << "nb edge orbits : " << getNbOrbits<EDGE>() << std::endl ;
std::cout << "nb edge cells : " << m_attribs[EDGE].size() << std::endl ;
std::cout << "nb face orbits : " << getNbOrbits<FACE>() << std::endl ;
std::cout << "nb face cells : " << m_attribs[FACE].size() << std::endl ;
std::cout << "nb volume orbits : " << getNbOrbits<VOLUME>() << std::endl ;
std::cout << "nb volume cells : " << m_attribs[VOLUME].size() << std::endl ;
return true ;
}
inline Dart GMap1<MAP_IMPL>::alpha_1(Dart d) const
{
return beta0(beta1(d)) ;
}
void EmbeddedGMap3::splitFace(Dart d, Dart e)
{
Dart dd = beta1(beta3(d));
Dart ee = beta1(beta3(e));
GMap3::splitFace(d, e);
if(isOrbitEmbedded<VERTEX>())
{
unsigned int vEmb1 = getEmbedding<VERTEX>(d) ;
unsigned int vEmb2 = getEmbedding<VERTEX>(e) ;
setDartEmbedding<VERTEX>(beta1(d), vEmb1);
setDartEmbedding<VERTEX>(beta2(beta1(d)), vEmb1);
setDartEmbedding<VERTEX>(beta1(beta2(beta1(d))), vEmb1);
setDartEmbedding<VERTEX>(beta1(dd), vEmb1);
setDartEmbedding<VERTEX>(beta2(beta1(dd)), vEmb1);
setDartEmbedding<VERTEX>(beta1(beta2(beta1(dd))), vEmb1);
setDartEmbedding<VERTEX>(beta1(e), vEmb2);
setDartEmbedding<VERTEX>(beta2(beta1(e)), vEmb2);
setDartEmbedding<VERTEX>(beta1(beta2(beta1(e))), vEmb2);
setDartEmbedding<VERTEX>(beta1(ee), vEmb2);
setDartEmbedding<VERTEX>(beta2(beta1(ee)), vEmb2);
setDartEmbedding<VERTEX>(beta1(beta2(beta1(ee))), vEmb2);
}
if(isOrbitEmbedded<EDGE>())
{
setOrbitEmbedding<EDGE>(beta1(d), getEmbedding<EDGE>(beta1(d))) ;
setOrbitEmbedding<EDGE>(beta1(e), getEmbedding<EDGE>(beta1(e))) ;
setOrbitEmbedding<EDGE>(d, getEmbedding<EDGE>(d)) ;
setOrbitEmbedding<EDGE>(e, getEmbedding<EDGE>(e)) ;
setOrbitEmbedding<EDGE>(beta1(beta2(beta1(d))), getEmbedding<EDGE>(beta1(beta2(beta1(d))))) ;
setOrbitEmbedding<EDGE>(beta1(beta2(beta1(e))), getEmbedding<EDGE>(beta1(beta2(beta1(e))))) ;
}
if(isOrbitEmbedded<FACE>())
{
unsigned int fEmb = getEmbedding<FACE>(d) ;
setDartEmbedding<FACE>(beta1(d), fEmb) ;
setDartEmbedding<FACE>(beta0(beta1(d)), fEmb) ;
setDartEmbedding<FACE>(beta1(beta0(beta1(d))), fEmb) ;
setDartEmbedding<FACE>(beta1(ee), fEmb) ;
setDartEmbedding<FACE>(beta0(beta1(ee)), fEmb) ;
setDartEmbedding<FACE>(beta1(beta0(beta1(ee))), fEmb) ;
setOrbitEmbeddingOnNewCell<FACE>(e);
copyCell<FACE>(e, d);
}
if(isOrbitEmbedded<VOLUME>())
{
unsigned int vEmb1 = getEmbedding<VOLUME>(d) ;
setDartEmbedding<VOLUME>(beta1(d), vEmb1);
setDartEmbedding<VOLUME>(beta0(beta1(d)), vEmb1);
setDartEmbedding<VOLUME>(beta1(beta0(beta1(d))), vEmb1) ;
setDartEmbedding<VOLUME>(beta1(e), vEmb1);
setDartEmbedding<VOLUME>(beta0(beta1(e)), vEmb1);
setDartEmbedding<VOLUME>(beta1(beta0(beta1(e))), vEmb1) ;
unsigned int vEmb2 = getEmbedding<VOLUME>(dd) ;
setDartEmbedding<VOLUME>(beta1(dd), vEmb2);
setDartEmbedding<VOLUME>(beta0(beta1(dd)), vEmb2);
setDartEmbedding<VOLUME>(beta1(beta0(beta1(dd))), vEmb2) ;
setDartEmbedding<VOLUME>(beta1(ee), vEmb2);
setDartEmbedding<VOLUME>(beta0(beta1(ee)), vEmb2);
setDartEmbedding<VOLUME>(beta1(beta0(beta1(ee))), vEmb2) ;
}
}
inline Dart GMap1<MAP_IMPL>::phi_1(Dart d) const
{
return this->beta0(beta1(d)) ;
}
inline Dart GMap2::alpha_1(Dart d) const
{
return beta1(beta2(d)) ;
}
inline bool GMap2::foreach_dart_of_vertex(Dart d, FunctorType& f, unsigned int thread) const
{
return GMap2::foreach_dart_of_oriented_vertex(d, f, thread) || GMap2::foreach_dart_of_oriented_vertex(beta1(d), f, thread) ;
}
inline Dart GMap2::alpha1(Dart d)
{
return beta2(beta1(d)) ;
}
void dgCollisionDeformableSolidMesh::ResolvePositionsConstraints (dgFloat32 timestep)
{
dgAssert (m_myBody);
dgInt32 strideInBytes = sizeof (dgVector) * m_clustersCount + sizeof (dgMatrix) * m_clustersCount + sizeof (dgMatrix) * m_particles.m_count;
m_world->m_solverMatrixMemory.ExpandCapacityIfNeessesary (1, strideInBytes);
dgVector* const regionCom = (dgVector*)&m_world->m_solverMatrixMemory[0];
dgMatrix* const sumQiPi = (dgMatrix*) ®ionCom[m_clustersCount];
dgMatrix* const covarianceMatrix = (dgMatrix*) &sumQiPi[m_clustersCount];
const dgFloat32* const masses = m_particles.m_unitMass;
dgVector zero (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
regionCom[i] = zero;
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (masses[i]);
const dgVector& r = m_posit[i];
const dgVector& r0 = m_shapePosit[i];
dgVector mr (r.Scale4(masses[i]));
covarianceMatrix[i] = dgMatrix (r0, mr);
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
regionCom[index] += mr;
}
}
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
dgVector mcr (regionCom[i]);
regionCom[i] = mcr.Scale4 (dgFloat32 (1.0f) / m_clusterMass[i]);
const dgVector& cr0 = m_clusterCom0[i];
sumQiPi[i] = dgMatrix (cr0, mcr.CompProduct4(dgVector::m_negOne));
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
const dgMatrix& covariance = covarianceMatrix[i];
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
dgMatrix& QiPi = sumQiPi[index];
QiPi.m_front += covariance.m_front;
QiPi.m_up += covariance.m_up;
QiPi.m_right += covariance.m_right;
}
}
dgVector beta0 (dgFloat32 (0.93f));
//dgVector beta0 (dgFloat32 (0.0f));
dgVector beta1 (dgVector::m_one - beta0);
dgFloat32 stiffness = dgFloat32 (0.3f);
for (dgInt32 i = 0; i < m_clustersCount; i ++) {
dgMatrix& QiPi = sumQiPi[i];
dgMatrix S (QiPi * QiPi.Transpose4X4());
dgVector eigenValues;
S.EigenVectors (eigenValues, m_clusterRotationInitialGuess[i]);
m_clusterRotationInitialGuess[i] = S;
#ifdef _DEBUG_____
dgMatrix P0 (QiPi * QiPi.Transpose4X4());
dgMatrix D (dgGetIdentityMatrix());
D[0][0] = eigenValues[0];
D[1][1] = eigenValues[1];
D[2][2] = eigenValues[2];
dgMatrix P1 (S.Transpose4X4() * D * S);
dgAssert (P0.TestSymetric3x3());
dgAssert (P1.TestSymetric3x3());
dgMatrix xx (P1 * P0.Symetric3by3Inverse());
dgAssert (dgAbsf (xx[0][0] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[1][1] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[2][2] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[0][1]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[0][2]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[1][0]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[1][2]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[2][0]) < dgFloat32 (1.0e-3f));
dgAssert (dgAbsf (xx[2][1]) < dgFloat32 (1.0e-3f));
#endif
eigenValues = eigenValues.InvSqrt();
dgMatrix m;
m.m_front = S.m_front.CompProduct4(eigenValues.BroadcastX());
m.m_up = S.m_up.CompProduct4(eigenValues.BroadcastY());
m.m_right = S.m_right.CompProduct4(eigenValues.BroadcastZ());
m.m_posit = dgVector::m_wOne;
dgMatrix invS (S.Transpose4X4() * m);
dgMatrix R (invS * QiPi);
dgMatrix A (m_clusterAqqInv[i] * QiPi);
QiPi.m_front = A.m_front.CompProduct4(beta0) + R.m_front.CompProduct4(beta1);
QiPi.m_up = A.m_up.CompProduct4(beta0) + R.m_up.CompProduct4(beta1);
QiPi.m_right = A.m_right.CompProduct4(beta0) + R.m_right.CompProduct4(beta1);
}
dgVector invTimeScale (stiffness / timestep);
dgVector* const veloc = m_particles.m_veloc;
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
dgVector v (dgFloat32 (0.0f));
const dgVector& p = m_posit[i];
const dgVector& p0 = m_shapePosit[i];
for (dgInt32 j = 0; j < count; j ++) {
dgInt32 index = m_clusterPosit[start + j];
const dgMatrix& matrix = sumQiPi[index];
dgVector gi (matrix.RotateVector(p0 - m_clusterCom0[index]) + regionCom[index]);
v += ((gi - p).CompProduct4(invTimeScale).Scale4 (m_clusterWeight[index]));
}
veloc[i] += v;
}
// resolve collisions here
//for now just a hack a collision plane until I get the engine up an running
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector p (m_basePosit + m_posit[i].m_y);
if (p.m_y < dgFloat32 (0.0f)) {
m_posit[i].m_y = -m_basePosit.m_y;
veloc[i].m_y = dgFloat32 (0.0f);
}
}
dgVector time (timestep);
dgVector minBase(dgFloat32 (1.0e10f));
dgVector minBox (dgFloat32 (1.0e10f));
dgVector maxBox (dgFloat32 (-1.0e10f));
dgMatrix matrix (m_myBody->GetCollision()->GetGlobalMatrix().Inverse());
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_posit[i] += veloc[i].CompProduct4 (time);
m_particles.m_posit[i] = matrix.TransformVector(m_posit[i] + m_basePosit);
minBase = minBase.GetMin (m_posit[i]);
minBox = minBox.GetMin(m_particles.m_posit[i]);
maxBox = maxBox.GetMax(m_particles.m_posit[i]);
}
minBase = minBase.Floor();
dgVector mask ((minBase < dgVector (dgFloat32 (0.0f))) | (minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))));
dgInt32 test = mask.GetSignMask();
if (test & 0x07) {
dgVector offset (((minBase < dgVector (dgFloat32 (0.0f))) & dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE/2))) +
((minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))) & dgVector (dgFloat32 (-DG_SOFTBODY_BASE_SIZE/2))));
m_basePosit -= offset;
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_posit[i] += offset;
}
}
// integrate each particle by the deformation velocity, also calculate the new com
// calculate the new body average velocity
// if (m_myBody->m_matrixUpdate) {
// myBody->m_matrixUpdate (*myBody, myBody->m_matrix, threadIndex);
// }
// the collision changed shape, need to update spatial structure
// UpdateCollision ();
// SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
SetCollisionBBox (minBox, maxBox);
}
inline Dart GMap3::alpha1(Dart d)
{
return beta3(beta1(d)) ;
}
