double DexAnalogMixin::FilterLoadForce( Vector3 load_force ) {
// Combine the new force sample with previous filtered value (recursive filtering).
ScaleVector( filteredLoadForce, filteredLoadForce, filterConstant );
AddVectors( filteredLoadForce, filteredLoadForce, load_force );
ScaleVector( filteredLoadForce, filteredLoadForce, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( load_force, filteredLoadForce );
return( VectorNorm( filteredLoadForce ) );
}
double DexAnalogMixin::FilterCoP( int which_ati, Vector3 center_of_pressure ) {
// Combine the new force sample with previous filtered value (recursive filtering).
ScaleVector( filteredCoP[which_ati], filteredCoP[which_ati], filterConstant );
AddVectors( filteredCoP[which_ati], filteredCoP[which_ati], center_of_pressure );
ScaleVector( filteredCoP[which_ati], filteredCoP[which_ati], 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( center_of_pressure, filteredCoP[which_ati] );
return( VectorNorm( filteredCoP[which_ati] ) );
}
bool IntersectPolygon (TPolygon p, TVector3 *v) {
TRay ray;
TVector3 nml, edge_nml, edge_vec;
TVector3 pt;
double d, s, nuDotProd, wec;
double edge_len, t, distsq;
int i;
nml = MakeNormal (p, v);
ray.pt = MakeVector (0., 0., 0.);
ray.vec = nml;
nuDotProd = DotProduct (nml, ray.vec);
if (fabs(nuDotProd) < EPS)
return false;
d = - (nml.x * v[p.vertices[0]].x +
nml.y * v[p.vertices[0]].y +
nml.z * v[p.vertices[0]].z);
if (fabs (d) > 1) return false;
for (i=0; i < p.num_vertices; i++) {
TVector3 *v0, *v1;
v0 = &v[p.vertices[i]];
v1 = &v[p.vertices[ (i+1) % p.num_vertices ]];
edge_vec = SubtractVectors (*v1, *v0);
edge_len = NormVector (&edge_vec);
t = - DotProduct (*((TVector3 *) v0), edge_vec);
if (t < 0) {
distsq = MAG_SQD2 (*v0);
} else if (t > edge_len) {
distsq = MAG_SQD2 (*v1);
} else {
*v0 = AddVectors (*v0, ScaleVector (t, edge_vec));
distsq = MAG_SQD2 (*v0);
}
if (distsq <= 1) return true;
}
s = - (d + DotProduct (nml, MakeVector (ray.pt.x, ray.pt.y, ray.pt.z))) / nuDotProd;
pt = AddVectors (ray.pt, ScaleVector (s, ray.vec));
for (i=0; i < p.num_vertices; i++) {
edge_nml = CrossProduct (nml,
SubtractVectors (v[p.vertices[ (i+1) % p.num_vertices ]], v[p.vertices[i]]));
wec = DotProduct (SubtractVectors (pt, v[p.vertices[i]]), edge_nml);
if (wec < 0) return false;
}
return true;
}
double DexAnalogMixin::FilterAcceleration( Vector3 acceleration ) {
// Combine the new position sample with previous filtered value (recursive filtering).
ScaleVector( filteredAcceleration, filteredAcceleration, filterConstant );
AddVectors( filteredAcceleration, filteredAcceleration, acceleration );
ScaleVector( filteredAcceleration, filteredAcceleration, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( acceleration, filteredAcceleration );
return( VectorNorm( acceleration ) );
}
double DexAnalogMixin::FilterManipulandumRotations( Vector3 rotations ) {
// Combine the new rotations sample with previous filtered value (recursive filtering).
ScaleVector( filteredManipulandumRotations, filteredManipulandumRotations, filterConstant );
AddVectors( filteredManipulandumRotations, filteredManipulandumRotations, rotations );
ScaleVector( filteredManipulandumRotations, filteredManipulandumRotations, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( rotations, filteredManipulandumRotations );
return( VectorNorm( rotations ) );
}
void CCharShape::AdjustOrientation (CControl *ctrl, double dtime,
double dist_from_surface, TVector3 surf_nml) {
TVector3 new_x, new_y, new_z;
TMatrix cob_mat, inv_cob_mat;
TMatrix rot_mat;
TQuaternion new_orient;
double time_constant;
static TVector3 minus_z_vec = { 0, 0, -1};
static TVector3 y_vec = { 0, 1, 0 };
if (dist_from_surface > 0) {
new_y = ScaleVector (1, ctrl->cvel);
NormVector (&new_y);
new_z = ProjectToPlane (new_y, MakeVector(0, -1, 0));
NormVector (&new_z);
new_z = AdjustRollvector (ctrl, ctrl->cvel, new_z);
} else {
new_z = ScaleVector (-1, surf_nml);
new_z = AdjustRollvector (ctrl, ctrl->cvel, new_z);
new_y = ProjectToPlane (surf_nml, ScaleVector (1, ctrl->cvel));
NormVector(&new_y);
}
new_x = CrossProduct (new_y, new_z);
MakeBasismatrix_Inv (cob_mat, inv_cob_mat, new_x, new_y, new_z);
new_orient = MakeQuaternionFromMatrix (cob_mat);
if (!ctrl->orientation_initialized) {
ctrl->orientation_initialized = true;
ctrl->corientation = new_orient;
}
time_constant = dist_from_surface > 0 ? TO_AIR_TIME : TO_TIME;
ctrl->corientation = InterpolateQuaternions (
ctrl->corientation, new_orient,
min (dtime / time_constant, 1.0));
ctrl->plane_nml = RotateVector (ctrl->corientation, minus_z_vec);
ctrl->cdirection = RotateVector (ctrl->corientation, y_vec);
MakeMatrixFromQuaternion (cob_mat, ctrl->corientation);
// Trick rotations
new_y = MakeVector (cob_mat[1][0], cob_mat[1][1], cob_mat[1][2]);
RotateAboutVectorMatrix (rot_mat, new_y, (ctrl->roll_factor * 360));
MultiplyMatrices (cob_mat, rot_mat, cob_mat);
new_x = MakeVector (cob_mat[0][0], cob_mat[0][1], cob_mat[0][2]);
RotateAboutVectorMatrix (rot_mat, new_x, ctrl->flip_factor * 360);
MultiplyMatrices (cob_mat, rot_mat, cob_mat);
TransposeMatrix (cob_mat, inv_cob_mat);
TransformNode (0, cob_mat, inv_cob_mat);
}
void TScene::RenderPlanet(float radius, Vector pos, int NumMesh)
{
glLoadIdentity();
float dist = DistanceBetweenVector(pos, Camera->GetPosition());
//glPrint(10, 60, "dist: %f", dist);
if (dist < 1000.0f)
{
glTranslatef(pos.x, pos.y, pos.z);
glScalef(radius, radius, radius);
//glPrint(10, 90, "PosPlanet: %f, %f, %f", pos.x, pos.y, pos.z);
}
else
{
Vector cam = Camera->GetPosition();
Vector a = NormalizeVector( cam - pos);
pos = cam - ScaleVector ( a, 1000.0f );
glTranslatef( pos.x, pos.y, pos.z );
float scale = (radius / dist) * 1000.0f;
glScalef(scale, scale, scale);
//glPrint(10, 90, "PosPlanet: %f, %f, %f", pos.x, pos.y, pos.z);
//glPrint(10, 120, "scale: %f", scale);
}
Mesh[NumMesh].Render();
}
double NormVector (TVector3 &v) {
double square = v.x * v.x + v.y * v.y + v.z * v.z;
if (square == 0.0) return 0.0;
double denom = sqrt (square);
v = ScaleVector (1.0 / denom, v);
return denom;
}
double NormVectorN (TVector3 &v) {
double denom = (v.x * v.x + v.y * v.y + v.z * v.z);
if (denom <= 0.0) return 0.0;
denom = sqrt (denom);
v = ScaleVector (1.0 / denom, v);
return denom;
}
double NormVector (TVector3 *v) {
double denom = (v->x * v->x + v->y * v->y + v->z * v->z);
if (denom <= 0.0) return 0.0;
denom = sqrt (denom);
*v = ScaleVector (1.0 / denom, *v);
return denom;
}
double NormVector (TVector3 *v) {
double denom = 1;
if (v->x == 0 && v->y == 0 && v->z == 0) return 0.0;
denom = sqrt (v->x * v->x + v->y * v->y + v->z * v->z);
*v = ScaleVector (1.0 / denom, *v);
return denom;
}
static void DistribVector( VECTOR *d, VECTOR *n, double sa, double sb )
{
VECTOR a, b;
double nl;
if( fabs( n->z ) > EPSILON ) {
a.x = n->y*n->z; a.y = -n->x*n->z; a.z = 0.0;
b.x = a.y*n->z; b.y = -a.x*n->z; b.z = a.x*n->y - a.y*n->x;
} else {
a.x = n->y; a.y = -n->x; a.z = 0.0;
b.x = b.y = 0.0; b.z = 1.0;
}
nl = VectorLength( n );
ScaleVector( &a, sa*(nl/VectorLength( &a ))*Jitter() );
ScaleVector( &b, sb*(nl/VectorLength( &b ))*Jitter() );
d->x = a.x+b.x; d->y = a.y+b.y; d->z = a.z+b.z;
}
///////////////////////////////////////////////////////////////////////////////
// Function: SetMouseForce
// Purpose: Allows the user to interact with selected points by dragging
// Arguments: Delta distance from clicked point, local x and y axes
///////////////////////////////////////////////////////////////////////////////
void CPhysEnv::SetMouseForce(int deltaX,int deltaY, tVector *localX, tVector *localY)
{
/// Local Variables ///////////////////////////////////////////////////////////
tVector tempX,tempY;
///////////////////////////////////////////////////////////////////////////////
ScaleVector(localX, (float)deltaX * 0.03f, &tempX);
ScaleVector(localY, -(float)deltaY * 0.03f, &tempY);
if (m_Pick[0] > -1)
{
VectorSum(&m_CurrentSys[m_Pick[0]].pos,&tempX,&m_MouseDragPos[0]);
VectorSum(&m_MouseDragPos[0],&tempY,&m_MouseDragPos[0]);
}
if (m_Pick[1] > -1)
{
VectorSum(&m_CurrentSys[m_Pick[1]].pos,&tempX,&m_MouseDragPos[1]);
VectorSum(&m_MouseDragPos[1],&tempY,&m_MouseDragPos[1]);
}
}
TVector3 ProjectToPlane (TVector3 nml, TVector3 v){
TVector3 nmlComp;
double dotProd;
dotProd = DotProduct (nml, v);
nmlComp = ScaleVector (dotProd, nml);
return SubtractVectors (v, nmlComp);
}
int DexMouseTracker::RetrieveMarkerFrames( CodaFrame frames[], int max_frames ) {
int n_frames;
int previous_frame;
int next_frame;
Vector3 jump, delta;
double time, interval, offset, relative;
// Copy data into an array.
previous_frame = 0;
next_frame = 0;
// Fill an array of frames at a constant frequency by interpolating the
// frames that were taken at a variable frequency in real time.
for ( n_frames = 0; n_frames < max_frames; n_frames++ ) {
// Compute the time of each slice at a constant sampling frequency.
time = (double) n_frames * samplePeriod;
frames[n_frames].time = (float) time;
// See if we have caught up with the real-time data.
if ( time > recordedMarkerFrames[next_frame].time ) previous_frame = next_frame;
// Find the next real-time frame that has a time stamp later than this one.
// It could be that this true already.
while ( recordedMarkerFrames[next_frame].time <= time && next_frame < nAcqFrames ) next_frame++;
// If we reached the end of the real-time samples, then we are done.
if ( next_frame >= nAcqFrames ) break;
// Compute the time difference between the two adjacent real-time frames.
interval = recordedMarkerFrames[next_frame].time - recordedMarkerFrames[previous_frame].time;
// Compute the time between the current frame and the previous real_time frame.
offset = time - recordedMarkerFrames[previous_frame].time;
// Use the relative time to interpolate.
relative = offset / interval;
for ( int j = 0; j < nMarkers; j++ ) {
frames[n_frames].marker[j].visibility =
recordedMarkerFrames[previous_frame].marker[j].visibility &&
recordedMarkerFrames[next_frame].marker[j].visibility;
if ( frames[n_frames].marker[j].visibility ) {
SubtractVectors( jump,
recordedMarkerFrames[next_frame].marker[j].position,
recordedMarkerFrames[previous_frame].marker[j].position );
ScaleVector( delta, jump, (float) relative );
AddVectors( frames[n_frames].marker[j].position, recordedMarkerFrames[previous_frame].marker[j].position, delta );
}
}
}
return( n_frames );
}
void CPhysEnv::ResolveCollisions( tParticle *system )
{
tContact *contact;
tParticle *particle; // THE PARTICLE COLLIDING
float VdotN;
tVector Vn,Vt; // CONTACT RESOLUTION IMPULSE
contact = m_Contact;
for (int loop = 0; loop < m_ContactCnt; loop++,contact++)
{
particle = &system[contact->particle];
// CALCULATE Vn
VdotN = DotProduct(&contact->normal,&particle->v);
ScaleVector(&contact->normal, VdotN, &Vn);
// CALCULATE Vt
VectorDifference(&particle->v, &Vn, &Vt);
// SCALE Vn BY COEFFICIENT OF RESTITUTION
ScaleVector(&Vn, m_Kr, &Vn);
// SET THE VELOCITY TO BE THE NEW IMPULSE
VectorDifference(&Vt, &Vn, &particle->v);
}
}
TVector3 CCourse::FindCourseNormal (double x, double z) const {
double *elevation = Course.elevation;
int x0, x1, y0, y1;
GetIndicesForPoint (x, z, &x0, &y0, &x1, &y1);
TIndex2 idx0, idx1, idx2;
double u, v;
FindBarycentricCoords (x, z, &idx0, &idx1, &idx2, &u, &v);
const TVector3& n0 = Course.nmls[ idx0.i + nx * idx0.j ];
const TVector3& n1 = Course.nmls[ idx1.i + nx * idx1.j ];
const TVector3& n2 = Course.nmls[ idx2.i + nx * idx2.j ];
TVector3 p0 = COURSE_VERTX (idx0.i, idx0.j);
TVector3 p1 = COURSE_VERTX (idx1.i, idx1.j);
TVector3 p2 = COURSE_VERTX (idx2.i, idx2.j);
TVector3 smooth_nml = AddVectors (
ScaleVector (u, n0),
AddVectors (ScaleVector (v, n1), ScaleVector (1.-u-v, n2)));
TVector3 tri_nml = CrossProduct (
SubtractVectors (p1, p0), SubtractVectors (p2, p0));
NormVector (tri_nml);
double min_bary = min (u, min (v, 1. - u - v));
double interp_factor = min (min_bary / NORM_INTERPOL, 1.0);
TVector3 interp_nml = AddVectors (
ScaleVector (interp_factor, tri_nml),
ScaleVector (1.-interp_factor, smooth_nml));
NormVector (interp_nml);
return interp_nml;
}
/////////////////////////////////////////////////////////////////////////////
// Rasterizaiton of an primtive in AABB
// v=primtive vertices buffer address, nV=vertices count
// nCidLimit=the max length of cell id buffer
// pCid=cell id buffer address
// nCid=number of rasterized cell id
inline void COMap::RasterAABB(vertex* v, long nV, DWORD* pCid, long& nCid, const long nCidLimit)
{
long i,j,k;
DWORD dwCell;
vertex vmin, vmax; //AABB
long xmin, ymin, zmin, xmax, ymax, zmax; // Rasterized AABB
ScaleVector(v, TOMM, v);
vmin.x=vmax.x=(*v).x;
vmin.y=vmax.y=(*v).y;
vmin.z=vmax.z=(*v).z;
for(i=1;i<nV;i++)
{
vmin.x=__min(vmin.x, (*(v+i)).x);
vmax.x=__max(vmax.x, (*(v+i)).x);
vmin.y=__min(vmin.y, (*(v+i)).y);
vmax.y=__max(vmax.y, (*(v+i)).y);
vmin.z=__min(vmin.z, (*(v+i)).z);
vmax.z=__max(vmax.z, (*(v+i)).z);
}
xmin=Real2Int(vmin.x)/m_dwR;
xmax=Real2Int(vmax.x)/m_dwR;
ymin=Real2Int(vmin.y)/m_dwR;
ymax=Real2Int(vmax.y)/m_dwR;
zmin=Real2Int(vmin.z)/m_dwR;
zmax=Real2Int(vmax.z)/m_dwR;
nCid=0;
for(k=zmin;k<=zmax;k++)
for(j=ymin;j<=ymax;j++)
for(i=xmin;i<=xmax;i++)
{
if(nCid>=nCidLimit)
{
//TRACE("Cell id RASTER_SIZE overflows.\n");
}
else{
dwCell=i+50+(j+50)*100+(k+50)*10000; // x, y, z [-500,500], Related to dwR.
*(pCid+nCid++)=dwCell;
}
}
}
static void TraceScene(void)
{
VECTOR PixColor, Col, LinD, Scale;
VECTOR LinD2, D;
int sx, sy, i;
Scale.y = 1.0;
for( sy = 0; sy < HEIGHT; sy++ ) {
Scale.z = ((double)(HEIGHT/2-sy))/(double)HEIGHT;
for( sx = 0; sx < WIDTH; sx++ ) {
Scale.x = ((double)(sx-WIDTH/2))/(double)WIDTH;
/* Calculate line-direction (from camera-center through a pixel) */
LinD.x = Cameraright.x*Scale.x + Cameradir.x*Scale.y + Cameraup.x*Scale.z;
LinD.y = Cameraright.y*Scale.x + Cameradir.y*Scale.y + Cameraup.y*Scale.z;
LinD.z = Cameraright.z*Scale.x + Cameradir.z*Scale.y + Cameraup.z*Scale.z;
/* Get color for pixel */
#if (DISTLEVELS > 0)
PixColor.x = PixColor.y = PixColor.z = 0.0;
for( i = 0; i < DISTRIB; i++ ) {
DistribVector( &D, &LinD, 0.5/(double)WIDTH, 0.5/(double)HEIGHT );
LinD2 = LinD; LinD2.x += D.x; LinD2.y += D.y; LinD2.z += D.z;
TraceLine( &Camerapos, &LinD2, &Col, MAXREC );
PixColor.x += Col.x;
PixColor.y += Col.y;
PixColor.z += Col.z;
}
ScaleVector( &PixColor, 1.0/DISTRIB );
#else
TraceLine( &Camerapos, &LinD, &PixColor, MAXREC );
#endif
memory[3*(sx+sy*WIDTH)]=(UBYTE)(PixColor.x*255.0);
memory[3*(sx+sy*WIDTH)+1]=(UBYTE)(PixColor.y*255.0);
memory[3*(sx+sy*WIDTH)+2]=(UBYTE)(PixColor.z*255.0);
}
}
}
// Calculate forces at imperfect interfaces and both CrackVelocityFields are present and have particles
// Return TRUE if imperfect interface or FALSE if not
// Only for cracks as imperfect interfaces
bool CrackSurfaceContact::GetInterfaceForceOnCrack(NodalPoint *np,Vector *fImp,CrackVelocityField *cva,
CrackVelocityField *cvb,Vector *unnorm,int number,double *rawEnergy,double nodalx)
{
// no forces needed if really perfect, was handled by contact momentum change
if(crackContactLaw[number]->IsPerfectTangentialInterface() && crackContactLaw[number]->IsPerfectNormalInterface())
{ return false;
}
// displacement or position
Vector da,db;
double mnode=1./cva->GetTotalMass(true);
Vector dispa=cva->GetCMDisplacement(np,true);
da.x=dispa.x*mnode;
da.y=dispa.y*mnode;
mnode=1./cvb->GetTotalMass(true);
Vector dispb=cvb->GetCMDisplacement(np,true);
db.x=dispb.x*mnode;
db.y=dispb.y*mnode;
// normal vector (assumes 2D because this is for cracks only)
Vector norm = *unnorm;
ScaleVector(&norm,1./sqrt(norm.x*norm.x+norm.y*norm.y));
// Angled path correction (2D only)
double dist = mpmgrid.GetPerpendicularDistance(&norm, NULL, 0.);
// Area correction method (new): sqrt(2*vmin/vtot)*vtot/dist = sqrt(2*vmin*vtot)/dist
double vola = cva->GetVolumeNonrigid(true),volb = cvb->GetVolumeNonrigid(true),voltot=vola+volb;
double surfaceArea = sqrt(2.0*fmin(vola,volb)*voltot)/dist;
// If axisymmetric, multiply by radial position (vola, volb above were areas)
if(fmobj->IsAxisymmetric()) surfaceArea *= nodalx;
// pass to imperfect interface law
return crackContactLaw[number]->GetCrackInterfaceForce(&da,&db,&norm,surfaceArea,dist,fImp,rawEnergy);
}
void CPhysEnv::ApplyUserForce(tVector *force)
{
ScaleVector(force, m_UserForceMag, &m_UserForce);
m_UserForceActive = TRUE;
}
// Update particle position, velocity, temp, and conc
// throws CommonException()
void UpdateParticlesTask::Execute(void)
{
CommonException *upErr = NULL;
#ifdef CONST_ARRAYS
int ndsArray[MAX_SHAPE_NODES];
double fn[MAX_SHAPE_NODES];
#else
int ndsArray[maxShapeNodes];
double fn[maxShapeNodes];
#endif
// Damping terms on the grid or on the particles
// particleAlpha = (1-beta)/dt + pdamping(t)
// gridAlpha = -m*(1-beta)/dt + damping(t)
double particleAlpha = bodyFrc.GetParticleDamping(mtime);
double gridAlpha = bodyFrc.GetDamping(mtime);
// nonPICGridAlpha = damping(t)
// globalPIC = (1-beta)/dt
double nonPICGridAlpha = bodyFrc.GetNonPICDamping(mtime);
double globalPIC = bodyFrc.GetPICDamping();
// Update particle position, velocity, temp, and conc
#pragma omp parallel for private(ndsArray,fn)
for(int p=0;p<nmpmsNR;p++)
{ MPMBase *mpmptr = mpm[p];
try
{ // get shape functions
const ElementBase *elemRef = theElements[mpmptr->ElemID()];
int *nds = ndsArray;
elemRef->GetShapeFunctions(fn,&nds,mpmptr);
int numnds = nds[0];
// Update particle position and velocity
const MaterialBase *matRef=theMaterials[mpmptr->MatID()];
int matfld=matRef->GetField();
// Allow material to override global settings
double localParticleAlpha = particleAlpha;
double localGridAlpha = gridAlpha;
matRef->GetMaterialDamping(localParticleAlpha,localGridAlpha,nonPICGridAlpha,globalPIC);
// data structure for extrapolations
GridToParticleExtrap *gp = new GridToParticleExtrap;
// acceleration on the particle
gp->acc = mpmptr->GetAcc();
ZeroVector(gp->acc);
// extrapolate nodal velocity from grid to particle
ZeroVector(&gp->vgpnp1);
// only two possible transport tasks
double rate[2];
rate[0] = rate[1] = 0.;
int task;
TransportTask *nextTransport;
// Loop over nodes
for(int i=1;i<=numnds;i++)
{ // increment velocity and acceleraton
const NodalPoint *ndptr = nd[nds[i]];
short vfld = (short)mpmptr->vfld[i];
// increment
ndptr->IncrementDelvaTask5(vfld,matfld,fn[i],gp);
#ifdef CHECK_NAN
// conditionally compiled check for nan velocities
if(gp->vgpnp1.x!=gp->vgpnp1.x || gp->vgpnp1.y!=gp->vgpnp1.y || gp->vgpnp1.z!=gp->vgpnp1.z)
{
#pragma omp critical (output)
{ cout << "\n# UpdateParticlesTask::Execute: bad material velocity field for vfld=" << vfld << "matfld=" << matfld << " fn[i]=" << fn[i] << endl;;
PrintVector("# Particle velocity vgpn1 = ",&gp->vgpnp1);
cout << endl;
ndptr->Describe();
}
}
#endif
// increment transport rates
nextTransport=transportTasks;
task=0;
while(nextTransport!=NULL)
nextTransport=nextTransport->IncrementTransportRate(ndptr,fn[i],rate[task++]);
}
// Find grid damping acceleration parts = ag*Vgp(n) = ag*(Vgp(n+1) - Agp(n)*dt)
Vector accExtra = gp->vgpnp1;
AddScaledVector(&accExtra, gp->acc, -timestep);
ScaleVector(&accExtra,localGridAlpha);
// update position, and must be before velocity update because updates need initial velocity
// This section does second order update
mpmptr->MovePosition(timestep,&gp->vgpnp1,&accExtra,localParticleAlpha);
// update velocity in mm/sec
mpmptr->MoveVelocity(timestep,&accExtra);
// update transport values
nextTransport=transportTasks;
task=0;
while(nextTransport!=NULL)
nextTransport=nextTransport->MoveTransportValue(mpmptr,timestep,rate[task++]);
// energy coupling here adds adiabtic temperature rise
if(ConductionTask::adiabatic)
{ double dTad = mpmptr->GetBufferClear_dTad(); // in K
mpmptr->pTemperature += dTad; // in K
}
// delete grid to particle extrap data
delete gp;
}
catch(CommonException& err)
{ if(upErr==NULL)
{
#pragma omp critical (error)
upErr = new CommonException(err);
}
}
catch(std::bad_alloc&)
{ if(upErr==NULL)
{
#pragma omp critical (error)
upErr = new CommonException("Memory error","UpdateParticlesTask::Execute");
}
}
catch(...)
{ if(upErr==NULL)
{
#pragma omp critical (error)
upErr = new CommonException("Unexpected error","UpdateParticlesTask::Execute");
}
}
}
// throw any errors
if(upErr!=NULL) throw *upErr;
// rigid materials move at their current velocity
for(int p=nmpmsNR;p<nmpms;p++)
{ mpm[p]->MovePosition(timestep);
}
}
// Adjust change in momentum for frictional contact in tangential direction
// If has component of tangential motion, calculate force depending on whether it is sticking or sliding
// When frictional sliding, find tangential force (times dt) and set flag, if not set flag false
// When friction heating is on, set Ftdt term and set hasFriction to true
// (hasFriction (meaning has frictional heating value) must be initialized to false when called)
// contactArea only provided if frictional law needs it
// Normally in contact when called, but some laws might want call even when not in contact. If return
// value is false, the contact should be treated as no contact
bool CoulombFriction::GetFrictionalDeltaMomentum(Vector *delPi,Vector *norm,double dotn,double *mredDE,double mred,
bool getHeating,double contactArea,bool inContact,double deltime,Vector *at) const
{
// indicate no frictional heat yet
*mredDE=-1.;
// stick and frictionless are easy and no heating
if(frictionStyle==STICK)
{ // stick conditions no change
return true;
}
else if(frictionStyle==FRICTIONLESS)
{ // remove tangential term
CopyScaleVector(delPi,norm,dotn);
return true;
}
// Rest implements friction sliding
// The initial delPi = (-N A dt) norm + (Sstick A dt) tang = dotn norm + dott tang
// where N is normal traction (positive in compression), A is contact area, and dt is timestep
// get unnormalized tangential vector and its magnitude
// tang = delPi - dotn norm
Vector tang;
CopyVector(&tang,delPi);
AddScaledVector(&tang,norm,-dotn);
double tangMag = sqrt(DotVectors(&tang,&tang));
// if has tangential motion, we need to change momemtum if frictional sliding is occuring
if(!DbleEqual(tangMag,0.))
{ ScaleVector(&tang,1./tangMag);
double dott = DotVectors(delPi,&tang);
// make it positive for comparison to the positive frictional force Sslide
if(dott < 0.)
{ ScaleVector(&tang,-1.);
dott = -dott;
}
// Let frictional sliding force be Sslide Ac dt = f(N) Ac dt
// Then if dott > Sslide Ac dt (which means Sstick>Sslide)
// a. Set delPi = dotn norm + Sslide Ac dt tang
// For example, Coulomb friction has Fslide dt = mu(dotn) so delPi = (norm - mu tang) dotn
double SslideAcDt = GetSslideAcDt(-dotn,dott,0.,mred,contactArea,inContact,deltime);
if(!inContact) return false;
if(dott > SslideAcDt)
{ CopyScaleVector(delPi,norm,dotn);
AddScaledVector(delPi,&tang,SslideAcDt);
// get frictional heating term as friction work times reduced mass
// As heat source need Energy/sec or divide by timestep*reduced mass
// Note: only add frictional heating during momentum update (when friction
// force is appropriate) and only if frictional contact heat is enabled.
if(getHeating)
{ if(at!=NULL)
{ double Vs = SslideAcDt*(dott-SslideAcDt);
AddScaledVector(at, delPi, -1./deltime);
double AsDt = SslideAcDt*DotVectors(at,&tang)*deltime;
if(AsDt>Vs)
{ //*mredDE = Vs*(Vs/AsDt-1.)+0.5*AsDt;
*mredDE = 0.5*Vs*Vs/AsDt;
}
else
*mredDE = Vs - 0.5*AsDt;
}
else
*mredDE = SslideAcDt*(dott-SslideAcDt);
}
}
}
// still in contact
return true;
}
bool DexMouseTracker::GetCurrentMarkerFrame( CodaFrame &frame ) {
POINT mouse_position;
GetCursorPos( &mouse_position );
RECT rect;
GetWindowRect( GetDesktopWindow(), &rect );
Vector3 position, rotated;
Vector3 x_dir, y_span, z_span;
Vector3 x_displacement, y_displacement, z_displacement;
double x, y, z;
Quaternion Ry, Rz, Q, nominalQ, midQ;
Matrix3x3 xform = {{-1.0, 0.0, 0.0},{0.0, 0.0, 1.0},{0.0, 1.0, 0.0}};
int mrk, id;
// Just set the target frame markers at their nominal fixed positions.
for ( mrk = 0; mrk < nFrameMarkers; mrk++ ) {
id = FrameMarkerID[mrk];
CopyVector( frame.marker[id].position, TargetFrameBody[mrk] );
}
// Shift the vertical bar markers to simulate being in the left position.
if ( IsDlgButtonChecked( dlg, IDC_LEFT ) ) {
frame.marker[DEX_NEGATIVE_BAR_MARKER].position[X] += 300.0;
frame.marker[DEX_POSITIVE_BAR_MARKER].position[X] += 300.0;
}
// Transform the marker positions of the target box and frame according
// to whether the system was upright or supine when it was aligned and
// according to whether the system is currently installed in the upright
// or supine configuration.
if ( ( IsDlgButtonChecked( dlg, IDC_SUPINE ) && TRACKER_ALIGNED_SUPINE != SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) ||
( IsDlgButtonChecked( dlg, IDC_SEATED ) && TRACKER_ALIGNED_UPRIGHT != SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) ) {
for ( mrk = 0; mrk < nFrameMarkers; mrk++ ) {
id = FrameMarkerID[mrk];
position[X] = - frame.marker[id].position[X];
position[Z] = frame.marker[id].position[Y];
position[Y] = frame.marker[id].position[Z];
CopyVector( frame.marker[id].position, position );
}
MatrixToQuaternion( nominalQ, xform );
}
else CopyQuaternion( nominalQ, nullQuaternion );
// Now set the visibility flag as a funciton of the GUI.
if ( IsDlgButtonChecked( dlg, IDC_BAR_OCCLUDED ) ) {
frame.marker[DEX_NEGATIVE_BAR_MARKER].visibility = false;
frame.marker[DEX_POSITIVE_BAR_MARKER].visibility = false;
}
else {
frame.marker[DEX_NEGATIVE_BAR_MARKER].visibility = true;
frame.marker[DEX_POSITIVE_BAR_MARKER].visibility = true;
}
if ( IsDlgButtonChecked( dlg, IDC_BOX_OCCLUDED ) ) {
frame.marker[DEX_NEGATIVE_BOX_MARKER].visibility = false;
frame.marker[DEX_POSITIVE_BOX_MARKER].visibility = false;
}
else {
frame.marker[DEX_NEGATIVE_BOX_MARKER].visibility = true;
frame.marker[DEX_POSITIVE_BOX_MARKER].visibility = true;
}
// Map mouse coordinates to world coordinates. The factors used here are empirical.
y = (double) ( mouse_position.y - rect.top ) / (double) ( rect.bottom - rect.top );
z = (double) (mouse_position.x - rect.right) / (double) ( rect.left - rect.right );
x = 0.0;
// By default, the orientation of the manipulandum is the nominal orientation.
CopyQuaternion( Q, nominalQ );
// Simulate wobbly movements of the manipulandum. This has not really been tested.
if ( IsDlgButtonChecked( dlg, IDC_CODA_WOBBLY ) ) {
// We will make the manipulandum rotate in a strange way as a function of the distance from 0.
// This is just so that we can test the routines that compute the manipulandum position and orientation.
double theta = y * 45.0;
double gamma = - z * 45.0;
SetQuaterniond( Ry, theta, iVector );
SetQuaterniond( Rz, gamma, jVector );
MultiplyQuaternions( midQ, Rz, nominalQ );
MultiplyQuaternions( Q, Ry, midQ );
// Make the movement a little bit in X as well so that we test the routines in 3D.
x = 0.0 + 5.0 * sin( y / 80.0);
}
// Map screen position of the mouse pointer to 3D position of the wrist and manipulandum.
// Top of the screen corresponds to the bottom of the bar and vice versa. It's inverted to protect the right hand rule.
// Right of the screen correponds to the nearest horizontal target and left corresponds to the farthest.
// The X position is set to be just to the right of the box.
SubtractVectors( y_span, frame.marker[DEX_POSITIVE_BAR_MARKER].position, frame.marker[DEX_NEGATIVE_BAR_MARKER].position );
SubtractVectors( x_dir, frame.marker[DEX_POSITIVE_BOX_MARKER].position, frame.marker[DEX_NEGATIVE_BOX_MARKER].position );
NormalizeVector( x_dir );
ComputeCrossProduct( z_span, x_dir, y_span );
ScaleVector( y_displacement, y_span, y );
ScaleVector( z_displacement, z_span, z );
ScaleVector( x_displacement, x_dir, x );
// Reference position is the bottom target on the vertical target bar.
CopyVector( position, frame.marker[DEX_NEGATIVE_BAR_MARKER].position );
// Place the manipulandum to the right of the box, even if the target bar is in the left position.
position[X] = frame.marker[DEX_NEGATIVE_BOX_MARKER].position[X];
// Shift the position in X if the is any wobble to it.
AddVectors( position, position, x_displacement );
// Shift the position in Y and Z according to the displacements computed from the mouse position.
AddVectors( position, position, y_displacement );
AddVectors( position, position, z_displacement );
frame.time = DexTimerElapsedTime( acquisitionTimer );
// Displace the manipulandum with the mouse and make it rotate.
for ( mrk = 0; mrk < nManipulandumMarkers; mrk++ ) {
id = ManipulandumMarkerID[mrk];
RotateVector( rotated, Q, ManipulandumBody[mrk] );
AddVectors( frame.marker[id].position, position, rotated );
frame.marker[id].visibility = true;
}
// Displace the wrist with the mouse, but don't rotate it.
for ( mrk = 0; mrk < nWristMarkers; mrk++ ) {
id = WristMarkerID[mrk];
AddVectors( frame.marker[id].position, position, WristBody[mrk] );
frame.marker[id].visibility = true;
}
// Output the position and orientation used to compute the simulated
// marker positions. This is for testing only.
fprintf( fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
frame.time,
position[X], position[Y], position[Z],
Q[X], Q[Y], Q[Z], Q[M] );
return( true );
}
int DexMouseTracker::RetrieveMarkerFrames( CodaFrame frames[], int max_frames, int unit ) {
int previous;
int next;
int frm;
Vector3f jump, delta;
double time, interval, offset, relative;
// Copy data into an array.
previous = 0;
next = 1;
// Fill an array of frames at a constant frequency by interpolating the
// frames that were taken at a variable frequency in real time.
for ( frm = 0; frm < max_frames; frm++ ) {
// Compute the time of each slice at a constant sampling frequency.
time = (double) frm * samplePeriod;
frames[frm].time = (float) time;
// Fill the first frames with the same position as the first polled frame;
if ( time < polledMarkerFrames[previous].time ) {
CopyMarkerFrame( frames[frm], polledMarkerFrames[previous] );
}
else {
// See if we have caught up with the real-time data.
if ( time > polledMarkerFrames[next].time ) {
previous = next;
// Find the next real-time frame that has a time stamp later than this one.
while ( polledMarkerFrames[next].time <= time && next < nPolled ) next++;
// If we reached the end of the real-time samples, then we are done.
if ( next >= nPolled ) {
break;
}
}
// Compute the time difference between the two adjacent real-time frames.
interval = polledMarkerFrames[next].time - polledMarkerFrames[previous].time;
// Compute the time between the current frame and the previous real_time frame.
offset = time - polledMarkerFrames[previous].time;
// Use the relative time to interpolate.
relative = offset / interval;
for ( int j = 0; j < nMarkers; j++ ) {
// Both the previous and next polled frame must be visible for the
// interpolated sample to be visible.
frames[frm].marker[j].visibility =
( polledMarkerFrames[previous].marker[j].visibility
&& polledMarkerFrames[next].marker[j].visibility );
if ( frames[frm].marker[j].visibility ) {
SubtractVectors( jump,
polledMarkerFrames[next].marker[j].position,
polledMarkerFrames[previous].marker[j].position );
ScaleVector( delta, jump, (float) relative );
AddVectors( frames[frm].marker[j].position, polledMarkerFrames[previous].marker[j].position, delta );
}
}
}
}
nAcqFrames = frm;
return( nAcqFrames );
}
void PCG_ParaSails(Matrix *mat, ParaSails *ps, double *b, double *x,
double tol, HYPRE_Int max_iter)
{
double *p, *s, *r;
double alpha, beta;
double gamma, gamma_old;
double bi_prod, i_prod, eps;
HYPRE_Int i = 0;
HYPRE_Int mype;
/* local problem size */
HYPRE_Int n = mat->end_row - mat->beg_row + 1;
MPI_Comm comm = mat->comm;
hypre_MPI_Comm_rank(comm, &mype);
/* compute square of absolute stopping threshold */
/* bi_prod = <b,b> */
bi_prod = InnerProd(n, b, b, comm);
eps = (tol*tol)*bi_prod;
/* Check to see if the rhs vector b is zero */
if (bi_prod == 0.0)
{
/* Set x equal to zero and return */
CopyVector(n, b, x);
return;
}
p = (double *) malloc(n * sizeof(double));
s = (double *) malloc(n * sizeof(double));
r = (double *) malloc(n * sizeof(double));
/* r = b - Ax */
MatrixMatvec(mat, x, r); /* r = Ax */
ScaleVector(n, -1.0, r); /* r = -r */
Axpy(n, 1.0, b, r); /* r = r + b */
/* p = C*r */
if (ps != NULL)
ParaSailsApply(ps, r, p);
else
CopyVector(n, r, p);
/* gamma = <r,p> */
gamma = InnerProd(n, r, p, comm);
while ((i+1) <= max_iter)
{
i++;
/* s = A*p */
MatrixMatvec(mat, p, s);
/* alpha = gamma / <s,p> */
alpha = gamma / InnerProd(n, s, p, comm);
gamma_old = gamma;
/* x = x + alpha*p */
Axpy(n, alpha, p, x);
/* r = r - alpha*s */
Axpy(n, -alpha, s, r);
/* s = C*r */
if (ps != NULL)
ParaSailsApply(ps, r, s);
else
CopyVector(n, r, s);
/* gamma = <r,s> */
gamma = InnerProd(n, r, s, comm);
/* set i_prod for convergence test */
i_prod = InnerProd(n, r, r, comm);
#ifdef PARASAILS_CG_PRINT
if (mype == 0 && i % 100 == 0)
hypre_printf("Iter (%d): rel. resid. norm: %e\n", i, sqrt(i_prod/bi_prod));
#endif
/* check for convergence */
if (i_prod < eps)
break;
/* non-convergence test */
if (i >= 1000 && i_prod/bi_prod > 0.01)
{
if (mype == 0)
hypre_printf("Aborting solve due to slow or no convergence.\n");
break;
}
/* beta = gamma / gamma_old */
beta = gamma / gamma_old;
/* p = s + beta p */
ScaleVector(n, beta, p);
Axpy(n, 1.0, s, p);
}
free(p);
free(s);
/* compute exact relative residual norm */
MatrixMatvec(mat, x, r); /* r = Ax */
ScaleVector(n, -1.0, r); /* r = -r */
Axpy(n, 1.0, b, r); /* r = r + b */
i_prod = InnerProd(n, r, r, comm);
free(r);
if (mype == 0)
hypre_printf("Iter (%4d): computed rrn : %e\n", i, sqrt(i_prod/bi_prod));
}
static void TraceLine( VECTOR *LinP, VECTOR *LinD, VECTOR *Color, int reccount )
{
VECTOR Pnt, Norm, LDir, NewDir, NewDir2, TmpCol, TmpCol2;
VECTOR TmpPnt, TmpNorm, D;
double t, A, cosfi;
TEXTURE *txt, *tmptxt;
int i, shadowcount, usedist;
Color->x = Color->y = Color->z = 0.0;
if( reccount > 0 ) {
/* Only use distributed tracing in higher nodes of the recursion tree */
usedist = ( (MAXREC-reccount) < DISTLEVELS ) ? 1 : 0;
/* Try intersection with objects */
t = IntersectObjs( LinP, LinD, &Pnt, &Norm, &txt );
/* Get light-intensity in intersection-point (store in cosfi) */
if( t > EPSILON ) {
LDir.x = Lightpos.x-Pnt.x; /* Get line to light from surface */
LDir.y = Lightpos.y-Pnt.y;
LDir.z = Lightpos.z-Pnt.z;
cosfi = LDir.x*Norm.x + LDir.y*Norm.y + LDir.z*Norm.z;
if(cosfi > 0.0) { /* If angle between lightline and normal < PI/2 */
shadowcount = 0;
if( usedist ) {
A = Lightr / VectorLength( &LDir );
for( i = 0; i < DISTRIB; i++ ) {
DistribVector( &D, &LDir, A, A );
NewDir = LDir;
NewDir.x += D.x; NewDir.y += D.y; NewDir.z += D.z;
/* Check for shadows (ignore hit info, may be used though) */
t = IntersectObjs( &Pnt, &NewDir, &TmpPnt, &TmpNorm, &tmptxt );
if( ( t < EPSILON ) || ( t > 1.0 ) ) shadowcount++;
}
} else {
t = IntersectObjs( &Pnt, &LDir, &TmpPnt, &TmpNorm, &tmptxt );
if( ( t < EPSILON ) || ( t > 1.0 ) ) shadowcount = DISTRIB;
}
if( shadowcount > 0 ) {
A = Norm.x*Norm.x + Norm.y*Norm.y + Norm.z*Norm.z;
A *= LDir.x*LDir.x + LDir.y*LDir.y + LDir.z*LDir.z;
cosfi = (cosfi/sqrt(A))*txt->diffuse*(double)shadowcount/DISTRIB;
} else {
cosfi = 0.0;
}
} else {
cosfi = 0.0;
}
Color->x = txt->color.x*(Ambient+cosfi);
Color->y = txt->color.y*(Ambient+cosfi);
Color->z = txt->color.z*(Ambient+cosfi);
if( txt->reflect > EPSILON ) {
ReflectVector( &NewDir, LinD, &Norm );
TmpCol.x = TmpCol.y = TmpCol.z = 0.0;
if( usedist && ( txt->roughness > EPSILON ) ) {
for( i = 0; i < DISTRIB; i++ ) {
DistribVector( &D, &NewDir, txt->roughness, txt->roughness );
NewDir2 = NewDir;
NewDir2.x += D.x; NewDir2.y += D.y; NewDir2.z += D.z;
TraceLine( &Pnt, &NewDir2, &TmpCol2, reccount-1 );
TmpCol.x += TmpCol2.x;
TmpCol.y += TmpCol2.y;
TmpCol.z += TmpCol2.z;
}
ScaleVector( &TmpCol, 1.0/DISTRIB );
} else {
TraceLine( &Pnt, &NewDir, &TmpCol, reccount-1 );
}
Color->x += TmpCol.x * txt->reflect;
Color->y += TmpCol.y * txt->reflect;
Color->z += TmpCol.z * txt->reflect;
}
} else {
/* Get sky-color (interpolate between horizon and zenit) */
A = sqrt( LinD->x*LinD->x + LinD->y*LinD->y );
if( A > 0.0 ) A = atan( fabs( LinD->z ) / A )*0.63661977;
else A = 1.0;
Color->x = Skycolor[1].x*A + Skycolor[0].x*(1.0-A);
Color->y = Skycolor[1].y*A + Skycolor[0].y*(1.0-A);
Color->z = Skycolor[1].z*A + Skycolor[0].z*(1.0-A);
}
/* Make sure that the color does not exceed the maximum level */
if(Color->x > 1.0) Color->x = 1.0;
if(Color->y > 1.0) Color->y = 1.0;
if(Color->z > 1.0) Color->z = 1.0;
}
}
void CPhysEnv::ComputeForces( tParticle *system )
{
int loop;
tParticle *curParticle,*p1, *p2;
tSpring *spring;
float dist, Hterm, Dterm;
tVector springForce,deltaV,deltaP;
curParticle = system;
for (loop = 0; loop < m_ParticleCnt; loop++)
{
MAKEVECTOR(curParticle->f,0.0f,0.0f,0.0f) // CLEAR FORCE VECTOR
if (m_UseGravity && curParticle->oneOverM != 0)
{
curParticle->f.x += (m_Gravity.x / curParticle->oneOverM);
curParticle->f.y += (m_Gravity.y / curParticle->oneOverM);
curParticle->f.z += (m_Gravity.z / curParticle->oneOverM);
}
if (m_UseDamping)
{
curParticle->f.x += (-m_Kd * curParticle->v.x);
curParticle->f.y += (-m_Kd * curParticle->v.y);
curParticle->f.z += (-m_Kd * curParticle->v.z);
}
else
{
curParticle->f.x += (-DEFAULT_DAMPING * curParticle->v.x);
curParticle->f.y += (-DEFAULT_DAMPING * curParticle->v.y);
curParticle->f.z += (-DEFAULT_DAMPING * curParticle->v.z);
}
curParticle++;
}
// CHECK IF THERE IS A USER FORCE BEING APPLIED
if (m_UserForceActive)
{
if (m_Pick[0] != -1)
{
VectorSum(&system[m_Pick[0]].f,&m_UserForce,&system[m_Pick[0]].f);
}
if (m_Pick[1] != -1)
{
VectorSum(&system[m_Pick[1]].f,&m_UserForce,&system[m_Pick[1]].f);
}
MAKEVECTOR(m_UserForce,0.0f,0.0f,0.0f); // CLEAR USER FORCE
}
// NOW DO ALL THE SPRINGS
spring = m_Spring;
for (loop = 0; loop < m_SpringCnt; loop++)
{
p1 = &system[spring->p1];
p2 = &system[spring->p2];
VectorDifference(&p1->pos,&p2->pos,&deltaP); // Vector distance
dist = VectorLength(&deltaP); // Magnitude of deltaP
Hterm = (dist - spring->restLen) * spring->Ks; // Ks * (dist - rest)
VectorDifference(&p1->v,&p2->v,&deltaV); // Delta Velocity Vector
Dterm = (DotProduct(&deltaV,&deltaP) * spring->Kd) / dist; // Damping Term
ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
ScaleVector(&springForce,-(Hterm + Dterm),&springForce); // Calc Force
VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
VectorDifference(&p2->f,&springForce,&p2->f); // - Force on Particle 2
spring++; // DO THE NEXT SPRING
}
// APPLY THE MOUSE DRAG FORCES IF THEY ARE ACTIVE
if (m_MouseForceActive)
{
// APPLY TO EACH PICKED PARTICLE
if (m_Pick[0] > -1)
{
p1 = &system[m_Pick[0]];
VectorDifference(&p1->pos,&m_MouseDragPos[0],&deltaP); // Vector distance
dist = VectorLength(&deltaP); // Magnitude of deltaP
if (dist != 0.0f)
{
Hterm = (dist) * m_MouseForceKs; // Ks * dist
ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
ScaleVector(&springForce,-(Hterm),&springForce); // Calc Force
VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
}
}
if (m_Pick[1] > -1)
{
p1 = &system[m_Pick[1]];
VectorDifference(&p1->pos,&m_MouseDragPos[1],&deltaP); // Vector distance
dist = VectorLength(&deltaP); // Magnitude of deltaP
if (dist != 0.0f)
{
Hterm = (dist) * m_MouseForceKs; // Ks * dist
ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
ScaleVector(&springForce,-(Hterm),&springForce); // Calc Force
VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
}
}
}
}
/* Compute form-factors from the shooting patch to every elements */
static void ComputeFormfactors(unsigned long shootPatch)
{
unsigned long i;
TVector3f up[5];
TPoint3f lookat[5];
TPoint3f center;
TVector3f normal, tangentU, tangentV, vec;
int face;
double norm;
TPatch* sp;
double* fp;
TElement* ep;
/* get the center of shootPatch */
sp = &(params->patches[shootPatch]);
center = sp->center;
normal = sp->normal;
/* rotate the hemi-cube along the normal axis of the patch randomly */
/* this will reduce the hemi-cube aliasing artifacts */
do {
vec.x = RandomFloat;
vec.y = RandomFloat;
vec.z = RandomFloat;
/* get a tangent vector */
CrossVector(tangentU, normal, vec);
NormalizeVector(norm, tangentU);
} while (norm==0); /* bad choice of the random vector */
/* compute tangentV */
CrossVector(tangentV, normal, tangentU);
/* assign the lookats and ups for each hemicube face */
AddVector(lookat[0], center, normal);
up[0] = tangentU;
AddVector(lookat[1], center, tangentU);
up[1] = normal;
AddVector(lookat[2], center, tangentV);
up[2] = normal;
SubVector(lookat[3], center, tangentU);
up[3] = normal;
SubVector(lookat[4], center, tangentV);
up[4] = normal;
/* position the hemicube slightly above the center of the shooting patch */
ScaleVector(normal, params->worldSize*0.0001);
AddVector(hemicube.view.camera, center, normal);
/* clear the formfactors */
fp = formfactors;
for (i=params->nElements; i--; fp++)
*fp = 0.0;
for (face=0; face < 5; face++)
{
hemicube.view.lookat = lookat[face];
hemicube.view.up = up[face];
/* draw elements */
BeginDraw(&(hemicube.view), kBackgroundItem);
for (i=0; i< params->nElements; i++)
DrawElement(¶ms->elements[i], i);
/* color element i with its index */
EndDraw();
/* get formfactors */
if (face==0)
SumFactors(formfactors, hemicube.view.xRes, hemicube.view.yRes,
hemicube.view.buffer, hemicube.topFactors);
else
SumFactors(formfactors, hemicube.view.xRes, hemicube.view.yRes/2,
hemicube.view.buffer, hemicube.sideFactors);
}
/* compute reciprocal form-factors */
ep = params->elements;
fp = formfactors;
for (i=params->nElements; i--; ep++, fp++)
{
*fp *= sp->area / ep->area;
/* This is a potential source of hemi-cube aliasing */
/* To do this right, we need to subdivide the shooting patch
and reshoot. For now we just clip it to unity */
if ((*fp) > 1.0) *fp = 1.0;
}
}
void CEnvironment::DrawFog () {
if (!fog.is_on)
return;
TPlane bottom_plane, top_plane;
TVector3 left, right, vpoint;
TVector3 topleft, topright;
TVector3 bottomleft, bottomright;
// the clipping planes are calculated by view frustum (view.cpp)
const TPlane& leftclip = get_left_clip_plane ();
const TPlane& rightclip = get_right_clip_plane ();
const TPlane& farclip = get_far_clip_plane ();
const TPlane& bottomclip = get_bottom_clip_plane ();
// --------------- calculate the planes ---------------------------
float slope = tan (ANGLES_TO_RADIANS (Course.GetCourseAngle()));
// TPlane left_edge_plane = MakePlane (1.0, 0.0, 0.0, 0.0);
// TPlane right_edge_plane = MakePlane (-1.0, 0.0, 0.0, Course.width);
bottom_plane.nml = TVector3(0.0, 1, -slope);
float height = Course.GetBaseHeight (0);
bottom_plane.d = -height * bottom_plane.nml.y;
top_plane.nml = bottom_plane.nml;
height = Course.GetMaxHeight (0) + fog.height;
top_plane.d = -height * top_plane.nml.y;
if (!IntersectPlanes (bottom_plane, farclip, leftclip, &left)) return;
if (!IntersectPlanes (bottom_plane, farclip, rightclip, &right)) return;
if (!IntersectPlanes (top_plane, farclip, leftclip, &topleft)) return;
if (!IntersectPlanes (top_plane, farclip, rightclip, &topright)) return;
if (!IntersectPlanes (bottomclip, farclip, leftclip, &bottomleft)) return;
if (!IntersectPlanes (bottomclip, farclip, rightclip, &bottomright)) return;
TVector3 leftvec = SubtractVectors (topleft, left);
TVector3 rightvec = SubtractVectors (topright, right);
// --------------- draw the fog plane -----------------------------
ScopedRenderMode rm(FOG_PLANE);
glEnable (GL_FOG);
// only the alpha channel is used
float bottom_dens[4] = {0, 0, 0, 1.0};
float top_dens[4] = {0, 0, 0, 0.9};
float leftright_dens[4] = {0, 0, 0, 0.3};
float top_bottom_dens[4] = {0, 0, 0, 0.0};
glBegin (GL_QUAD_STRIP);
glColor4fv (bottom_dens);
glVertex3f (bottomleft.x, bottomleft.y, bottomleft.z);
glVertex3f (bottomright.x, bottomright.y, bottomright.z);
glVertex3f (left.x, left.y, left.z);
glVertex3f (right.x, right.y, right.z);
glColor4fv (top_dens);
glVertex3f (topleft.x, topleft.y, topleft.z);
glVertex3f (topright.x, topright.y, topright.z);
glColor4fv (leftright_dens);
vpoint = AddVectors (topleft, leftvec);
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
vpoint = AddVectors (topright, rightvec);
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
glColor4fv (top_bottom_dens);
vpoint = AddVectors (topleft, ScaleVector (3.0, leftvec));
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
vpoint = AddVectors (topright, ScaleVector (3.0, rightvec));
glVertex3f (vpoint.x, vpoint.y, vpoint.z);
glEnd();
}