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::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 QGraphicsGuideView::simplifyPainterPath() {
ParticleVector data;
data.resize(_drawingPath.elementCount());
for (int i = 0; i < data.size(); i++) {
data[i].x[0] = _drawingPath.elementAt(i).x;
data[i].x[1] = _drawingPath.elementAt(i).y;
}
CurveFitting fitter;
fitter.FitCurve(data);
ParticleVector& result = fitter.GetResult();
_drawingPath = QPainterPath();
_drawingPath.moveTo(result[0].x[0], result[0].x[1]);
for (int i = 1; i < result.size(); i++) {
_drawingPath.lineTo(result[i].x[0], result[i].x[1]);
}
}
int main(int argc, char* argv[]) {
typedef std::vector<Particle> ParticleVector;
ParticleVector particleTrace;
Options parser;
// StringVector& args = parser.ParseOptions(argc, argv, NULL);
itkcmds::itkImageIO<LabelImage> io;
// LabelImage::Pointer field = io.ReadImageT("/NIRAL/work/joohwi/data/ellipse/ellipse3.nrrd");
LabelImage::Pointer field = io.ReadImageT("/tmpfs/circle_ellipse.nrrd");
ParticleCollision collision;
collision.SetBinaryMask(field);
collision.UseDistanceMapCache("/tmpfs/circle_ellipse_distancemap.nrrd");
collision.UpdateImages();
itkcmds::itkImageIO<DoubleImage> iox;
DoubleImage::Pointer outField = iox.NewImageT<LabelImage>(field);
double t0 = 0;
double t1 = 100;
double dt = 0.05;
VectorGrid grid;
grid.CreateScalar("time");
grid.CreateVector("normal");
grid.CreateVector("velocity");
grid.CreateVector("tangent");
LabelImage::IndexType pIdx;
LabelImage::RegionType fieldRegion = field->GetBufferedRegion();
VNLVector normal(__Dim, 0);
VNLVector force(__Dim, 0);
VNLVector tangent(__Dim, 0);
const int Nx = 8, Ny = 8, Nz = 8;
const double S = 0.5;
ParticleSubjectArray subs;
subs.resize(1);
subs[0].NewParticles(Nx*Ny*Nz);
const bool useRandomSampling = true;
if (useRandomSampling) {
subs[0].InitializeRandomPoints(field);
} else {
for (int i = 0; i < Nx; i++) {
for (int j = 0; j < Ny; j ++) {
for (int k = 0; k < Nz; k++) {
int m = j%2;
int n = 0;
arrayset3(subs[0][Ny*Nz*i+Nz*j+k].x, 40+(i-Nx/2.0+m/2.0)*S, 40+(j-Ny/2.0+n/2.0)*S, 40);
arrayset3(subs[0][Ny*Nz*i+Nz*j+k].v, 0, 0, 0);
}
}
}
}
EntropyInternalForce internalForce;
const int nPoints = subs[0].GetNumberOfPoints();
for (double t = t0; t <= t1; t += dt) {
cout << "t: " << t << endl;
for (int n = 0; n < 1; n++) {
for (int i = 0; i < subs[n].GetNumberOfPoints(); i++) {
// system setup
particleTrace.push_back(subs[n][i]);
forset(subs[n][i].x, subs[n][i].w);
forfill(subs[n][i].f, 0);
}
}
internalForce.ComputeForce(subs);
// collision handling
collision.HandleCollision(subs);
// system update
for (int n = 0; n < 1; n++) {
for (int i = 0; i < nPoints; i++) {
fordim(k) {
subs[n][i].f[k] -= 1 * subs[n][i].v[k];
subs[n][i].v[k] += dt * subs[n][i].f[k];
subs[n][i].x[k] += dt * subs[n][i].v[k];
}
Particle &p = subs[n][i];
// cout << t << ": x = (" << x2string(p.x) << "); v = (" << x2string(p.v) << "); f = (" << x2string(p.f) << "); " << endl;
if (!fieldRegion.IsInside(pIdx)) {
cout << "Stop system: out of region" << endl;
goto quit;
}
fordim(k) {
pIdx[k] = p.x[k] + 0.5;
}
outField->SetPixel(pIdx, t);
if (dimequal(p.v,0,0,0)) {
cout << "Stop system: " << t << endl;
goto quit;
}
subs[n][i].t = t;
}
}
}
quit:
for (int n = 0; n < 1; n++) {
for (int i = 0; i < subs[n].GetNumberOfPoints(); i++) {
// system setup
particleTrace.push_back(subs[n][i]);
}
}
ofstream of("/tmpfs/trace.txt");
for (int i = 0; i < particleTrace.size(); i++) {
of << particleTrace[i].t << " " << particleTrace[i].idx << " " << particleTrace[i] << endl;
}
of.close();
const char* particleOutput = "/tmpfs/particle.nrrd";
iox.WriteImageT(particleOutput, outField);
}
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 );
}
