line lineToSpace( line in, math::matrix4x4 mat )
{
in.position = pVector( mat.pointMultiply( in.position ) );
in.direction = pVector( mat.vectorMultiply( in.direction ) );
return in;
}
PARTICLEDLL_API void pTargetSize(float size_x, float size_y, float size_z,
float scale_x, float scale_y, float scale_z)
{
PATargetSize S;
S.size = pVector(size_x, size_y, size_z);
S.scale = pVector(scale_x, scale_y, scale_z);
_pSendAction(&S, PATargetSizeID, sizeof(PATargetSize));
}
PARTICLEDLL_API void pOrbitLine(float p_x, float p_y, float p_z,
float axis_x, float axis_y, float axis_z,
float magnitude, float epsilon, float max_radius)
{
PAOrbitLine S;
S.p = pVector(p_x, p_y, p_z);
S.axis = pVector(axis_x, axis_y, axis_z);
S.axis.normalize();
S.magnitude = magnitude;
S.epsilon = epsilon;
S.max_radius = max_radius;
_pSendAction(&S, PAOrbitLineID, sizeof(PAOrbitLine));
}
PARTICLEDLL_API void pVortex(float center_x, float center_y, float center_z,
float axis_x, float axis_y, float axis_z,
float magnitude, float epsilon, float max_radius)
{
PAVortex S;
S.center = pVector(center_x, center_y, center_z);
S.axis = pVector(axis_x, axis_y, axis_z);
S.axis.normalize();
S.magnitude = magnitude;
S.epsilon = epsilon;
S.max_radius = max_radius;
_pSendAction(&S, PAVortexID, sizeof(PAVortex));
}
void pDrawGroupl(int dlist, bool const_size, bool const_color, bool const_rotation)
{
// Get a pointer to the particles in gp memory
ParticleGroup *pg = _ps.pgrp;
if(pg == NULL)
return; // ERROR
if(pg->p_count < 1)
return;
//if(const_color)
// glColor4fv((GLfloat *)&pg->list[0].color);
for(int i = 0; i < pg->p_count; i++)
{
Particle &m = pg->list[i];
glPushMatrix();
glTranslatef(m.pos.x, m.pos.y, m.pos.z);
if(!const_size)
glScalef(m.size, m.size, m.size);
// Expensive! A sqrt, cross prod and acos. Yow.
if(!const_rotation)
{
pVector vN(m.vel);
vN.normalize();
pVector voN(m.velB);
voN.normalize();
pVector biN;
if(voN.x == vN.x && voN.y == vN.y && voN.z == vN.z)
biN = pVector(0, 1, 0);
else
biN = vN ^ voN;
biN.normalize();
pVector N(vN ^ biN);
double M[16];
M[0] = vN.x; M[4] = biN.x; M[8] = N.x; M[12] = 0;
M[1] = vN.y; M[5] = biN.y; M[9] = N.y; M[13] = 0;
M[2] = vN.z; M[6] = biN.z; M[10] = N.z; M[14] = 0;
M[3] = 0; M[7] = 0; M[11] = 0; M[15] = 1;
glMultMatrixd(M);
}
// Warning: this depends on alpha following color in the Particle struct.
if(!const_color)
glColor4fv((GLfloat *)&m.color);
glCallList(dlist);
glPopMatrix();
}
glEnd();
}
PARTICLEDLL_API void pGravity(float dir_x, float dir_y, float dir_z)
{
PAGravity S;
S.direction = pVector(dir_x, dir_y, dir_z);
_pSendAction(&S, PAGravityID, sizeof(PAGravity));
}
PARTICLEDLL_API void pTargetVelocity(float vel_x, float vel_y, float vel_z, float scale)
{
PATargetVelocity S;
S.velocity = pVector(vel_x, vel_y, vel_z);
S.scale = scale;
_pSendAction(&S, PATargetVelocityID, sizeof(PATargetVelocity));
}
PARTICLEDLL_API void pTargetColor(float color_x, float color_y, float color_z,
float alpha, float scale)
{
PATargetColor S;
S.color = pVector(color_x, color_y, color_z);
S.alpha = alpha;
S.scale = scale;
_pSendAction(&S, PATargetColorID, sizeof(PATargetColor));
}
PARTICLEDLL_API void pDamping(float damping_x, float damping_y, float damping_z,
float vlow, float vhigh)
{
PADamping S;
S.damping = pVector(damping_x, damping_y, damping_z);
S.vlowSqr = fsqr(vlow);
S.vhighSqr = fsqr(vhigh);
_pSendAction(&S, PADampingID, sizeof(PADamping));
}
PARTICLEDLL_API void pOrbitPoint(float center_x, float center_y, float center_z,
float magnitude, float epsilon, float max_radius)
{
PAOrbitPoint S;
S.center = pVector(center_x, center_y, center_z);
S.magnitude = magnitude;
S.epsilon = epsilon;
S.max_radius = max_radius;
_pSendAction(&S, PAOrbitPointID, sizeof(PAOrbitPoint));
}
PARTICLEDLL_API void pJet(float center_x, float center_y, float center_z,
float magnitude, float epsilon, float max_radius)
{
_ParticleState &_ps = _GetPState();
PAJet S;
S.center = pVector(center_x, center_y, center_z);
S.acc = _ps.Vel;
S.magnitude = magnitude;
S.epsilon = epsilon;
S.max_radius = max_radius;
_pSendAction(&S, PAJetID, sizeof(PAJet));
}
PARTICLEDLL_API void pExplosion(float center_x, float center_y, float center_z, float velocity,
float magnitude, float stdev, float epsilon, float age)
{
PAExplosion S;
S.center = pVector(center_x, center_y, center_z);
S.velocity = velocity;
S.magnitude = magnitude;
S.stdev = stdev;
S.epsilon = epsilon;
S.age = age;
if(S.epsilon < 0.0f)
S.epsilon = P_EPS;
_pSendAction(&S, PAExplosionID, sizeof(PAExplosion));
}
RJMCMCBase(REAL *points,int numPoints, REAL *vfmap, REAL *dimg, const int *dsz, double *voxsize, double cellsize, REAL *cellsize2, REAL len, int numcores)
{
dataimg = dimg;
datasz = dsz;
this->vfmap = vfmap;
width = datasz[2]*voxsize[0];
height = datasz[3]*voxsize[1];
depth = datasz[4]*voxsize[2];
this->voxsize = voxsize;
this->numcores = numcores;
fprintf(stderr,"Data dimensions (mm) : %f x %f x %f\n",width,height,depth);
fprintf(stderr,"Data dimensions (voxel) : %i x %i x %i\n",datasz[2],datasz[3],datasz[4]);
fprintf(stderr,"Data directions (sizeLUT * dirs) : %i * %i\n",datasz[0],datasz[1]);
fprintf(stderr,"voxel size (mm) : %f x %f x %f\n",voxsize[0],voxsize[1],voxsize[2]);
////// dimensions of the particle grid
// the one to find connection partners
REAL cellcnt_x = (int)((REAL)width/cellsize) +2;
REAL cellcnt_y = (int)((REAL)height/cellsize) +2;
REAL cellcnt_z = (int)((REAL)depth/cellsize) +2;
int cell_capacity = 2048;
// for finding partners whithn the same voxel
REAL cellcnt_x2 = (int)((REAL)width/cellsize2[0]) +2;
REAL cellcnt_y2 = (int)((REAL)height/cellsize2[1]) +2;
REAL cellcnt_z2 = (int)((REAL)depth/cellsize2[2]) +2;
int cell_capacity2 = 20;
fprintf(stderr,"grid dimensions : %f x %f x %f\n",cellcnt_x,cellcnt_y,cellcnt_z);
fprintf(stderr,"grid cell size (mm) : %f^3\n",cellsize);
fprintf(stderr,"grid2 dimensions : %f x %f x %f\n",cellcnt_x2,cellcnt_y2,cellcnt_z2);
fprintf(stderr,"grid2 cell size (mm) : %fx%fx%f\n",cellsize2[0],cellsize2[1],cellsize2[2]);
fprintf(stderr,"cell capacity : %i\n",cell_capacity);
fprintf(stderr,"#cells*cellcap : %.1f K\n",cell_capacity*cellcnt_x*cellcnt_y*cellcnt_z/1000);
#ifdef PARALLEL_OPENMP
fprintf(stderr,"#threads : %i/%i\n",numcores,omp_get_max_threads());
#endif
// allocate the particle grid
int minsize = 100000;
int err = pcontainer.allocate(((numPoints>minsize)? (numPoints+100000) : minsize),
cellcnt_x, cellcnt_y, cellcnt_z, cellsize,cell_capacity,
cellcnt_x2, cellcnt_y2, cellcnt_z2, cellsize2,cell_capacity2,len);
if (err == -1)
{
fprintf(stderr,"RJMCMCBase: out of Memory!\n");
return;
}
// read the data from MATLAB into our structure
attrcnt = 15;
for (int k = 0; k < numPoints; k++)
{
Particle *p = pcontainer.newParticle(pVector(points[attrcnt*k ], points[attrcnt*k+1],points[attrcnt*k+2]),
pVector(points[attrcnt*k+3], points[attrcnt*k+4],points[attrcnt*k+5]));
if (p!=0)
{
p->N = pVector(points[attrcnt*k+3],points[attrcnt*k+4],points[attrcnt*k+5]);
p->len = points[attrcnt*k+7];
p->mID = (int) points[attrcnt*k+8];
p->pID = (int) points[attrcnt*k+9];
p->Di = points[attrcnt*k+10];
p->Da = points[attrcnt*k+11];
p->Dp = points[attrcnt*k+12];
p->w = points[attrcnt*k+13];
p->q = points[attrcnt*k+14];
p->label = 0;
p->active = true;
if (p->mID != -1)
{
pcontainer.removeFromGrid(1,p);
pcontainer.concnt++;
}
if (p->pID != -1)
{
pcontainer.removeFromGrid(0,p);
pcontainer.concnt++;
}
}
else
{
fprintf(stderr,"error: cannot allocate particle, con. indices will be wrong! \n");
}
}
pcontainer.concnt /= 2;
itmax = 0;
accepted = 0;
}
static inline pVector RandVec()
{
return pVector(drand48(), drand48(), drand48());
}
// Generate a random point uniformly distrbuted within the domain
void pDomain::Generate(pVector &pos) const
{
switch (type)
{
case PDPoint:
pos = p1;
break;
case PDLine:
pos = p1 + p2 * drand48();
break;
case PDBox:
// Scale and translate [0,1] random to fit box
pos.x = p1.x + (p2.x - p1.x) * drand48();
pos.y = p1.y + (p2.y - p1.y) * drand48();
pos.z = p1.z + (p2.z - p1.z) * drand48();
break;
case PDTriangle:
{
float r1 = drand48();
float r2 = drand48();
if(r1 + r2 < 1.0f)
pos = p1 + u * r1 + v * r2;
else
pos = p1 + u * (1.0f-r1) + v * (1.0f-r2);
}
break;
case PDRectangle:
pos = p1 + u * drand48() + v * drand48();
break;
case PDPlane: // How do I sensibly make a point on an infinite plane?
pos = p1;
break;
case PDSphere:
// Place on [-1..1] sphere
pos = RandVec() - vHalf;
pos.normalize();
// Scale unit sphere pos by [0..r] and translate
// (should distribute as r^2 law)
if(radius1 == radius2)
pos = p1 + pos * radius1;
else
pos = p1 + pos * (radius2 + drand48() * (radius1 - radius2));
break;
case PDCylinder:
case PDCone:
{
// For a cone, p2 is the apex of the cone.
float dist = drand48(); // Distance between base and tip
float theta = drand48() * 2.0f * float(M_PI); // Angle around axis
// Distance from axis
float r = radius2 + drand48() * (radius1 - radius2);
float x = r * cosf(theta); // Weighting of each frame vector
float y = r * sinf(theta);
// Scale radius along axis for cones
if(type == PDCone)
{
x *= dist;
y *= dist;
}
pos = p1 + p2 * dist + u * x + v * y;
}
break;
case PDBlob:
pos.x = p1.x + NRand(radius1);
pos.y = p1.y + NRand(radius1);
pos.z = p1.z + NRand(radius1);
break;
case PDDisc:
{
float theta = drand48() * 2.0f * float(M_PI); // Angle around normal
// Distance from center
float r = radius2 + drand48() * (radius1 - radius2);
float x = r * cosf(theta); // Weighting of each frame vector
float y = r * sinf(theta);
pos = p1 + u * x + v * y;
}
break;
default:
pos = pVector(0,0,0);
}
}
pDomain::pDomain(PDomainEnum dtype, float a0, float a1,
float a2, float a3, float a4, float a5,
float a6, float a7, float a8)
{
type = dtype;
switch(type)
{
case PDPoint:
p1 = pVector(a0, a1, a2);
break;
case PDLine:
{
p1 = pVector(a0, a1, a2);
pVector tmp(a3, a4, a5);
// p2 is vector from p1 to other endpoint.
p2 = tmp - p1;
}
break;
case PDBox:
// p1 is the min corner. p2 is the max corner.
if(a0 < a3)
{
p1.x = a0; p2.x = a3;
}
else
{
p1.x = a3; p2.x = a0;
}
if(a1 < a4)
{
p1.y = a1; p2.y = a4;
}
else
{
p1.y = a4; p2.y = a1;
}
if(a2 < a5)
{
p1.z = a2; p2.z = a5;
}
else
{
p1.z = a5; p2.z = a2;
}
break;
case PDTriangle:
{
p1 = pVector(a0, a1, a2);
pVector tp2 = pVector(a3, a4, a5);
pVector tp3 = pVector(a6, a7, a8);
u = tp2 - p1;
v = tp3 - p1;
// The rest of this is needed for bouncing.
radius1Sqr = u.length();
pVector tu = u / radius1Sqr;
radius2Sqr = v.length();
pVector tv = v / radius2Sqr;
p2 = tu ^ tv; // This is the non-unit normal.
p2.normalize(); // Must normalize it.
// radius1 stores the d of the plane eqn.
radius1 = -(p1 * p2);
}
break;
case PDRectangle:
{
p1 = pVector(a0, a1, a2);
u = pVector(a3, a4, a5);
v = pVector(a6, a7, a8);
// The rest of this is needed for bouncing.
radius1Sqr = u.length();
pVector tu = u / radius1Sqr;
radius2Sqr = v.length();
pVector tv = v / radius2Sqr;
p2 = tu ^ tv; // This is the non-unit normal.
p2.normalize(); // Must normalize it.
// radius1 stores the d of the plane eqn.
radius1 = -(p1 * p2);
}
break;
case PDPlane:
{
p1 = pVector(a0, a1, a2);
p2 = pVector(a3, a4, a5);
p2.normalize(); // Must normalize it.
// radius1 stores the d of the plane eqn.
radius1 = -(p1 * p2);
}
break;
case PDSphere:
p1 = pVector(a0, a1, a2);
if(a3 > a4)
{
radius1 = a3; radius2 = a4;
}
else
{
radius1 = a4; radius2 = a3;
}
radius1Sqr = radius1 * radius1;
radius2Sqr = radius2 * radius2;
break;
case PDCone:
case PDCylinder:
{
// p2 is a vector from p1 to the other end of cylinder.
// p1 is apex of cone.
p1 = pVector(a0, a1, a2);
pVector tmp(a3, a4, a5);
p2 = tmp - p1;
if(a6 > a7)
{
radius1 = a6; radius2 = a7;
}
else
{
radius1 = a7; radius2 = a6;
}
radius1Sqr = fsqr(radius1);
// Given an arbitrary nonzero vector n, make two orthonormal
// vectors u and v forming a frame [u,v,n.normalize()].
pVector n = p2;
float p2l2 = n.length2(); // Optimize this.
n.normalize();
// radius2Sqr stores 1 / (p2.p2)
// XXX Used to have an actual if.
radius2Sqr = p2l2 ? 1.0f / p2l2 : 0.0f;
// Find a vector orthogonal to n.
pVector basis(1.0f, 0.0f, 0.0f);
if(fabs(basis * n) > 0.999)
basis = pVector(0.0f, 1.0f, 0.0f);
// Project away N component, normalize and cross to get
// second orthonormal vector.
u = basis - n * (basis * n);
u.normalize();
v = n ^ u;
}
break;
case PDBlob:
{
p1 = pVector(a0, a1, a2);
radius1 = a3;
float tmp = (float)(1./radius1);
radius2Sqr = -0.5f*fsqr(tmp);
radius2 = (float)(ONEOVERSQRT2PI * tmp);
}
break;
case PDDisc:
{
p1 = pVector(a0, a1, a2); // Center point
p2 = pVector(a3, a4, a5); // Normal (not used in Within and Generate)
p2.normalize();
if(a6 > a7)
{
radius1 = a6; radius2 = a7;
}
else
{
radius1 = a7; radius2 = a6;
}
// Find a vector orthogonal to n.
pVector basis(1.0f, 0.0f, 0.0f);
if(fabs(basis * p2) > 0.999)
basis = pVector(0.0f, 1.0f, 0.0f);
// Project away N component, normalize and cross to get
// second orthonormal vector.
u = basis - p2 * (basis * p2);
u.normalize();
v = p2 ^ u;
radius1Sqr = -(p1 * p2); // D of the plane eqn.
}
break;
}
}
// Over time, restore particles to initial positions
// Put all particles on the surface of a statue, explode the statue,
// and then suck the particles back to the original position. Cool!
void PARestore::Execute(ParticleGroup *group)
{
if(time_left <= 0)
{
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// Already constrained, keep it there.
m.pos = m.posB;
m.vel = pVector(0,0,0);
}
}
else
{
float t = time_left;
float dtSqr = dt * dt;
float tSqrInv2dt = dt * 2.0f / (t * t);
float tCubInv3dtSqr = dtSqr * 3.0f / (t * t * t);
for(int i = 0; i < group->p_count; i++)
{
#if 1
Particle &m = group->list[i];
// Solve for a desired-behavior velocity function in each axis
// _pconstrain(m.pos.x, m.vel.x, m.posB.x, 0., timeLeft, &a, &b, &c);
// Figure new velocity at next timestep
// m.vel.x = a * dtSqr + b * dt + c;
float b = (-2*t*m.vel.x + 3*m.posB.x - 3*m.pos.x) * tSqrInv2dt;
float a = (t*m.vel.x - m.posB.x - m.posB.x + m.pos.x + m.pos.x) * tCubInv3dtSqr;
// Figure new velocity at next timestep
m.vel.x += a + b;
b = (-2*t*m.vel.y + 3*m.posB.y - 3*m.pos.y) * tSqrInv2dt;
a = (t*m.vel.y - m.posB.y - m.posB.y + m.pos.y + m.pos.y) * tCubInv3dtSqr;
// Figure new velocity at next timestep
m.vel.y += a + b;
b = (-2*t*m.vel.z + 3*m.posB.z - 3*m.pos.z) * tSqrInv2dt;
a = (t*m.vel.z - m.posB.z - m.posB.z + m.pos.z + m.pos.z) * tCubInv3dtSqr;
// Figure new velocity at next timestep
m.vel.z += a + b;
#else
Particle &m = group->list[i];
// XXX Optimize this.
// Solve for a desired-behavior velocity function in each axis
float a, b, c; // Coefficients of velocity function needed
_pconstrain(m.pos.x, m.vel.x, m.posB.x, 0.,
timeLeft, &a, &b, &c);
// Figure new velocity at next timestep
m.vel.x = a * dtSqr + b * dt + c;
_pconstrain(m.pos.y, m.vel.y, m.posB.y, 0.,
timeLeft, &a, &b, &c);
// Figure new velocity at next timestep
m.vel.y = a * dtSqr + b * dt + c;
_pconstrain(m.pos.z, m.vel.z, m.posB.z, 0.,
timeLeft, &a, &b, &c);
// Figure new velocity at next timestep
m.vel.z = a * dtSqr + b * dt + c;
#endif
}
}
time_left -= dt;
}
RJMCMCBase(float *points,int numPoints, float *dimg, const int *dsz, double *voxsize, double cellsize)
: m_QBallImageData(dimg)
, datasz(dsz)
, enc(0)
, width(dsz[1]*voxsize[0])
, height(dsz[2]*voxsize[1])
, depth(dsz[3]*voxsize[2])
, voxsize(voxsize)
, m_NumAttributes(0)
, m_AcceptedProposals(0)
{
fprintf(stderr,"Data dimensions (mm) : %f x %f x %f\n",width,height,depth);
fprintf(stderr,"Data dimensions (voxel) : %i x %i x %i\n",datasz[1],datasz[2],datasz[3]);
fprintf(stderr,"voxel size (mm) : %lf x %lf x %lf\n",voxsize[0],voxsize[1],voxsize[2]);
float cellcnt_x = (int)((float)width/cellsize) +1;
float cellcnt_y = (int)((float)height/cellsize) +1;
float cellcnt_z = (int)((float)depth/cellsize) +1;
//int cell_capacity = 2048;
//int cell_capacity = 64;
int cell_capacity = 512;
fprintf(stderr,"grid dimensions : %f x %f x %f\n",cellcnt_x,cellcnt_y,cellcnt_z);
fprintf(stderr,"grid cell size (mm) : %f^3\n",cellsize);
fprintf(stderr,"cell capacity : %i\n",cell_capacity);
fprintf(stderr,"#cells*cellcap : %.1f K\n",cell_capacity*cellcnt_x*cellcnt_y*cellcnt_z/1000);
int minsize = 1000000;
int err = m_ParticleGrid.allocate(((numPoints>minsize)? (numPoints+100000) : minsize), cellcnt_x, cellcnt_y, cellcnt_z, cellsize, cell_capacity);
if (err == -1)
{
fprintf(stderr,"RJMCMCBase: out of Memory!\n");
return;
}
m_NumAttributes = 10;
for (int k = 0; k < numPoints; k++)
{
Particle *p = m_ParticleGrid.newParticle(pVector(points[m_NumAttributes*k], points[m_NumAttributes*k+1],points[m_NumAttributes*k+2]));
if (p!=0)
{
p->N = pVector(points[m_NumAttributes*k+3],points[m_NumAttributes*k+4],points[m_NumAttributes*k+5]);
p->cap = points[m_NumAttributes*k+6];
p->len = points[m_NumAttributes*k+7];
p->mID = (int) points[m_NumAttributes*k+8];
p->pID = (int) points[m_NumAttributes*k+9];
if (p->mID != -1)
m_ParticleGrid.concnt++;
if (p->pID != -1)
m_ParticleGrid.concnt++;
p->label = 0;
}
else
{
fprintf(stderr,"error: cannot allocate particle, con. indices will be wrong! \n");
}
}
m_ParticleGrid.concnt /= 2;
m_Iterations = 0;
m_AcceptedProposals = 0;
}