void RigidCoordinator::convertToFluidForce(const ParticleVector& particles) { Math::Vector3d totalForce( 0.0, 0.0, 0.0 ); for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { totalForce += (*iter)->getForce() * (*iter)->getVolume(); } const float weight = getWeight( particles ); for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->setForce( totalForce / weight * (*iter)->getDensity() ); } }
void ParticleVisualizer::publish(const ParticleVector& particles) { if (marker_publisher_.getNumSubscribers() == 0) return; double min = std::min_element(particles.begin(), particles.end())->weight; double max = std::max_element(particles.begin(), particles.end())->weight; double range = (max - min) * 0.9; visualization_msgs::MarkerArray markers; for (const auto& particle : particles) { visualization_msgs::Marker marker; marker.ns = "particles"; marker.header.frame_id = frame_id_; marker.type = visualization_msgs::Marker::ARROW; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.6; marker.scale.y = 0.6; marker.scale.z = 0.2; marker.color.a = range > 0 ? 0.1 + (particle.weight - min) / range : 1.0; marker.color.r = 0.0; marker.color.g = 0.2; marker.color.b = 0.8; marker.pose.orientation = tf::createQuaternionMsgFromYaw(particle.pose.theta); marker.pose.position.x = particle.pose.x; marker.pose.position.y = particle.pose.y; marker.pose.position.z = 0.05; marker.id = markers.markers.size(); markers.markers.push_back(marker); } marker_publisher_.publish(markers); }
Vector3d getAverageVelosity(const ParticleVector& particles) { Vector3d averageVelosity( 0.0, 0.0, 0.0 ); for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { averageVelosity += (*iter)->getVelocity();// variable.velocity; } return averageVelosity / static_cast<float>(particles.size()); }
Vector3d getCenter(const ParticleVector& particles){ if( particles.empty() ) { return Vector3d( 0.0, 0.0, 0.0); } Vector3d center( 0.0, 0.0, 0.0); for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { center += (*iter)->getCenter(); } return center /= static_cast<float>(particles.size()); }
void RigidCoordinator::coordinate(const ParticleVector& particles, const float proceedTime) { const Math::Vector3d& objectCenter = getCenter( particles ); const Math::Vector3d& velocityAverage = getAverageVelosity( particles ); for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->setVelocity( velocityAverage ); } for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->addCenter( -1.0 * objectCenter ); } //std::cout << getCenter( particles ).x << std::endl; //std::cout << getCenter( particles ).y << std::endl; //std::cout << getCenter( particles ).z << std::endl; //assert( getCenter( particles ) == Math::Vector3d( 0.0, 0.0, 0.0 ) ); Math::Vector3d inertiaMoment( 0.0, 0.0, 0.0 ); Math::Vector3d torque( 0.0, 0.0, 0.0 ); for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { Particle* particle = (*iter); const Math::Vector3d& center = particle->getCenter(); Math::Vector3d particleMoment( pow( center.getY(), 2) + pow( center.getZ(), 2), pow( center.getZ(), 2 ) + pow( center.getX(), 2), pow( center.getX(), 2 ) + pow( center.getY(), 2) ); inertiaMoment += (particleMoment) * particle->getMass(); const Math::Vector3d diffVector( Math::Vector3d( 0.0, 0.0, 0.0), particle->getCenter() ); const Math::Vector3d& particleTorque = diffVector.getOuterProduct( particle->getForce() * particle->getVolume() ); torque += particleTorque; } getAngleVelosity( inertiaMoment , torque, proceedTime ); if( Math::Tolerances::isEqualStrictly( angleVelosity.getLength() ) ) { for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->addCenter( objectCenter ); } convertToFluidForce( particles); return; } const float rotateAngle = angleVelosity.getLength() * proceedTime; if( rotateAngle < 1.0e-5 ) { for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->addCenter( objectCenter ); } convertToFluidForce( particles); return; } Math::Quaternion quaternion( angleVelosity.getNormalized(), rotateAngle ); const Math::Matrix3d& rotateMatrix = quaternion.getMatrix(); /* for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->variable.center.rotate( rotateMatrix ); } for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) { (*iter)->variable.center += objectCenter; } */ convertToFluidForce( particles ); }