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(); }