void VortexForce::addGradEToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, VectorXs& gradE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == gradE.size() );
assert( x.size()%2 == 0 );
assert( m_particles.first >= 0 ); assert( m_particles.first < x.size()/2 );
assert( m_particles.second >= 0 ); assert( m_particles.second < x.size()/2 );
Vector2s rhat = x.segment<2>(2*m_particles.second)-x.segment<2>(2*m_particles.first);
scalar r = rhat.norm();
assert( r != 0.0 );
rhat /= r;
rhat *= m_kbs;
// Rotate rhat 90 degrees clockwise
scalar temp = rhat.x();
rhat.x() = -rhat.y();
rhat.y() = temp;
rhat -= v.segment<2>(2*m_particles.second)-v.segment<2>(2*m_particles.first);
rhat /= r*r;
rhat *= m_kvc;
gradE.segment<2>(2*m_particles.first) -= -rhat;
gradE.segment<2>(2*m_particles.second) += -rhat;
}
static VectorXs parallelTransport2D( const VectorXs& n0, const VectorXs& n1, const VectorXs& t0 )
{
assert( fabs( n0.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( n1.norm() - 1.0 ) <= 1.0e-6 );
// x is cos of angle, y is sin of angle
const Vector2s r{ n0.dot( n1 ), MathUtilities::cross( n0, n1 ) };
assert( fabs( r.norm() - 1.0 ) <= 1.0e-6 );
// Rotate t0
VectorXs t1{ 2 };
t1( 0 ) = r.x() * t0.x() - r.y() * t0.y();
t1( 1 ) = r.y() * t0.x() + r.x() * t0.y();
assert( fabs( t0.norm() - t1.norm() ) <= 1.0e-6 );
// TODO: Assert n0,t0 angle is same as n1,t1 angle
return t1;
}
void TwoDSceneSVGRenderer::renderSolidCircle( std::fstream& file, const Vector2s& center, const scalar& r, const renderingutils::Color& color ) const
{
file << "<circle fill=\"#";
file << intToHexString(floor(255.0*color.r+0.5)) << intToHexString(floor(255.0*color.g+0.5)) << intToHexString(floor(255.0*color.b+0.5)) << "\"" << " stroke=\"#";
file << intToHexString(floor(255.0*color.r+0.5)) << intToHexString(floor(255.0*color.g+0.5)) << intToHexString(floor(255.0*color.b+0.5)) << "\"" << " stroke-width=\"0\" cx=\"";
file << center.x();
file << "\" cy=\"";
file << center.y();
file << "\" r=\"";
file << r;
file << "\"/>" << std::endl;
}
void TwoDSceneSVGRenderer::renderSweptEdge( std::fstream& file, const Vector2s& x0, const Vector2s& x1, const scalar& r, const renderingutils::Color& color ) const
{
scalar theta = atan2(x1.y()-x0.y(),x1.x()-x0.x());
scalar rx = -r*sin(theta);
scalar ry = r*cos(theta);
scalar p0x = x0.x() + rx;
scalar p0y = x0.y() + ry;
scalar p1x = x0.x() - rx;
scalar p1y = x0.y() - ry;
scalar p2x = x1.x() - rx;
scalar p2y = x1.y() - ry;
scalar p3x = x1.x() + rx;
scalar p3y = x1.y() + ry;
file << "<polygon points=\"" << p0x << "," << p0y << " " << p1x << "," << p1y << " " << p2x << "," << p2y << " " << p3x << "," << p3y;
file << "\" style=\"fill:#" << intToHexString(floor(255.0*color.r+0.5)) << intToHexString(floor(255.0*color.g+0.5)) << intToHexString(floor(255.0*color.b+0.5));
file << "; stroke:#000000;stroke-width:0\"/>" << std::endl;
renderSolidCircle( file, x0, r, color );
renderSolidCircle( file, x1, r, color );
}
void TwoDSceneSVGRenderer::renderRigidBodyScene( const std::string& name, const RigidBodyScene& rbscene ) const
{
// const VectorXs& x = m_scene.getX();
// 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(name.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<RigidBody> rbs = rbscene.getRigidBodies();
assert( m_particle_colors.size() == rbs.size() );
// For each rigid body
for( std::vector<RigidBody>::size_type i = 0; i < rbs.size(); ++i )
{
scalar r = rbs[i].getRadius();
renderingutils::Color rbcolor = m_particle_colors[i];
// Render each edge of each rigid body
for( int j = 0; j < rbs[i].getNumVertices(); ++j )
{
const Vector2s& xi = rbs[i].getWorldSpaceVertex(j);
const Vector2s& xj = rbs[i].getWorldSpaceVertex((j+1)%rbs[i].getNumVertices());
Vector2s p0;
p0 << scale*(xi.x()-xmin) + xshift, ((scalar)m_h) - scale*(xi.y()-ymin) - yshift;
Vector2s p1;
p1 << scale*(xj.x()-xmin) + xshift, ((scalar)m_h) - scale*(xj.y()-ymin) - yshift;
renderSweptEdge( file, p0, p1, scale*r, rbcolor );
}
}
// Render each spring force
for( std::vector<RigidBody>::size_type i = 0; i < m_svg_renderers.size(); ++i )
{
renderingutils::Color clr = m_svg_renderers[i]->getColor();
Vector2s xi = m_svg_renderers[i]->getVertexOne(rbs);
Vector2s xj = m_svg_renderers[i]->getVertexTwo(rbs);
Vector2s p0;
p0 << scale*(xi.x()-xmin) + xshift, ((scalar)m_h) - scale*(xi.y()-ymin) - yshift;
Vector2s p1;
p1 << scale*(xj.x()-xmin) + xshift, ((scalar)m_h) - scale*(xj.y()-ymin) - yshift;
renderSweptEdge( file, p0, p1, scale*0.03, clr );
}
file << "</svg>" << std::endl;
file.close();
}
void TwoDSceneSVGRenderer::renderShared( std::fstream& file, const VectorXs& x, const std::vector<std::pair<int,int> >& edges, const std::vector<scalar>& radii, const std::vector<scalar>& edgeradii, const scalar& scale, const scalar& xmin, const scalar& ymin, const scalar& xshift, const scalar& yshift ) const
{
int numparticles = x.size()/2;
for( std::vector<renderingutils::ParticlePath>::size_type i = 0; i < m_particle_paths.size(); ++i )
{
const std::list<Vector2s>& ppath = m_particle_paths[i].getPath();
const renderingutils::Color& pathcolor = m_particle_paths[i].getColor();
file << "<polyline points=\"";
for( std::list<Vector2s>::const_iterator itr = ppath.begin(); itr != ppath.end(); ++itr )
{
Vector2s point;
point << scale*(itr->x()-xmin) + xshift, ((scalar)m_h) - scale*(itr->y()-ymin) - yshift;
file << point.x() << "," << point.y() << " ";
}
file << "\" style=\"fill:none;stroke:#";
file << intToHexString(floor(255.0*pathcolor.r+0.5)) << intToHexString(floor(255.0*pathcolor.g+0.5)) << intToHexString(floor(255.0*pathcolor.b+0.5));
file << ";stroke-width:1\"/>" << std::endl;
}
// Render edges
assert( edgeradii.size() == edges.size() );
for( std::vector<std::pair<int,int> >::size_type i = 0; i < edges.size(); ++i )
{
assert( edges[i].first >= 0 );
assert( edges[i].first < m_scene.getNumParticles() );
assert( edges[i].second >= 0 );
assert( edges[i].second < m_scene.getNumParticles() );
int i0 = edges[i].first;
int i1 = edges[i].second;
Vector2s p0;
p0 << scale*(x(2*i0)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i0+1)-ymin) - yshift;
Vector2s p1;
p1 << scale*(x(2*i1)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i1+1)-ymin) - yshift;
renderSweptEdge( file, p0, p1, scale*edgeradii[i], m_edge_colors[i] );
}
// Render halfplanes
for(int i=0; i < m_scene.getNumHalfplanes(); i++)
{
Vector2s p0;
p0 << scale * (m_scene.getHalfplane(i).first[0]-xmin) + xshift, ((scalar)m_h) - scale*(m_scene.getHalfplane(i).first[1]-ymin) - yshift;
Vector2s n0;
n0 << m_scene.getHalfplane(i).second[0], -m_scene.getHalfplane(i).second[1];
n0.normalize();
renderHalfplane(file, p0, n0, m_halfplane_colors[i]);
}
// Render particles
for( int i = 0; i < numparticles; ++i )
{
Vector2s center;
center << scale*(x(2*i)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i+1)-ymin) - yshift;
renderSolidCircle( file, center, scale*radii[i], m_particle_colors[i] );
}
}
static PyObject* insertBall( PyObject* self, PyObject* args )
{
Vector2s q;
Vector2s v;
scalar r;
scalar m;
int fixed;
assert( args != nullptr );
using std::is_same;
static_assert( is_same<scalar,double>::value || is_same<scalar,float>::value, "Error, scalar type must be double or float for Python interface." );
if( !PyArg_ParseTuple( args, is_same<scalar,double>::value ? "ddddddi" : "ffffffi", &q.x(), &q.y(), &v.x(), &v.y(), &r, &m, &fixed ) )
{
PyErr_Print();
std::cerr << "Failed to read parameters for insertBall, parameters are: double x, double y, double vx, double vy, double radius, double mass, bool fixed. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
if( r <= 0.0 )
{
std::cerr << "Error in insert ball, radius must be positive. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
if( m <= 0.0 )
{
std::cerr << "Error in insert ball, mass must be positive. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
if( fixed != 0 && fixed != 1 )
{
std::cerr << "Error in insert ball, fixed value must be 0 or 1. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
assert( s_ball_state != nullptr );
s_ball_state->pushBallBack( q, v, r, m, fixed );
return Py_BuildValue( "" );
}
