AREXPORT void ArActionDesired::log(void) const
{
// all those maxes and movement parameters
if (getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransVel %.0f", getMaxVel());
if (getMaxNegVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransNegVel %.0f",
getMaxNegVel());
if (getTransAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransAccel %.0f", getTransAccel());
if (getTransDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransDecel %.0f", getTransDecel());
if (getMaxRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tMaxRotVel %.0f", "",
getMaxRotVel());
if (getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotAccel %.0f", "",
getRotAccel());
if (getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotDecel %.0f", "",
getRotDecel());
if (getMaxLeftLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxLeftLatVel %.0f", "",
getMaxLeftLatVel());
if (getMaxRightLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxRightLatVel %.0f", "",
getMaxRightLatVel());
if (getLatAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatAccel %.0f", "",
getLatAccel());
if (getLatDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatDecel %.0f", "",
getLatDecel());
// the actual movement part
if (getVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tVel %.0f", getVel());
if (getHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tHeading %.0f", "",
getHeading());
if (getDeltaHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tDeltaHeading %.0f", "",
getDeltaHeading());
if (getRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotVel %.0f", "",
getRotVel());
if (getLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatVel %.0f", "",
getLatVel());
}
// Do the main part of my calculations. This function fills in the array
// of times in each of the 7 possible sub-segments.
void CalcTimes( void )
{
double Ve = velEnd;
double Vs = getVelStart();
double P = length;
double A = getMaxAcc();
double D = getMaxDec();
double V = getMaxVel();
double J = getMaxJrk();
// We start out assuming that we will hit the max velocity.
// If this calculation is successful, then we're done
if( CalcForVel( V ) )
return;
// Make a quick check here for a zero length segment.
if( P <= 0.0 )
{
for( int i=0; i<7; i++ ) SegT[i] = 0;
return;
}
// OK, we aren't going to hit the velocity limit in this move.
// I'll try running at the accel & decel limits only
double ta = ( sqrt( 8*A*D*J*J*P*(A+D) +
4*J*J*(Vs*Vs*D*D + (Vs*Vs+Ve*Ve)*A*D + Ve*Ve*A*A) -
4*A*D*J*(Ve*D*D + (Vs+Ve)*A*D + Vs*A*A) +
A*A*D*D*( D*D + 2*A*D + A*A )
)
-2*J*Vs*(A+D) -A*D*D -3*A*A*D - 2*A*A*A
)
/ ( 2*A*J*(A+D) );
double tj = A/J;
double tk = D/J;
double td = (Vs - Ve + J*tj*ta + J*tj*tj - J*tk*tk)/(J*tk);
// If both of these times came out positive, we're done
if( (ta >= 0.0) && (td >= 0.0) )
{
SegT[0] = tj;
SegT[1] = ta;
SegT[2] = tj;
SegT[3] = 0;
SegT[4] = tk;
SegT[5] = td;
SegT[6] = tk;
return;
}
// We can't reach both the accel & decel limits.
// There isn't a simple formulat for calculating out the optimal times
// in this case, so I'll itterate with various maximum velocities until
// I find one that will work.
double Vup, Vdn;
// I'll find the max velocity that I'd use without jerk limiting as an
// initial upper limit
CalcNoJrk();
Vup = SegV[1];
// For my lower limit, I'll pick the higher of the starting or ending velocity
Vdn = (Vs>Ve) ? Vs : Ve;
// First calculation uses the lower velocity limit.
// This should never fail.
CalcForVel( Vdn, true );
// Itterate as many as 10 times to try to get a faster move.
// I'll quit when my constant velocity segment is less then
// 1 millisecond long.
for( int i=0; (i<10) && (SegT[3] > MIN_PVT_TIME); i++ )
{
V = (Vup+Vdn)/2;
if( CalcForVel( V ) )
Vdn = V;
else
Vup = V;
}
return;
}
// Calculate run times with no jerk limits. This is quicker & simpler
// then the full blown calculations that include jerk limits.
virtual void CalcNoJrk( void )
{
double ve = velEnd;
double vs = getVelStart();
double P = length;
double A = getMaxAcc();
double D = getMaxDec();
double V = getMaxVel();
// Assume for the moment that we will hit our accel & decel limits.
double ta = (V-vs) / A;
double td = (V-ve) / D;
double tv;
double remain = P - (vs*ta + ta*ta*A/2) - (ve*td + td*td*D/2);
if( remain >= 0 )
tv = remain / V;
else
{
ta = sqrt( (A+D)*(D*vs*vs + A*ve*ve + 2*A*D*P) ) / (A*(A+D)) - vs/A;
td = (A*ta+vs-ve)/D;
tv = 0.0;
}
// Set the times for each sub-segment
SegT[0] = 0.0;
SegT[1] = ta;
SegT[2] = 0.0;
SegT[3] = tv;
SegT[4] = 0.0;
SegT[5] = td;
SegT[6] = 0.0;
// Set the acceleration at the end of each sub-segment
SegA[0] = A;
SegA[1] = 0;
SegA[2] = 0;
SegA[3] = 0;
SegA[4] = -D;
SegA[5] = 0;
SegA[6] = 0;
// Fill in the position, and velocity for
// the end of each of the sub-segments.
double p = 0;
double v = getVelStart();
SegP[0] = 0;
SegV[0] = v;
for( int i=1; i<7; i+=2 )
{
double t = SegT[i];
double a = SegA[i-1];
p += v*t + a*t*t/2;
v += a*t;
SegP[i] = SegP[i+1] = p;
SegV[i] = SegV[i+1] = v;
}
return;
}
// Return the maximum velocity increase possible for this segment
// based on the length of the segment and it's limits.
// We assume some known starting velocity.
//
// @param Vs starting velocity to use
// @param A Acceleration limit to use
// @return Maximum ending velocity
virtual double getMaxVelInc( double Vs, double A )
{
double P = getLength();
double Vmax = getMaxVel();
// If we aren't using jerk limiting, then this is
// simply the starting velocity plus the amount that we
// can accelerate during this segment.
if( !usingJerkLimits() )
{
double v = sqrt( Vs*Vs + 2*A*P );
if( v > Vmax ) v = Vmax;
return v;
}
// If we are using jerk limits, this is considerably more complex.
// Note that we are assuming that acceleration is zero between
// segments to keep things from getting too hairy.
//
// First, see what our max veloctiy would be if we only used
// the jerk and position limits.
//
// P = 2*Vs*t + J*t^3
// Vs = starting velocity
// P = total segment length
// J = jerk limit
// t = half time for segment
//
// solve this for t:
double J = getMaxJrk();
double M = pow( sqrt( (32*Vs*Vs*Vs+27*J*P*P)/(108*J*J*J) )+P/(2*J), 1.0/3.0);
double t = M -(2*Vs)/(3*J*M);
// The peak acceleration would be at time t. Make sure this doesn't
// exceed my limit.
if( J*t <= A )
{
// OK, we didn't exceed the accel limit, so find the max ending
// velocity. If this is over my maximum then just return the max.
double v = Vs + J * t * t;
if( v > Vmax ) v = Vmax;
return v;
}
// We exceeded the accel limit, so I'll recalculate using that limit
// as well as my jerk limit. Basically, I'll split the segment into
// three parts, two parts will run at the jerk limits and one will
// run at the accel limit
// Find the time it will take to run at the jerk limit
double tj = A/J;
// Find the time at the accel limit
double AA = A*A;
double AAAA = AA*AA;
double JJ = J*J;
double ta = (sqrt(8*A*JJ*P + 4*Vs*Vs*JJ - 4*Vs*AA*J + AAAA) - 2*Vs*J - 3*AA)/(2*A*J);
// Find the velocity at the end of those times
double v = Vs + A*(tj+ta);
// Limit based on our max
if( v > Vmax ) v = Vmax;
return v;
}
//Do the interpolation calculations
bool SIM_SnowSolver::solveGasSubclass(SIM_Engine &engine, SIM_Object *obj, SIM_Time time, SIM_Time framerate){
/// STEP #0: Retrieve all data objects from Houdini
//Scalar params
freal particle_mass = getPMass();
freal YOUNGS_MODULUS = getYoungsModulus();
freal POISSONS_RATIO = getPoissonsRatio();
freal CRIT_COMPRESS = getCritComp();
freal CRIT_STRETCH = getCritStretch();
freal FLIP_PERCENT = getFlipPercent();
freal HARDENING = getHardening();
freal CFL = getCfl();
freal COF = getCof();
freal division_size = getDivSize();
freal max_vel = getMaxVel();
//Vector params
vector3 GRAVITY = getGravity();
vector3 bbox_min_limit = getBboxMin();
vector3 bbox_max_limit = getBboxMax();
//Particle params
UT_String s_p, s_vol, s_den, s_vel, s_fe, s_fp;
getParticles(s_p);
getPVol(s_vol);
getPD(s_den);
getPVel(s_vel);
getPFe(s_fe);
getPFp(s_fp);
SIM_Geometry* geometry = (SIM_Geometry*) obj->getNamedSubData(s_p);
if (!geometry) return true;
//Get particle data
//Do we use the attribute name???
// GU_DetailHandle gdh = geometry->getGeometry().getWriteableCopy();
GU_DetailHandle gdh = geometry->getOwnGeometry();
const GU_Detail* gdp_in = gdh.readLock(); // Must unlock later
GU_Detail* gdp_out = gdh.writeLock();
GA_RWAttributeRef p_ref_position = gdp_out->findPointAttribute("P");
GA_RWHandleT<vector3> p_position(p_ref_position.getAttribute());
GA_RWAttributeRef p_ref_volume = gdp_out->findPointAttribute(s_vol);
GA_RWHandleT<freal> p_volume(p_ref_volume.getAttribute());
GA_RWAttributeRef p_ref_density = gdp_out->findPointAttribute(s_den);
GA_RWHandleT<freal> p_density(p_ref_density.getAttribute());
GA_RWAttributeRef p_ref_vel = gdp_out->findPointAttribute(s_vel);
GA_RWHandleT<vector3> p_vel(p_ref_vel.getAttribute());
GA_RWAttributeRef p_ref_Fe = gdp_out->findPointAttribute(s_fe);
GA_RWHandleT<matrix3> p_Fe(p_ref_Fe.getAttribute());
GA_RWAttributeRef p_ref_Fp = gdp_out->findPointAttribute(s_fp);
GA_RWHandleT<matrix3> p_Fp(p_ref_Fp.getAttribute());
//EVALUATE PARAMETERS
freal mu = YOUNGS_MODULUS/(2+2*POISSONS_RATIO);
freal lambda = YOUNGS_MODULUS*POISSONS_RATIO/((1+POISSONS_RATIO)*(1-2*POISSONS_RATIO));
//Get grid data
SIM_ScalarField *g_mass_field;
SIM_DataArray g_mass_data;
getMatchingData(g_mass_data, obj, MPM_G_MASS);
g_mass_field = SIM_DATA_CAST(g_mass_data(0), SIM_ScalarField);
SIM_VectorField *g_nvel_field;
SIM_DataArray g_nvel_data;
getMatchingData(g_nvel_data, obj, MPM_G_NVEL);
g_nvel_field = SIM_DATA_CAST(g_nvel_data(0), SIM_VectorField);
SIM_VectorField *g_ovel_field;
SIM_DataArray g_ovel_data;
getMatchingData(g_ovel_data, obj, MPM_G_OVEL);
g_ovel_field = SIM_DATA_CAST(g_ovel_data(0), SIM_VectorField);
SIM_ScalarField *g_active_field;
SIM_DataArray g_active_data;
getMatchingData(g_active_data, obj, MPM_G_ACTIVE);
g_active_field = SIM_DATA_CAST(g_active_data(0), SIM_ScalarField);
SIM_ScalarField *g_density_field;
SIM_DataArray g_density_data;
getMatchingData(g_density_data, obj, MPM_G_DENSITY);
g_density_field = SIM_DATA_CAST(g_density_data(0), SIM_ScalarField);
SIM_ScalarField *g_col_field;
SIM_DataArray g_col_data;
getMatchingData(g_col_data, obj, MPM_G_COL);
g_col_field = SIM_DATA_CAST(g_col_data(0), SIM_ScalarField);
SIM_VectorField *g_colVel_field;
SIM_DataArray g_colVel_data;
getMatchingData(g_colVel_data, obj, MPM_G_COLVEL);
g_colVel_field = SIM_DATA_CAST(g_colVel_data(0), SIM_VectorField);
SIM_VectorField *g_extForce_field;
SIM_DataArray g_extForce_data;
getMatchingData(g_extForce_data, obj, MPM_G_EXTFORCE);
g_extForce_field = SIM_DATA_CAST(g_extForce_data(0), SIM_VectorField);
UT_VoxelArrayF
*g_mass = g_mass_field->getField()->fieldNC(),
*g_nvelX = g_nvel_field->getField(0)->fieldNC(),
*g_nvelY = g_nvel_field->getField(1)->fieldNC(),
*g_nvelZ = g_nvel_field->getField(2)->fieldNC(),
*g_ovelX = g_ovel_field->getField(0)->fieldNC(),
*g_ovelY = g_ovel_field->getField(1)->fieldNC(),
*g_ovelZ = g_ovel_field->getField(2)->fieldNC(),
*g_colVelX = g_colVel_field->getField(0)->fieldNC(),
*g_colVelY = g_colVel_field->getField(1)->fieldNC(),
*g_colVelZ = g_colVel_field->getField(2)->fieldNC(),
*g_extForceX = g_extForce_field->getField(0)->fieldNC(),
*g_extForceY = g_extForce_field->getField(1)->fieldNC(),
*g_extForceZ = g_extForce_field->getField(2)->fieldNC(),
*g_col = g_col_field->getField()->fieldNC(),
*g_active = g_active_field->getField()->fieldNC();
int point_count = gdp_out->getPointRange().getEntries();
std::vector<boost::array<freal,64> > p_w(point_count);
std::vector<boost::array<vector3,64> > p_wgh(point_count);
//Get world-to-grid conversion ratios
//Particle's grid position can be found via (pos - grid_origin)/voxel_dims
vector3
voxel_dims = g_mass_field->getVoxelSize(),
grid_origin = g_mass_field->getOrig(),
grid_divs = g_mass_field->getDivisions();
//Houdini uses voxel centers for grid nodes, rather than grid corners
grid_origin += voxel_dims/2.0;
freal voxelArea = voxel_dims[0]*voxel_dims[1]*voxel_dims[2];
/*
//Reset grid
for(int iX=0; iX < grid_divs[0]; iX++){
for(int iY=0; iY < grid_divs[1]; iY++){
for(int iZ=0; iZ < grid_divs[2]; iZ++){
g_mass->setValue(iX,iY,iZ,0);
g_active->setValue(iX,iY,iZ,0);
g_ovelX->setValue(iX,iY,iZ,0);
g_ovelY->setValue(iX,iY,iZ,0);
g_ovelZ->setValue(iX,iY,iZ,0);
g_nvelX->setValue(iX,iY,iZ,0);
g_nvelY->setValue(iX,iY,iZ,0);
g_nvelZ->setValue(iX,iY,iZ,0);
}
}
}
*/
/// STEP #1: Transfer mass to grid
if (p_position.isValid()){
//Iterate through particles
for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
//Get grid position
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
//g_mass_field->posToIndex(p_position.get(pid),p_gridx,p_gridy,p_gridz);
freal particle_density = p_density.get(pid);
//Compute weights and transfer mass
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
//Z-dimension interpolation
freal z_pos = gpos[2]-z,
wz = SIM_SnowSolver::bspline(z_pos),
dz = SIM_SnowSolver::bsplineSlope(z_pos);
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
//Y-dimension interpolation
freal y_pos = gpos[1]-y,
wy = SIM_SnowSolver::bspline(y_pos),
dy = SIM_SnowSolver::bsplineSlope(y_pos);
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
//X-dimension interpolation
freal x_pos = gpos[0]-x,
wx = SIM_SnowSolver::bspline(x_pos),
dx = SIM_SnowSolver::bsplineSlope(x_pos);
//Final weight is dyadic product of weights in each dimension
freal weight = wx*wy*wz;
p_w[pid-1][idx] = weight;
//Weight gradient is a vector of partial derivatives
p_wgh[pid-1][idx] = vector3(dx*wy*wz, wx*dy*wz, wx*wy*dz)/voxel_dims;
//Interpolate mass
freal node_mass = g_mass->getValue(x,y,z);
node_mass += weight*particle_mass;
g_mass->setValue(x,y,z,node_mass);
}
}
}
}
}
/// STEP #2: First timestep only - Estimate particle volumes using grid mass
/*
if (time == 0.0){
//Iterate through particles
for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
freal density = 0;
//Get grid position
int p_gridx = 0, p_gridy = 0, p_gridz = 0;
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
//g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz);
//Transfer grid density (within radius) to particles
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
//Transfer density
density += w * g_mass->getValue(x,y,z);
}
}
}
}
density /= voxelArea;
p_density.set(pid,density);
p_volume.set(pid, particle_mass/density);
}
}
//*/
/// STEP #3: Transfer velocity to grid
//This must happen after transferring mass, to conserve momentum
//Iterate through particles and transfer
for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
vector3 vel_fac = p_vel.get(pid)*particle_mass;
//Get grid position
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
//g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz);
//Transfer to grid nodes within radius
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
freal nodex_vel = g_ovelX->getValue(x,y,z) + vel_fac[0]*w;
freal nodey_vel = g_ovelY->getValue(x,y,z) + vel_fac[1]*w;
freal nodez_vel = g_ovelZ->getValue(x,y,z) + vel_fac[2]*w;
g_ovelX->setValue(x,y,z,nodex_vel);
g_ovelY->setValue(x,y,z,nodey_vel);
g_ovelZ->setValue(x,y,z,nodez_vel);
g_active->setValue(x,y,z,1.0);
}
}
}
}
}
//Division is slow (maybe?); we only want to do divide by mass once, for each active node
for(int iX=0; iX < grid_divs[0]; iX++){
for(int iY=0; iY < grid_divs[1]; iY++){
for(int iZ=0; iZ < grid_divs[2]; iZ++){
//Only check nodes that have mass
if (g_active->getValue(iX,iY,iZ)){
freal node_mass = 1/(g_mass->getValue(iX,iY,iZ));
g_ovelX->setValue(iX,iY,iZ,(g_ovelX->getValue(iX,iY,iZ)*node_mass));
g_ovelY->setValue(iX,iY,iZ,(g_ovelY->getValue(iX,iY,iZ)*node_mass));
g_ovelZ->setValue(iX,iY,iZ,(g_ovelZ->getValue(iX,iY,iZ)*node_mass));
}
}
}
}
/// STEP #4: Compute new grid velocities
//Temporary variables for plasticity and force calculation
//We need one set of variables for each thread that will be running
eigen_matrix3 def_elastic, def_plastic, energy, svd_u, svd_v;
Eigen::JacobiSVD<eigen_matrix3, Eigen::NoQRPreconditioner> svd;
eigen_vector3 svd_e;
matrix3 HDK_def_plastic, HDK_def_elastic, HDK_energy;
freal* data_dp = HDK_def_plastic.data();
freal* data_de = HDK_def_elastic.data();
freal* data_energy = HDK_energy.data();
//Map Eigen matrices to HDK matrices
Eigen::Map<eigen_matrix3> data_dp_map(data_dp);
Eigen::Map<eigen_matrix3> data_de_map(data_de);
Eigen::Map<eigen_matrix3> data_energy_map(data_energy);
//Compute force at each particle and transfer to Eulerian grid
//We use "nvel" to hold the grid force, since that variable is not in use
for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
//Apply plasticity to deformation gradient, before computing forces
//We need to use the Eigen lib to do the SVD; transfer houdini matrices to Eigen matrices
HDK_def_plastic = p_Fp.get(pid);
HDK_def_elastic = p_Fe.get(pid);
def_plastic = Eigen::Map<eigen_matrix3>(data_dp);
def_elastic = Eigen::Map<eigen_matrix3>(data_de);
//Compute singular value decomposition (uev*)
svd.compute(def_elastic, Eigen::ComputeFullV | Eigen::ComputeFullU);
svd_e = svd.singularValues();
svd_u = svd.matrixU();
svd_v = svd.matrixV();
//Clamp singular values
for (int i=0; i<3; i++){
if (svd_e[i] < CRIT_COMPRESS)
svd_e[i] = CRIT_COMPRESS;
else if (svd_e[i] > CRIT_STRETCH)
svd_e[i] = CRIT_STRETCH;
}
//Put SVD back together for new elastic and plastic gradients
def_plastic = svd_v * svd_e.asDiagonal().inverse() * svd_u.transpose() * def_elastic * def_plastic;
svd_v.transposeInPlace();
def_elastic = svd_u * svd_e.asDiagonal() * svd_v;
//Now compute the energy partial derivative (which we use to get force at each grid node)
energy = 2*mu*(def_elastic - svd_u*svd_v)*def_elastic.transpose();
//Je is the determinant of def_elastic (equivalent to svd_e.prod())
freal Je = svd_e.prod(),
contour = lambda*Je*(Je-1),
jp = def_plastic.determinant(),
particle_vol = p_volume.get(pid);
for (int i=0; i<3; i++)
energy(i,i) += contour;
energy *= particle_vol * exp(HARDENING*(1-jp));
//Transfer Eigen matrices back to HDK
data_dp_map = def_plastic;
data_de_map = def_elastic;
data_energy_map = energy;
p_Fp.set(pid,HDK_def_plastic);
p_Fe.set(pid,HDK_def_elastic);
//Transfer energy to surrounding grid nodes
vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
vector3 ngrad = p_wgh[pid-1][idx];
g_nvelX->setValue(x,y,z,g_nvelX->getValue(x,y,z) + ngrad.dot(HDK_energy[0]));
g_nvelY->setValue(x,y,z,g_nvelY->getValue(x,y,z) + ngrad.dot(HDK_energy[1]));
g_nvelZ->setValue(x,y,z,g_nvelZ->getValue(x,y,z) + ngrad.dot(HDK_energy[2]));
}
}
}
}
}
//Use new forces to solve for new velocities
for(int iX=0; iX < grid_divs[0]; iX++){
for(int iY=0; iY < grid_divs[1]; iY++){
for(int iZ=0; iZ < grid_divs[2]; iZ++){
//Only compute for active nodes
if (g_active->getValue(iX,iY,iZ)){
freal nodex_ovel = g_ovelX->getValue(iX,iY,iZ);
freal nodey_ovel = g_ovelY->getValue(iX,iY,iZ);
freal nodez_ovel = g_ovelZ->getValue(iX,iY,iZ);
freal forcex = g_nvelX->getValue(iX,iY,iZ);
freal forcey = g_nvelY->getValue(iX,iY,iZ);
freal forcez = g_nvelZ->getValue(iX,iY,iZ);
freal node_mass = 1/(g_mass->getValue(iX,iY,iZ));
freal ext_forceX = GRAVITY[0] + g_extForceX->getValue(iX,iY,iZ);
freal ext_forceY = GRAVITY[1] + g_extForceY->getValue(iX,iY,iZ);
freal ext_forceZ = GRAVITY[2] + g_extForceZ->getValue(iX,iY,iZ);
nodex_ovel += framerate*(ext_forceX - forcex*node_mass);
nodey_ovel += framerate*(ext_forceY - forcey*node_mass);
nodez_ovel += framerate*(ext_forceZ - forcez*node_mass);
//Limit velocity to max_vel
vector3 g_nvel(nodex_ovel, nodey_ovel, nodez_ovel);
freal nvelNorm = g_nvel.length();
if(nvelNorm > max_vel){
freal velRatio = max_vel/nvelNorm;
g_nvel*= velRatio;
}
g_nvelX->setValue(iX,iY,iZ,g_nvel[0]);
g_nvelY->setValue(iX,iY,iZ,g_nvel[1]);
g_nvelZ->setValue(iX,iY,iZ,g_nvel[2]);
}
}
}
}
/// STEP #5: Grid collision resolution
vector3 sdf_normal;
//*
for(int iX=1; iX < grid_divs[0]-1; iX++){
for(int iY=1; iY < grid_divs[1]-1; iY++){
for(int iZ=1; iZ < grid_divs[2]-1; iZ++){
if (g_active->getValue(iX,iY,iZ)){
if (!computeSDFNormal(g_col, iX, iY, iZ, sdf_normal))
continue;
//Collider velocity
vector3 vco(
g_colVelX->getValue(iX,iY,iZ),
g_colVelY->getValue(iX,iY,iZ),
g_colVelZ->getValue(iX,iY,iZ)
);
//Grid velocity
vector3 v(
g_nvelX->getValue(iX,iY,iZ),
g_nvelY->getValue(iX,iY,iZ),
g_nvelZ->getValue(iX,iY,iZ)
);
//Skip if bodies are separating
vector3 vrel = v - vco;
freal vn = vrel.dot(sdf_normal);
if (vn >= 0) continue;
//Resolve collisions; also add velocity of collision object to snow velocity
//Sticks to surface (too slow to overcome static friction)
vector3 vt = vrel - (sdf_normal*vn);
freal stick = vn*COF, vt_norm = vt.length();
if (vt_norm <= -stick)
vt = vco;
//Dynamic friction
else vt += stick*vt/vt_norm + vco;
g_nvelX->setValue(iX,iY,iZ,vt[0]);
g_nvelY->setValue(iX,iY,iZ,vt[1]);
g_nvelZ->setValue(iX,iY,iZ,vt[2]);
}
}
}
}
//*/
/// STEP #6: Transfer grid velocities to particles and integrate
/// STEP #7: Particle collision resolution
vector3 pic, flip, col_vel;
matrix3 vel_grad;
//Iterate through particles
for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
int pid = it.getOffset();
//Particle position
vector3 pos(p_position.get(pid));
//Reset velocity
pic[0] = 0.0;
pic[1] = 0.0;
pic[2] = 0.0;
flip = p_vel.get(pid);
vel_grad.zero();
freal density = 0;
//Get grid position
vector3 gpos = (pos - grid_origin)/voxel_dims;
int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
freal w = p_w[pid-1][idx];
if (w > EPSILON){
const vector3 node_wg = p_wgh[pid-1][idx];
const vector3 node_nvel(
g_nvelX->getValue(x,y,z),
g_nvelY->getValue(x,y,z),
g_nvelZ->getValue(x,y,z)
);
//Transfer velocities
pic += node_nvel*w;
flip[0] += (node_nvel[0] - g_ovelX->getValue(x,y,z))*w;
flip[1] += (node_nvel[1]- g_ovelY->getValue(x,y,z))*w;
flip[2] += (node_nvel[2] - g_ovelZ->getValue(x,y,z))*w;
//Transfer density
density += w * g_mass->getValue(x,y,z);
//Transfer veloctiy gradient
vel_grad.outerproductUpdate(1.0, node_nvel, node_wg);
}
}
}
}
//Finalize velocity update
vector3 vel = flip*FLIP_PERCENT + pic*(1-FLIP_PERCENT);
//Reset collision data
freal col_sdf = 0;
sdf_normal[0] = 0;
sdf_normal[1] = 0;
sdf_normal[2] = 0;
col_vel[0] = 0;
col_vel[1] = 0;
col_vel[2] = 0;
//Interpolate surrounding nodes' SDF info to the particle (trilinear interpolation)
for (int idx=0, z=p_gridz, z_end=z+1; z<=z_end; z++){
freal w_z = gpos[2]-z;
for (int y=p_gridy, y_end=y+1; y<=y_end; y++){
freal w_zy = w_z*(gpos[1]-y);
for (int x=p_gridx, x_end=x+1; x<=x_end; x++, idx++){
freal weight = fabs(w_zy*(gpos[0]-x));
//cout << w_zy << "," << (gpos[0]-x) << "," << weight << endl;
vector3 temp_normal;
computeSDFNormal(g_col, x, y, z, temp_normal);
//goto SKIP_PCOLLIDE;
//cout << g_col->getValue(x, y, z) << endl;
//Interpolate
sdf_normal += temp_normal*weight;
col_sdf += g_col->getValue(x, y, z)*weight;
col_vel[0] += g_colVelX->getValue(x, y, z)*weight;
col_vel[1] += g_colVelY->getValue(x, y, z)*weight;
col_vel[2] += g_colVelZ->getValue(x, y, z)*weight;
}
}
}
//Resolve particle collisions
//cout << col_sdf << endl;
if (col_sdf > 0){
vector3 vrel = vel - col_vel;
freal vn = vrel.dot(sdf_normal);
//Skip if bodies are separating
if (vn < 0){
//Resolve and add velocity of collision object to snow velocity
//Sticks to surface (too slow to overcome static friction)
vel = vrel - (sdf_normal*vn);
freal stick = vn*COF, vel_norm = vel.length();
if (vel_norm <= -stick)
vel = col_vel;
//Dynamic friction
else vel += stick*vel/vel_norm + col_vel;
}
}
SKIP_PCOLLIDE:
//Finalize density update
density /= voxelArea;
p_density.set(pid,density);
//Update particle position
pos += framerate*vel;
//Limit particle position
int mask = 0;
/*
for (int i=0; i<3; i++){
if (pos[i] > bbox_max_limit[i]){
pos[i] = bbox_max_limit[i];
vel[i] = 0.0;
mask |= 1 << i;
}
else if (pos[i] < bbox_min_limit[i]){
pos[i] = bbox_min_limit[i];
vel[i] = 0.0;
mask |= 1 << i;
}
}
//Slow particle down at bounds (not really necessary...)
if (mask){
for (int i=0; i<3; i++){
if (mask & 0x1)
vel[i] *= .05;
mask >>= 1;
}
}//*/
p_vel.set(pid,vel);
p_position.set(pid,pos);
//Update particle deformation gradient
//Note: plasticity is computed on the next timestep...
vel_grad *= framerate;
vel_grad(0,0) += 1;
vel_grad(1,1) += 1;
vel_grad(2,2) += 1;
p_Fe.set(pid, vel_grad*p_Fe.get(pid));
}
gdh.unlock(gdp_out);
gdh.unlock(gdp_in);
return true;
}
