// Responds to a collision detected between a particle and a half-plane by
// applying an impulse to the velocity of the particle.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// pidx: The index of the half-plane.
// n: The shortest vector between the particle and the half-plane.
// Outputs:
// None.
void SimpleCollisionHandler::respondParticleHalfplane(TwoDScene &scene, int vidx, int pidx, const VectorXs &n)
{
VectorXs nhat = n;
nhat.normalize();
double cfactor = (1.0+getCOR())/2.0;
scene.getV().segment<2>(2*vidx) -= 2*cfactor*scene.getV().segment<2>(2*vidx).dot(nhat)*nhat;
}
void TwoDSceneXMLParser::loadEdges( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
twodscene.clearEdges();
int edge = 0;
for( rapidxml::xml_node<>* nd = node->first_node("edge"); nd; nd = nd->next_sibling("edge") )
{
std::pair<int,int> newedge;
if( nd->first_attribute("i") )
{
std::string attribute(nd->first_attribute("i")->value());
if( !stringutils::extractFromString(attribute,newedge.first) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of i attribute for edge " << edge << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of i attribute for edge " << edge << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("j") )
{
std::string attribute(nd->first_attribute("j")->value());
if( !stringutils::extractFromString(attribute,newedge.second) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of j attribute for edge " << edge << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of j attribute for edge " << edge << ". Exiting." << std::endl;
exit(1);
}
scalar radius = 0.015;
if( nd->first_attribute("radius") )
{
std::string attribute(nd->first_attribute("radius")->value());
if( !stringutils::extractFromString(attribute,radius) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse radius attribute for edge " << edge << ". Value must be scalar. Exiting." << std::endl;
exit(1);
}
}
//std::cout << "Edge: " << edge << " i: " << newedge.first << " j: " << newedge.second << std::endl;
twodscene.insertEdge( newedge, radius );
++edge;
}
}
// Detects whether two particles are overlapping (including the radii of each)
// and approaching.
// If the two particles overlap and are approaching, returns true and sets
// the vector n to be the vector between the first and second particle.
// Inputs:
// scene: The scene data structure. The positions and radii of the particles
// can be obtained from here.
// idx1: The index of the first particle. (Ie, the degrees of freedom
// corresponding to this particle are entries 2*idx1 and 2*idx1+1 in
// scene.getX().
// idx2: The index of the second particle.
// Outputs:
// n: The vector between the two particles.
// Returns true if the two particles overlap and are approaching.
bool SimpleCollisionHandler::detectParticleParticle(TwoDScene &scene, int idx1, int idx2, VectorXs &n)
{
VectorXs x1 = scene.getX().segment<2>(2*idx1);
VectorXs x2 = scene.getX().segment<2>(2*idx2);
n = x2-x1;
if(n.norm() < scene.getRadius(idx1) + scene.getRadius(idx2))
{
double relvel = (scene.getV().segment<2>(2*idx1)-scene.getV().segment<2>(2*idx2)).dot(n);
if(relvel > 0)
return true;
}
return false;
}
void TwoDSceneXMLParser::loadSceneFromXML( const std::string& filename, TwoDScene& twodscene, SceneStepper** scenestepper, scalar& dt, scalar& max_t, scalar& maxfreq, std::vector<renderingutils::Color>& particle_colors, std::vector<renderingutils::Color>& edge_colors, std::vector<renderingutils::ParticlePath>& particle_paths, renderingutils::Color& bgcolor, std::string& description, std::string& scenetag, TwoDimensionalDisplayController& display)
{
assert( *scenestepper == NULL );
//std::cout << "Loading scene: " << filename << std::endl;
// Load the xml document
std::vector<char> xmlchars;
rapidxml::xml_document<> doc;
loadXMLFile( filename, xmlchars, doc );
// Attempt to locate the root node
rapidxml::xml_node<>* node = doc.first_node("scene");
if( node == NULL )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse xml scene file. Failed to locate root <scene> node. Exiting." << std::endl;
exit(1);
}
// TODO: Clear old state
// Camera
loadCameraLocation(node, display);
// Scene
loadParticles( node, twodscene );
loadEdges( node, twodscene );
loadSceneTag( node, scenetag );
// Forces
loadSpringForces( node, twodscene );
loadSimpleGravityForces( node, twodscene );
loadGravitationalForces( node, twodscene );
loadDragDampingForces( node, twodscene );
loadVorexForces( node, twodscene );
// Integrator/solver
loadIntegrator( node, scenestepper, dt );
loadMaxTime( node, max_t );
// UI
loadMaxSimFrequency( node, maxfreq );
// Rendering state
particle_colors.resize(twodscene.getNumParticles(),renderingutils::Color(0.650980392156863,0.294117647058824,0.0));
loadParticleColors( node, particle_colors );
edge_colors.resize(twodscene.getNumEdges(),renderingutils::Color(0.0,0.388235294117647,0.388235294117647));
loadEdgeColors( node, edge_colors );
loadBackgroundColor( node, bgcolor );
loadParticlePaths( node, dt, particle_paths );
std::string description_string;
loadSceneDescriptionString( node, description );
}
// Detects whether a particle and a half-plane are overlapping (including the
// radius of the particle) and are approaching.
// If the two objects overlap and are approaching, returns true and sets the
// vector n to be the shortest vector between the particle and the half-plane.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// pidx: The index of the halfplane. The vectors (px, py) and (nx, ny) can
// be retrieved by calling scene.getHalfplane(pidx).
// Outputs:
// n: The shortest vector between the particle and the half-plane.
// Returns true if the two objects overlap and are approaching.
bool SimpleCollisionHandler::detectParticleHalfplane(TwoDScene &scene, int vidx, int pidx, VectorXs &n)
{
VectorXs x1 = scene.getX().segment<2>(2*vidx);
VectorXs px = scene.getHalfplane(pidx).first;
VectorXs pn = scene.getHalfplane(pidx).second;
pn.normalize();
n = (px-x1).dot(pn)*pn;
if(n.norm() < scene.getRadius(vidx))
{
double relvel = scene.getV().segment<2>(2*vidx).dot(n);
if(relvel > 0)
return true;
}
return false;
}
void TwoDSceneXMLParser::loadDragDampingForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
int forcenum = 0;
for( rapidxml::xml_node<>* nd = node->first_node("dragdamping"); nd; nd = nd->next_sibling("dragdamping") )
{
Vector3s constforce;
constforce.setConstant(std::numeric_limits<scalar>::signaling_NaN());
// Extract the linear damping coefficient
scalar b = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("b") )
{
std::string attribute(nd->first_attribute("b")->value());
if( !stringutils::extractFromString(attribute,b) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of b attribute for dragdamping " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse b attribute for dragdamping " << forcenum << ". Exiting." << std::endl;
exit(1);
}
//std::cout << "x: " << constforce.transpose() << std::endl;
twodscene.insertForce(new DragDampingForce(b));
++forcenum;
}
}
void SimpleCollisionHandler::handleCollisions(TwoDScene &scene, CollisionDetector &detector, const VectorXs &oldpos, VectorXs &oldvel, scalar dt)
{
class SimpleCollisionCallback : public DetectionCallback
{
public:
SimpleCollisionCallback(TwoDScene &scene, SimpleCollisionHandler &handler) : scene(scene), handler(handler) {}
virtual void ParticleParticleCallback(int idx1, int idx2)
{
VectorXs n(2);
if(handler.detectParticleParticle(scene, idx1, idx2, n))
{
handler.addParticleParticleImpulse(idx1, idx2, n, 0);
handler.respondParticleParticle(scene, idx1, idx2, n);
}
}
virtual void ParticleEdgeCallback(int vidx, int eidx)
{
VectorXs n(2);
if(handler.detectParticleEdge(scene, vidx, eidx, n))
{
handler.addParticleEdgeImpulse(vidx, eidx, n, 0);
handler.respondParticleEdge(scene, vidx, eidx, n);
}
}
virtual void ParticleHalfplaneCallback(int vidx, int hidx)
{
VectorXs n(2);
if(handler.detectParticleHalfplane(scene, vidx, hidx, n))
{
handler.addParticleHalfplaneImpulse(vidx, hidx, n, 0);
handler.respondParticleHalfplane(scene, vidx, hidx, n);
}
}
TwoDScene &scene;
SimpleCollisionHandler &handler;
};
SimpleCollisionCallback callback(scene, *this);
detector.performCollisionDetection(scene, scene.getX(), scene.getX(), callback);
}
// Responds to a collision detected between two particles by applying an impulse
// to the velocities of each one.
// You can get the COR of the simulation by calling getCOR().
// Inputs:
// scene: The scene data structure.
// idx1: The index of the first particle.
// idx2: The index of the second particle.
// n: The vector between the first and second particle.
// Outputs:
// None.
void SimpleCollisionHandler::respondParticleParticle(TwoDScene &scene, int idx1, int idx2, const VectorXs &n)
{
const VectorXs &M = scene.getM();
VectorXs &v = scene.getV();
VectorXs nhat = n;
nhat.normalize();
double cfactor = (1.0 + getCOR())/2.0;
double m1 = scene.isFixed(idx1) ? std::numeric_limits<double>::infinity() : M[2*idx1];
double m2 = scene.isFixed(idx2) ? std::numeric_limits<double>::infinity() : M[2*idx2];
double numerator = 2*cfactor * (v.segment<2>(2*idx2) - v.segment<2>(2*idx1) ).dot(nhat);
double denom1 = 1+m1/m2;
double denom2 = m2/m1 + 1;
if(!scene.isFixed(idx1))
v.segment<2>(2*idx1) += numerator/denom1 * nhat;
if(!scene.isFixed(idx2))
v.segment<2>(2*idx2) -= numerator/denom2 * nhat;
}
void TwoDSceneSVGRenderer::renderImpulse( std::fstream &file, const TwoDScene &scene, const CollisionInfo &impulse, bool buggy) const
{
scalar scale, xmin, ymin, xshift, yshift;
computeSimToImageMap(scale, xmin, ymin, xshift, yshift );
std::string color = (buggy ? "#FF0000" : "#00FF00");
assert(impulse.m_idx1 < scene.getNumParticles());
double x = scene.getX()[2*impulse.m_idx1];
double y = scene.getX()[2*impulse.m_idx1+1];
double x2 = x + impulse.m_n[0];
double y2 = y + impulse.m_n[1];
x = scale*(x-xmin)+xshift;
y = m_h-(scale*(y-ymin)+yshift);
x2 = scale*(x2-xmin)+xshift;
y2 = m_h-(scale*(y2-ymin)+yshift);
file << "<line x1=\"" << x << "\" y1=\"" << y << "\" x2=\"" << x2 << "\" y2=\"" << y2 << "\" style=\"stroke:" << color << ";stroke-width:2\"/>" << std::endl;
}
bool LinearizedImplicitEuler::stepScene( TwoDScene& scene, scalar dt )
{
VectorXs& x = scene.getX();
VectorXs& v = scene.getV();
const VectorXs& m = scene.getM();
assert(x.size() == v.size());
assert(x.size() == m.size());
// Implement implicit euler here!
VectorXs F(x.size());
F.setZero();
scene.accumulateGradU(F, dt*v, VectorXs(x.size()).setZero());
// Force is negative the energy gradient
F *= -1.0;
MatrixXs M(x.size(), x.size());
M.setZero();
for (int i=0;i<x.size();i+=2) {
M(i, i) = m[i];
M(i+1, i+1) = m[i+1];
}
MatrixXs MatQ(x.size(), x.size()); // dF/dq
MatQ.setZero();
scene.accumulateddUdxdx(MatQ, dt*v, VectorXs(x.size()).setZero());
MatrixXs MatV(x.size(), x.size()); // dF/dv
MatV.setZero();
scene.accumulateddUdxdv(MatV, dt*v, VectorXs(x.size()).setZero());
MatrixXs A = M-(dt*dt*MatQ+dt*MatV);
// Zero the force for fixed DoFs
for( int i = 0; i < scene.getNumParticles(); ++i ) if( scene.isFixed(i) ) F.segment<2>(2*i).setZero();
for( int i = 0; i < scene.getNumParticles(); ++i ) if( scene.isFixed(i) ) {
A.row(2*i).setZero();
A.row(2*i+1).setZero();
A.col(2*i).setZero();
A.col(2*i+1).setZero();
A(2*i, 2*i) = 1;
A(2*i+1, 2*i+1) = 1;
}
VectorXs dv = A.fullPivLu().solve(dt*F);
v = v+dv;
x = x+v*dt;
return true;
}
bool ExplicitEuler::stepScene( TwoDScene& scene, scalar dt )
{
VectorXs& x = scene.getX();
VectorXs& v = scene.getV();
const VectorXs& m = scene.getM();
// if( scene.isFixed(i) ) // Determine if the ith particle is fixed
int num_particles = scene.getNumParticles();
VectorXs forces(num_particles * 2);
VectorXs dx(num_particles * 2);
VectorXs dv(num_particles * 2);
for (int i = 0; i < num_particles * 2; i++){
forces(i) = 0.0;
dx(i) = 0.0;
dv(i) = 0.0;
}
scene.accumulateGradU(forces, dx, dv);//, const VectorXs& dx, const VectorXs& dv )
for (int i = 0; i < num_particles; i++){
if (!scene.isFixed(i)){
int index = 2 * i; // even indeces of vector are x params and odd are y params
const Vector2s next_velocity(v(index) + dt*forces(index)/m(index),
v(index+1) + dt*forces(index+1)/m(index));
scene.setVelocity(i, next_velocity);
const Vector2s next_position(x(index) + dt*v(index),
x(index + 1) + dt*v(index+1));
scene.setPosition(i, next_position);
scalar kinetic_energy = 0;
text_file << step_count*dt << "\t" << scene.computeKineticEnergy() << endl;
}
}
step_count++;
return true;
}
// Detects whether a particle and an edge are overlapping (including the radii
// of both) and are approaching.
// If the two objects overlap and are approaching, returns true and sets the
// vector n to be the shortest vector between the particle and the edge.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// eidx: The index of the edge. (Ie, the indices of particle with index e are
// scene.getEdges()[e].first and scene.getEdges()[e].second.)
// Outputs:
// n: The shortest vector between the particle and the edge.
// Returns true if the two objects overlap and are approaching.
bool SimpleCollisionHandler::detectParticleEdge(TwoDScene &scene, int vidx, int eidx, VectorXs &n)
{
VectorXs x1 = scene.getX().segment<2>(2*vidx);
VectorXs x2 = scene.getX().segment<2>(2*scene.getEdges()[eidx].first);
VectorXs x3 = scene.getX().segment<2>(2*scene.getEdges()[eidx].second);
double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
alpha = std::min(1.0, std::max(0.0, alpha));
VectorXs closest = x2 + alpha*(x3-x2);
n = closest-x1;
if(n.norm() < scene.getRadius(vidx)+scene.getEdgeRadii()[eidx])
{
VectorXs v1 = scene.getV().segment<2>(2*vidx);
VectorXs v2 = scene.getV().segment<2>(2*scene.getEdges()[eidx].first);
VectorXs v3 = scene.getV().segment<2>(2*scene.getEdges()[eidx].second);
double relvel = (v1 - v2 - alpha*(v3-v2)).dot(n);
if(relvel > 0)
{
return true;
}
}
return false;
}
void AllPairsDetector::performCollisionDetection(const TwoDScene &scene, const VectorXs &qs, const VectorXs &qe, DetectionCallback &dc)
{
for(int i=0; i<(int)scene.getNumParticles(); i++)
{
for(int j=i+1; j<(int)scene.getNumParticles(); j++)
{
dc.ParticleParticleCallback(i,j);
}
for(int e=0; e<(int)scene.getNumEdges(); e++)
{
if(scene.getEdge(e).first != i && scene.getEdge(e).second != i)
dc.ParticleEdgeCallback(i,e);
}
for(int h=0; h<(int)scene.getNumHalfplanes(); h++)
{
dc.ParticleHalfplaneCallback(i,h);
}
}
}
void TwoDSceneSVGRenderer::renderComparisonScene( const std::string& filename, const TwoDScene& otherscene, const std::vector<CollisionInfo> *impulses, const std::vector<CollisionInfo> *otherimpulses, const scalar &eps) const
{
const VectorXs& x = m_scene.getX();
const VectorXs& v = m_scene.getV();
assert( x.size()%2 == 0 );
assert( 2*m_scene.getNumParticles() == x.size() );
int numparticles = x.size()/2;
const std::vector<scalar>& radii = m_scene.getRadii();
assert( numparticles == (int) radii.size() );
std::fstream file(filename.c_str(), std::fstream::out);
if(!file)
{
std::cerr << "Failure writing SVG file!" << std::endl;
exit(1);
}
scalar scale, xmin, ymin, xshift, yshift;
computeSimToImageMap(scale, xmin, ymin, xshift, yshift );
file << "<?xml version=\"1.0\" encoding=\"utf-8\"?> <!-- Generator: Adobe Illustrator 13.0.0, SVG Export Plug-In . SVG Version: 6.00 Build 14948) --> <svg version=\"1.2\" baseProfile=\"tiny\" id=\"Layer_1\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\" x=\"0px\" y=\"0px\" width=\"";
file << m_w;
file << "px\" height=\"";
file << m_h;
file << "px\" viewBox=\"0 0 ";
file << m_w << " " << m_h;
file << "\" xml:space=\"preserve\">" << std::endl;
// Simulate a background color by rendering a large colored quad
file << "<polygon points=\"" << 0 << "," << 0 << " " << m_w << "," << 0 << " " << m_w << "," << m_h << " " << 0 << "," << m_h;
file << "\" style=\"fill:#" << intToHexString(floor(255.0*m_bgcolor.r+0.5)) << intToHexString(floor(255.0*m_bgcolor.g+0.5)) << intToHexString(floor(255.0*m_bgcolor.b+0.5));
file << "; stroke:#000000;stroke-width:0\"/>" << std::endl;
const std::vector<std::pair<int,int> >& edges = m_scene.getEdges();
const std::vector<scalar>& edgeradii = m_scene.getEdgeRadii();
renderShared( file, x, edges, radii, edgeradii, scale, xmin, ymin, xshift, yshift );
const VectorXs& otherx = otherscene.getX();
const VectorXs& otherv = otherscene.getV();
for( int i = 0; i < numparticles; ++i )
{
scalar x_resid = (otherx.segment<2>(2*i)-x.segment<2>(2*i)).norm();
scalar v_resid = (otherv.segment<2>(2*i)-v.segment<2>(2*i)).norm();
if( x_resid > eps || v_resid > eps )
{
Vector2s center;
center << scale*(x(2*i)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i+1)-ymin) - yshift;
renderCircle( file, center, 1.5*scale*radii[i], renderingutils::Color(1.0,0.0,0.0) );
}
}
if(impulses)
{
int i=0, j=0;
// loop over the real impulses
while(i < (int)impulses->size())
{
int curvert = (*impulses)[i].m_idx1;
CollisionInfo::collisiontype curtype = (*impulses)[i].m_type;
int curidx2 = (*impulses)[i].m_idx2;
// all student impulses less than this correct impulse are buggy
while(j < (int)otherimpulses->size()
&& (*otherimpulses)[j].m_idx1 < curvert
&& (*otherimpulses)[j].m_type < curtype
&& (*otherimpulses)[j].m_idx2 < curidx2)
{
renderImpulse( file, otherscene, (*otherimpulses)[j], true);
j++;
}
// check for missed collision
if( ! (j < (int)otherimpulses->size()
&& (*otherimpulses)[j].m_idx1 == curvert
&& (*otherimpulses)[j].m_type == curtype
&& (*otherimpulses)[j].m_idx2 == curidx2))
{
renderImpulse( file, otherscene, (*impulses)[i], false);
}
else
{
// check for buggy normal
if( ((*otherimpulses)[j].m_n - (*impulses)[i].m_n).norm() > eps)
{
renderImpulse( file, otherscene, (*impulses)[i], false);
renderImpulse( file, otherscene, (*otherimpulses)[j], true);
}
j++;
}
i++;
}
// Any remaining student impulses are buggy
while(j < (int)otherimpulses->size())
{
renderImpulse( file, otherscene, (*otherimpulses)[j], true);
j++;
}
}
file << "</svg>" << std::endl;
file.close();
}
void TwoDSceneXMLParser::loadSpringForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
// Extract the edge the force acts across
int forcenum = 0;
for( rapidxml::xml_node<>* nd = node->first_node("springforce"); nd; nd = nd->next_sibling("springforce") )
{
int edge = -1;
if( nd->first_attribute("edge") )
{
std::string attribute(nd->first_attribute("edge")->value());
if( !stringutils::extractFromString(attribute,edge) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of edge attribute for springforce " << forcenum << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of edge attribute for springforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
std::pair<int,int> newedge(twodscene.getEdge(edge));
// Extract the spring stiffness
scalar k = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("k") )
{
std::string attribute(nd->first_attribute("k")->value());
if( !stringutils::extractFromString(attribute,k) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of k attribute for springforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse k attribute for springforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the spring rest length
scalar l0 = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("l0") )
{
std::string attribute(nd->first_attribute("l0")->value());
if( !stringutils::extractFromString(attribute,l0) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of l0 attribute for springforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse l0 attribute for springforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the optional damping coefficient
scalar b = 0.0;
if( nd->first_attribute("b") )
{
std::string attribute(nd->first_attribute("b")->value());
if( !stringutils::extractFromString(attribute,b) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of b attribute for springforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
//std::cout << "Springforce: " << forcenum << " i: " << newedge.first << " j: " << newedge.second << " k: " << k << " l0: " << l0 << std::endl;
twodscene.insertForce(new SpringForce(newedge,k,l0,b));
++forcenum;
}
//SpringForce( const std::pair<int,int>& endpoints, const scalar& k, const scalar& l0 )
}
void TwoDSceneGrader::addToAccumulatedResidual( const TwoDScene& oracle_scene, const TwoDScene& testing_scene )
{
assert( oracle_scene.getNumParticles() == testing_scene.getNumParticles() );
assert( 2*oracle_scene.getNumParticles() == oracle_scene.getX().size() );
assert( 2*oracle_scene.getNumParticles() == testing_scene.getX().size() );
assert( 2*oracle_scene.getNumParticles() == oracle_scene.getV().size() );
assert( 2*oracle_scene.getNumParticles() == testing_scene.getV().size() );
const VectorXs& oracle_x = oracle_scene.getX();
const VectorXs& testing_x = testing_scene.getX();
const VectorXs& oracle_v = oracle_scene.getV();
const VectorXs& testing_v = testing_scene.getV();
for( int i = 0; i < oracle_scene.getNumParticles(); ++i )
{
scalar x_resid = (oracle_x.segment<2>(2*i)-testing_x.segment<2>(2*i)).norm();
assert( x_resid >= 0.0 );
scalar v_resid = (oracle_v.segment<2>(2*i)-testing_v.segment<2>(2*i)).norm();
assert( v_resid >= 0.0 );
m_accumulated_position_residual += x_resid;
m_accumulated_velocity_residual += v_resid;
if( x_resid > m_max_position_residual ) m_max_position_residual = x_resid;
if( v_resid > m_max_velocity_residual ) m_max_velocity_residual = v_resid;
}
}
void TwoDSceneXMLParser::loadGravitationalForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
// Extract the edge the force acts across
int forcenum = 0;
for( rapidxml::xml_node<>* nd = node->first_node("gravitationalforce"); nd; nd = nd->next_sibling("gravitationalforce") )
{
std::pair<int,int> newedge;
if( nd->first_attribute("i") )
{
std::string attribute(nd->first_attribute("i")->value());
if( !stringutils::extractFromString(attribute,newedge.first) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of i attribute for gravitationalforce " << forcenum << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of i attribute for gravitationalforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("j") )
{
std::string attribute(nd->first_attribute("j")->value());
if( !stringutils::extractFromString(attribute,newedge.second) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of j attribute for gravitationalforce " << forcenum << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of j attribute for gravitationalforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the gravitational constant
scalar G = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("G") )
{
std::string attribute(nd->first_attribute("G")->value());
if( !stringutils::extractFromString(attribute,G) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of G attribute for gravitationalforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse G attribute for gravitationalforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
//std::cout << "GravitationalForce: " << forcenum << " i: " << newedge.first << " j: " << newedge.second << " G: " << G << std::endl;
twodscene.insertForce(new GravitationalForce(newedge,G));
++forcenum;
}
//SpringForce( const std::pair<int,int>& endpoints, const scalar& k, const scalar& l0 )
}
void TwoDSceneXMLParser::loadVorexForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
// Extract the edge the force acts across
int forcenum = 0;
for( rapidxml::xml_node<>* nd = node->first_node("vortexforce"); nd; nd = nd->next_sibling("vortexforce") )
{
std::pair<int,int> newedge;
if( nd->first_attribute("i") )
{
std::string attribute(nd->first_attribute("i")->value());
if( !stringutils::extractFromString(attribute,newedge.first) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of i attribute for vortexforce " << forcenum << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of i attribute for vortexforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("j") )
{
std::string attribute(nd->first_attribute("j")->value());
if( !stringutils::extractFromString(attribute,newedge.second) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of j attribute for vortexforce " << forcenum << ". Value must be integer. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of j attribute for vortexforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the 'Biot-Savart' constant
scalar kbs = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("kbs") )
{
std::string attribute(nd->first_attribute("kbs")->value());
if( !stringutils::extractFromString(attribute,kbs) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of kbs attribute for vortexforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse kbs attribute for vortexforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the viscosity constant
scalar kvc = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("kvc") )
{
std::string attribute(nd->first_attribute("kvc")->value());
if( !stringutils::extractFromString(attribute,kvc) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of kvc attribute for vortexforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse kvc attribute for vortexforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
//std::cout << "VortexForce: " << forcenum << " i: " << newedge.first << " j: " << newedge.second << " kbs: " << kbs << " kvc: " << kvc << std::endl;
twodscene.insertForce(new VortexForce(newedge,kbs,kvc));
++forcenum;
}
}
// Responds to a collision detected between a particle and an edge by applying
// an impulse to the velocities of each one.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// eidx: The index of the edge.
// n: The shortest vector between the particle and the edge.
// Outputs:
// None.
void SimpleCollisionHandler::respondParticleEdge(TwoDScene &scene, int vidx, int eidx, const VectorXs &n)
{
const VectorXs &M = scene.getM();
int eidx1 = scene.getEdges()[eidx].first;
int eidx2 = scene.getEdges()[eidx].second;
VectorXs x1 = scene.getX().segment<2>(2*vidx);
VectorXs x2 = scene.getX().segment<2>(2*eidx1);
VectorXs x3 = scene.getX().segment<2>(2*eidx2);
VectorXs v1 = scene.getV().segment<2>(2*vidx);
VectorXs v2 = scene.getV().segment<2>(2*eidx1);
VectorXs v3 = scene.getV().segment<2>(2*eidx2);
VectorXs nhat = n;
nhat.normalize();
double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
alpha = std::min(1.0, std::max(0.0, alpha) );
VectorXs vedge = v2 + alpha*(v3-v2);
double cfactor = (1.0 + getCOR())/2.0;
double m1 = scene.isFixed(vidx) ? std::numeric_limits<double>::infinity() : M[2*vidx];
double m2 = scene.isFixed(eidx1) ? std::numeric_limits<double>::infinity() : M[2*eidx1];
double m3 = scene.isFixed(eidx2) ? std::numeric_limits<double>::infinity() : M[2*eidx2];
double numerator = 2*cfactor*(vedge-v1).dot(nhat);
double denom1 = 1.0 + (1-alpha)*(1-alpha)*m1/m2 + alpha*alpha*m1/m3;
double denom2 = m2/m1 + (1-alpha)*(1-alpha) + alpha*alpha*m2/m3;
double denom3 = m3/m1 + (1-alpha)*(1-alpha)*m3/m2 + alpha*alpha;
if(!scene.isFixed(vidx))
scene.getV().segment<2>(2*vidx) += numerator/denom1 * nhat;
if(!scene.isFixed(eidx1))
scene.getV().segment<2>(2*eidx1) -= (1.0-alpha)*numerator/denom2 * nhat;
if(!scene.isFixed(eidx2))
scene.getV().segment<2>(2*eidx2) -= alpha * numerator/denom3 * nhat;
}
void TwoDSceneXMLParser::loadSimpleGravityForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
// Load each constant force
int forcenum = 0;
for( rapidxml::xml_node<>* nd = node->first_node("simplegravity"); nd; nd = nd->next_sibling("simplegravity") )
{
Vector3s constforce;
constforce.setConstant(std::numeric_limits<scalar>::signaling_NaN());
// Extract the x component of the force
if( nd->first_attribute("fx") )
{
std::string attribute(nd->first_attribute("fx")->value());
if( !stringutils::extractFromString(attribute,constforce.x()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fx attribute for constantforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fx attribute for constantforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the y component of the force
if( nd->first_attribute("fy") )
{
std::string attribute(nd->first_attribute("fy")->value());
if( !stringutils::extractFromString(attribute,constforce.y()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fy attribute for constantforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fy attribute for constantforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
// Extract the y component of the force
if( nd->first_attribute("fz") )
{
std::string attribute(nd->first_attribute("fz")->value());
if( !stringutils::extractFromString(attribute,constforce.z()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fz attribute for constantforce " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fz attribute for constantforce " << forcenum << ". Exiting." << std::endl;
exit(1);
}
//std::cout << "x: " << constforce.transpose() << std::endl;
twodscene.insertForce(new SimpleGravityForce(constforce));
++forcenum;
}
}
void TwoDSceneXMLParser::loadParticles( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
// Count the number of particles
int numparticles = 0;
for( rapidxml::xml_node<>* nd = node->first_node("particle"); nd; nd = nd->next_sibling("particle") ) ++numparticles;
twodscene.resizeSystem(numparticles);
//std::cout << "Num particles " << numparticles << std::endl;
std::vector<std::string>& tags = twodscene.getParticleTags();
int particle = 0;
for( rapidxml::xml_node<>* nd = node->first_node("particle"); nd; nd = nd->next_sibling("particle") )
{
// Extract the particle's initial position
Vector3s pos;
if( nd->first_attribute("px") )
{
std::string attribute(nd->first_attribute("px")->value());
if( !stringutils::extractFromString(attribute,pos.x()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of px attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse px attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("py") )
{
std::string attribute(nd->first_attribute("py")->value());
if( !stringutils::extractFromString(attribute,pos.y()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of py attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse py attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("pz") )
{
std::string attribute(nd->first_attribute("pz")->value());
if( !stringutils::extractFromString(attribute,pos.z()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of pz attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse pz attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setPosition( particle, pos );
// Extract the particle's initial velocity
Vector3s vel;
if( nd->first_attribute("vx") )
{
std::string attribute(nd->first_attribute("vx")->value());
if( !stringutils::extractFromString(attribute,vel.x()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vx attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vx attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("vy") )
{
std::string attribute(nd->first_attribute("vy")->value());
if( !stringutils::extractFromString(attribute,vel.y()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vy attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vy attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("vz") )
{
std::string attribute(nd->first_attribute("vz")->value());
if( !stringutils::extractFromString(attribute,vel.z()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vz attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vz attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setVelocity( particle, vel );
// Extract the particle's mass
scalar mass = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("m") )
{
std::string attribute(nd->first_attribute("m")->value());
if( !stringutils::extractFromString(attribute,mass) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of m attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse m attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setMass( particle, mass );
// Determine if the particle is fixed
bool fixed;
if( nd->first_attribute("fixed") )
{
std::string attribute(nd->first_attribute("fixed")->value());
if( !stringutils::extractFromString(attribute,fixed) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fixed attribute for particle " << particle << ". Value must be boolean. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fixed attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setFixed( particle, fixed );
// Extract the particle's radius, if present
scalar radius = 0.1;
if( nd->first_attribute("radius") )
{
std::string attribute(nd->first_attribute("radius")->value());
if( !stringutils::extractFromString(attribute,radius) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse radius attribute for particle " << particle << ". Value must be scalar. Exiting." << std::endl;
exit(1);
}
}
twodscene.setRadius( particle, radius );
// Extract the particle's tag, if present
if( nd->first_attribute("tag") )
{
std::string tag(nd->first_attribute("tag")->value());
tags[particle] = tag;
}
//std::cout << "Particle: " << particle << " x: " << pos.transpose() << " v: " << vel.transpose() << " m: " << mass << " fixed: " << fixed << std::endl;
//std::cout << tags[particle] << std::endl;
++particle;
}
}
