Ejemplo n.º 1
0
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 ) );
}
Ejemplo n.º 2
0
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] ) );
}
Ejemplo n.º 3
0
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;
} 
Ejemplo n.º 4
0
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 ) );
}
Ejemplo n.º 5
0
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 ) );
}
Ejemplo n.º 6
0
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); 
}
Ejemplo n.º 7
0
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();

}
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
0
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;
}
Ejemplo n.º 10
0
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;
}
Ejemplo n.º 11
0
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;
}
Ejemplo n.º 12
0
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]);
	}
}
Ejemplo n.º 14
0
TVector3 ProjectToPlane (TVector3 nml, TVector3 v){
    TVector3 nmlComp;
    double dotProd;

    dotProd = DotProduct (nml, v);
    nmlComp = ScaleVector (dotProd, nml);

    return SubtractVectors (v, nmlComp);
} 
Ejemplo n.º 15
0
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);
	}
}
Ejemplo n.º 17
0
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;
}
Ejemplo n.º 18
0
/////////////////////////////////////////////////////////////////////////////
// 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;
				}
			}
}
Ejemplo n.º 19
0
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);
    }
  }
}
Ejemplo n.º 20
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;
}
Ejemplo n.º 22
0
// 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);
    }
}
Ejemplo n.º 23
0
// 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;
}
Ejemplo n.º 24
0
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 );

}
Ejemplo n.º 25
0
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 );

}
Ejemplo n.º 26
0
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));
}
Ejemplo n.º 27
0
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
			}
		}
	}
}   
Ejemplo n.º 29
0
/* 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(&params->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;	
	}

}
Ejemplo n.º 30
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();
}