void Synthesizer::computeAllTerms(const DisembodiedObject &object, const ObjectInstance &parent, const mat4f &modelToWorld, const vec3f &contactPos, ObjectScoreEntry &result)
{
computeBaseOffset(object, modelToWorld, contactPos, result);
//
// we don't compute left-right here, we just assume the speakers have already been placed correctly.
//
//computeLeftRight(object, contactPos, result);
computeOverhang(object, parent, modelToWorld, result);
computeInteractionRays(object, modelToWorld, result);
computeScanPlacement(object, modelToWorld, result);
computeCollision(object, modelToWorld, result);
}
void Synthesizer::evaluatePlacementB()
{
updateBaseColumns();
Timer tCollision;
if (verbose) cout << "Evaluating collisions...";
UINT collisionCount = 0, acceptableCount = 0;
for (CandidateObject &o : candidateObjects)
{
for (PlacementLocation &l : o.locations)
{
for (ObjectScoreEntry &r : l.rotations)
{
if (r.acceptable)
{
mat4f modelToWorld = makeModelToWorld(o.object, l, r.rotation);
computeCollision(o.object, modelToWorld, r);
collisionCount++;
if (r.acceptable)
acceptableCount++;
}
}
}
}
if (verbose)
{
cout << tCollision.getElapsedTime() << "s" << endl;
cout << acceptableCount << " / " << collisionCount << " collision free locations" << endl;
}
Timer tScanPlacement;
if(verbose) cout << "Evaluating scan placement...";
for (CandidateObject &o : candidateObjects)
{
for (PlacementLocation &l : o.locations)
{
for (ObjectScoreEntry &r : l.rotations)
{
if (r.acceptable)
{
mat4f modelToWorld = makeModelToWorld(o.object, l, r.rotation);
computeScanPlacement(o.object, modelToWorld, r);
}
}
}
}
if (verbose) cout << tScanPlacement.getElapsedTime() << "s" << endl;
}
std::vector<FlowCollisionResult> FlowCollisionSystem::checkFlowThreadCollisions(FlowThread* flowThread) {
std::vector<std::vector<FlowCollisionResult>> FlowThreadResults;
FlowThreadResults.resize(flowThread->_joints.size());
for (size_t j = 0; j < _allCollisions.size(); j++) {
FlowCollisionSphere &sphere = _allCollisions[j];
FlowCollisionResult rootCollision = sphere.computeSphereCollision(flowThread->_positions[0], flowThread->_radius);
std::vector<FlowCollisionResult> collisionData = { rootCollision };
bool tooFar = rootCollision._distance >(flowThread->_length + rootCollision._radius);
FlowCollisionResult nextCollision;
if (!tooFar) {
if (sphere._isTouch) {
for (size_t i = 1; i < flowThread->_joints.size(); i++) {
auto prevCollision = collisionData[i - 1];
nextCollision = _allCollisions[j].computeSphereCollision(flowThread->_positions[i], flowThread->_radius);
collisionData.push_back(nextCollision);
if (prevCollision._offset > 0.0f) {
if (i == 1) {
FlowThreadResults[i - 1].push_back(prevCollision);
}
} else if (nextCollision._offset > 0.0f) {
FlowThreadResults[i].push_back(nextCollision);
} else {
FlowCollisionResult segmentCollision = _allCollisions[j].checkSegmentCollision(flowThread->_positions[i - 1], flowThread->_positions[i], prevCollision, nextCollision);
if (segmentCollision._offset > 0) {
FlowThreadResults[i - 1].push_back(segmentCollision);
FlowThreadResults[i].push_back(segmentCollision);
}
}
}
} else {
if (rootCollision._offset > 0.0f) {
FlowThreadResults[0].push_back(rootCollision);
}
for (size_t i = 1; i < flowThread->_joints.size(); i++) {
nextCollision = _allCollisions[j].computeSphereCollision(flowThread->_positions[i], flowThread->_radius);
if (nextCollision._offset > 0.0f) {
FlowThreadResults[i].push_back(nextCollision);
}
}
}
}
}
std::vector<FlowCollisionResult> results;
for (size_t i = 0; i < flowThread->_joints.size(); i++) {
results.push_back(computeCollision(FlowThreadResults[i]));
}
return results;
};
void CudaMasterContactSolver<real>::step(const core::ExecParams* params /* PARAMS FIRST */, double dt) {
sofa::helper::AdvancedTimer::stepBegin("AnimationStep");
context = dynamic_cast<simulation::Node *>(this->getContext()); // access to current node
#ifdef DISPLAY_TIME
CTime *timer;
double timeScale = 1.0 / (double)CTime::getRefTicksPerSec();
timer = new CTime();
double time_Free_Motion = (double) timer->getTime();
#endif
// Update the BehaviorModels
// Required to allow the RayPickInteractor interaction
simulation::BehaviorUpdatePositionVisitor updatePos(params /* PARAMS FIRST */, dt);
context->execute(&updatePos);
simulation::MechanicalBeginIntegrationVisitor beginVisitor(params /* PARAMS FIRST */, dt);
context->execute(&beginVisitor);
// Free Motion
simulation::SolveVisitor freeMotion(params /* PARAMS FIRST */, dt, true);
context->execute(&freeMotion);
//simulation::MechanicalPropagateFreePositionVisitor().execute(context);
{
sofa::core::MechanicalParams mparams(*params);
sofa::core::MultiVecCoordId xfree = sofa::core::VecCoordId::freePosition();
mparams.x() = xfree;
simulation::MechanicalPropagatePositionVisitor(&mparams /* PARAMS FIRST */, 0, xfree, true).execute(context);
}
//core::VecId dx_id = (VecId)core::VecDerivId::dx();
//simulation::MechanicalVOpVisitor(params /* PARAMS FIRST */, dx_id).execute( context);
//simulation::MechanicalPropagateDxVisitor(params /* PARAMS FIRST */, dx_id, true).execute( context); //ignore the masks (is it necessary?)
//simulation::MechanicalVOpVisitor(params /* PARAMS FIRST */, dx_id).execute( context);
#ifdef DISPLAY_TIME
time_Free_Motion = ((double) timer->getTime() - time_Free_Motion)*timeScale;
double time_computeCollision = (double) timer->getTime();
#endif
computeCollision();
#ifdef DISPLAY_TIME
time_computeCollision = ((double) timer->getTime() - time_computeCollision)*timeScale;
double time_build_LCP = (double) timer->getTime();
#endif
//MechanicalResetContactForceVisitor().execute(context);
for (unsigned int i=0;i<constraintCorrections.size();i++) {
core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i];
cc->resetContactForce();
}
build_LCP();
#ifdef DISPLAY_TIME
time_build_LCP = ((double) timer->getTime() - time_build_LCP)*timeScale;
double time_solve_LCP = (double) timer->getTime();
#endif
double _tol = tol.getValue();
int _maxIt = maxIt.getValue();
if (! initial_guess.getValue()) _f.clear();
double error = 0.0;
#ifdef CHECK
real t1,t2;
if (_mu > 0.0) {
f_check.resize(_numConstraints,MBSIZE);
for (unsigned i=0;i<_numConstraints;i++) f_check[i] = _f[i];
real toln = ((int) (_realNumConstraints/3) + 1) * (real)_tol;
t2 = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), f_check.getCudaVector(), _mu,_toln, _maxIt);
t1 = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(0,_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,_toln, _maxIt);
} else {
f_check.resize(_numConstraints);
for (unsigned i=0;i<_numConstraints;i++) f_check[i] = _f[i];
t2 = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(useGPU_d.getValue(),_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), f_check.getCudaVector(), _tol, _maxIt);
t1 = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(0,_numConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _tol, _maxIt);
}
for (unsigned i=0;i<f_check.size();i++) {
if ((f_check.element(i)-_f.element(i)>CHECK) || (f_check.element(i)-_f.element(i)<-CHECK)) {
std::cerr << "Error(" << useGPU_d.getValue() << ") dim(" << _numConstraints << ") realDim(" << _realNumConstraints << ") elmt(" << i << ") : (cpu," << f_check.element(i) << ") (gpu,(" << _f.element(i) << ")" << std::endl;
}
}
#else
sout << "numcontacts = " << _realNumConstraints << " RealNumContacts =" << _numConstraints << sendl;
if (_mu > 0.0) {
real toln = ((int) (_realNumConstraints/3) + 1) * (real)_tol;
error = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,toln, _maxIt);
// printf("\nFE = [");
// for (unsigned j=0;j<_numConstraints;j++) {
// printf("%f ",_f.element(j));
// }
// printf("]\n");
// real toln = ((int) (_numConstraints/3) + 1) * (real)_tol;
// error = sofa::gpu::cuda::CudaLCP<real>::CudaNlcp_gaussseidel(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), _mu,toln, _maxIt);
if (this->f_printLog.getValue()) {
printf("M = [\n");
for (unsigned j=0;j<_numConstraints;j++) {
for (unsigned i=0;i<_numConstraints;i++) {
printf("%f\t",_W.element(i,j));
}
printf("\n");
}
printf("]\n");
printf("q = [");
for (unsigned j=0;j<_numConstraints;j++) {
printf("%f\t",_dFree.element(j));
}
printf("]\n");
printf("FS = [");
for (unsigned j=0;j<_numConstraints;j++) {
printf("%f ",_f.element(j));
}
printf("]\n");
}
} else {
error = sofa::gpu::cuda::CudaLCP<real>::CudaGaussSeidelLCP1(useGPU_d.getValue(),_realNumConstraints, _dFree.getCudaVector(), _W.getCudaMatrix(), _f.getCudaVector(), (real)_tol, _maxIt);
}
#endif
if (error > _tol) sout << "No convergence in gaussSeidelLCP1 : error = " << error << sendl;
#ifdef DISPLAY_TIME
time_solve_LCP = ((double) timer->getTime() - time_solve_LCP)*timeScale;
double time_contactCorrections = (double) timer->getTime();
#endif
if (initial_guess.getValue()) keepContactForcesValue();
// MechanicalApplyContactForceVisitor(_result).execute(context);
for (unsigned int i=0;i<constraintCorrections.size();i++) {
core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i];
cc->applyContactForce(&_f);
}
core::MechanicalParams mparams(*params);
simulation::MechanicalPropagateAndAddDxVisitor(&mparams).execute(context);
//simulation::MechanicalPropagatePositionAndVelocityVisitor().execute(context);
//simulation::MechanicalPropagateAndAddDxVisitor().execute( context);
//MechanicalResetContactForceVisitor().execute(context);
for (unsigned int i=0;i<constraintCorrections.size();i++)
{
core::behavior::BaseConstraintCorrection* cc = constraintCorrections[i];
cc->resetContactForce();
}
#ifdef DISPLAY_TIME
time_contactCorrections = ((double) timer->getTime() - time_contactCorrections)*timeScale;
#endif
#ifdef DISPLAY_TIME
double total = time_Free_Motion + time_computeCollision + time_build_LCP + time_solve_LCP + time_contactCorrections;
if (this->print_info.getValue()) {
sout<<"********* Start Iteration : " << _numConstraints << " contacts *********" <<sendl;
sout<<"Free Motion\t" << time_Free_Motion <<" s \t| " << time_Free_Motion*100.0/total << "%" <<sendl;
sout<<"ComputeCollision\t" << time_computeCollision <<" s \t| " << time_computeCollision*100.0/total << "%" <<sendl;
sout<<"Build_LCP\t" << time_build_LCP <<" s \t| " << time_build_LCP*100.0/total << "%" <<sendl;
sout<<"Solve_LCP\t" << time_solve_LCP <<" s \t| " << time_solve_LCP*100.0/total << "%" <<sendl;
sout<<"ContactCorrections\t" << time_contactCorrections <<" s \t| " << time_contactCorrections*100.0/total << "%" <<sendl;
unsigned nbNnul = 0;
unsigned sz = _realNumConstraints * _realNumConstraints;
for (unsigned j=0;j<sz;j++) if (_W.getCudaMatrix().hostRead()[j]==0.0) nbNnul++;
sout<<"Sparsity = " << ((double) (((double) nbNnul * 100.0) / ((double)sz))) <<"%" <<sendl;
}
#endif
simulation::MechanicalEndIntegrationVisitor endVisitor(params /* PARAMS FIRST */, dt);
context->execute(&endVisitor);
sofa::helper::AdvancedTimer::stepEnd("AnimationStep");
}
void Ivy::grow()
{
//parameters that depend on the scene object bounding sphere
float local_ivySize = Common::mesh.boundingSphereRadius * ivySize;
float local_maxFloatLength = Common::mesh.boundingSphereRadius * maxFloatLength;
//normalize weights of influence
float sum = primaryWeight + randomWeight + adhesionWeight;
primaryWeight /= sum;
randomWeight /= sum;
adhesionWeight /= sum;
//lets grow
for (std::vector<IvyRoot>::iterator root = roots.begin(); root != roots.end(); ++root)
{
//process only roots that are alive
if (!root->alive) continue;
//let the ivy die, if the maximum float length is reached
if (root->nodes.back().floatingLength > local_maxFloatLength) root->alive = false;
//grow vectors: primary direction, random influence, and adhesion of scene objectss
//primary vector = weighted sum of previous grow vectors
Vector3d primaryVector = root->nodes.back().primaryDir;
//random influence plus a little upright vector
Vector3d randomVector = Vector3d::getNormalized( Vector3d::getRandomized() + Vector3d(0.0f, 0.2f, 0.0f) );
//adhesion influence to the nearest triangle = weighted sum of previous adhesion vectors
Vector3d adhesionVector = computeAdhesion(root->nodes.back().pos);
//compute grow vector
Vector3d growVector = local_ivySize * (primaryVector * primaryWeight + randomVector * randomWeight + adhesionVector * adhesionWeight);
//gravity influence
//compute gravity vector
Vector3d gravityVector = local_ivySize * Vector3d(0.0f, -1.0f, 0.0f) * gravityWeight;
//gravity depends on the floating length
gravityVector *= pow(root->nodes.back().floatingLength / local_maxFloatLength, 0.7f);
//next possible ivy node
//climbing state of that ivy node, will be set during collision detection
bool climbing;
//compute position of next ivy node
Vector3d newPos = root->nodes.back().pos + growVector + gravityVector;
//combine alive state with result of the collision detection, e.g. let the ivy die in case of a collision detection problem
root->alive = root->alive & computeCollision(root->nodes.back().pos, newPos, climbing);
//update grow vector due to a changed newPos
growVector = newPos - root->nodes.back().pos - gravityVector;
//create next ivy node
IvyNode tmpNode;
tmpNode.pos = newPos;
tmpNode.primaryDir = Vector3d::getNormalized( 0.5f * root->nodes.back().primaryDir + 0.5f * Vector3d::getNormalized( growVector ) );
tmpNode.adhesionVector = adhesionVector;
tmpNode.length = root->nodes.back().length + Vector3d::getLength(newPos - root->nodes.back().pos);
tmpNode.floatingLength = climbing ? 0.0f : root->nodes.back().floatingLength + Vector3d::getLength(newPos - root->nodes.back().pos);
tmpNode.climb = climbing;
root->nodes.push_back( tmpNode );
}
//lets produce child ivys
for (std::vector<IvyRoot>::iterator root = roots.begin(); root != roots.end(); ++root)
{
//process only roots that are alive
if (!root->alive) continue;
//process only roots up to hierarchy level 3, results in a maximum hierarchy level of 4
if (root->parents > 3) continue;
//add child ivys on existing ivy nodes
for (std::vector<IvyNode>::iterator node = root->nodes.begin(); node != root->nodes.end(); ++node)
{
//weight depending on ratio of node length to total length
float weight = 1.0f - ( cos( node->length / root->nodes.back().length * 2.0f * PI) * 0.5f + 0.5f );
//random influence
float probability = rand()/(float)RAND_MAX;
if (probability * weight > branchingProbability)
{
//new ivy node
IvyNode tmpNode;
tmpNode.pos = node->pos;
tmpNode.primaryDir = Vector3d(0.0f, 1.0f, 0.0f);
tmpNode.adhesionVector = Vector3d(0.0f, 0.0f, 0.0f);
tmpNode.length = 0.0f;
tmpNode.floatingLength = node->floatingLength;
tmpNode.climb = true;
//new ivy root
IvyRoot tmpRoot;
tmpRoot.nodes.push_back( tmpNode );
tmpRoot.alive = true;
tmpRoot.parents = root->parents + 1;
roots.push_back( tmpRoot );
//limit the branching to only one new root per iteration, so return
return;
}
}
}
}
