particle3d.java

运用java3D模拟刚体间的碰撞,爆炸及在万有引力作用下的运动轨迹,设置适当的参数可以模拟天体运动等多种物理现象.

		Vector3d angRel2 = (Vector3d)p2.angularVelocity.clone();
		angRel2.cross(angRel2, relPosition2);
		
		relativeVelocity.add(angRel);
		
		relativeVelocity.sub(p2vector);
		
		relativeVelocity.sub(angRel2);
		
		
		////////////////////////
		
		//Calculate normal of collision (needs to be updated if new shapes are introduced)
		Vector3d normal = (Vector3d)p1.position.clone();
		normal.sub(p2.position);
		
		normal.normalize();
		///////////////////////////////////////////////////////////////////
		
		//Calculate elasticity of the collision (is this correct?)
		double e = (p1.elasticity + p2.elasticity) / 2;
		//////////////////////////////////////////////////////
		
		//Caculate the impulse (Thank you to Chris Hecker for his great article about
		//Physics in Games avaliable at: http://www.d6.com/users/checker
		Vector3d temp = (Vector3d)relativeVelocity.clone();
		temp.scale((-(1 + e)));
		
		Matrix3d p1moiInverse = (Matrix3d)p1.momentOfInertia.clone();
		p1moiInverse.invert();
		
		Matrix3d p2moiInverse = (Matrix3d)p2.momentOfInertia.clone();
		p2moiInverse.invert();
		
		double tempJ = ((1 / p1.mass) + (1 / p2.mass));
		
		Vector3d temp2 = (Vector3d)relPosition1.clone();
		temp2.cross(temp2, normal);
		temp2 = multMatrixVector(p1moiInverse, temp2);
		temp2.cross(temp2, relPosition1);
		tempJ += temp2.dot(normal);
		
		temp2 = (Vector3d)relPosition2.clone();
		temp2.cross(temp2, normal);
		temp2 = multMatrixVector(p2moiInverse, temp2);
		temp2.cross(temp2, relPosition2);
		tempJ += temp2.dot(normal);
		
		double j = temp.dot(normal) / tempJ;
		
		//Reverse the elasticity if rewinding
		if(time == -1)
			j /= -e;
		///////////////////////////////////////////////
		
		System.out.println("Collision between " + p1.name + " and " + p2.name + " impulse = " + j);
		
		
		//Calculate the resultant velocities
		Vector3d temp3 = (Vector3d)normal.clone();
		temp3.scale((j / p1.mass));
		
		if(time == 1)
			p1vector.add(temp3);
		else
			p1vector.sub(temp3);
		
		
		temp3 = (Vector3d)normal.clone();
		temp3.scale(j / p2.mass);

		if(time == 1)
			p2vector.sub(temp3);
		else
			p2vector.add(temp3);
			
		
		temp3 = (Vector3d)normal.clone();
		temp3.scale(j);
		temp3.cross(temp3, relPosition1);
		p1.momentOfInertia.transform(temp3);
		
		if(time == 1)
			p1avector.add(temp3);
		else
			p1avector.sub(temp3);
		
		temp3 = (Vector3d)normal.clone();
		temp3.scale(j);
		temp3.cross(temp3, relPosition2);
		p2.momentOfInertia.transform(temp3);
		
		if(time == 1)
			p2avector.add(temp3);
		else
			p2avector.sub(temp3);
		
		p1.velocityVector = p1vector;
		p1.angularVelocity = p1avector;
		
		p2.velocityVector = p2vector;
		p2.angularVelocity = p2avector;
		//////////////////////////////
	
	
	}	
	
	
	//////////////////////////////
	
	//Are the two particles colliding? (used by updateAll function);
	public static boolean areColliding(Particle3D p1, Particle3D p2, int time, int i1, int i2)
	{
		
		//Quick special case check
		if(p1.equals(p2))
		{
			return false;
		}
		
	//	if(p1.closestFeatures[i2].distanceTo(p2.position) < p2.closestFeatures[i1].distanceTo(p2.position))
	//	{
	//		System.out.println("CollisionHandling 2 Detected Collision: " + p1 + " : " + p2);
	//	}
		
		
		//Get the current positions of the particles	
	//	Vector3d	positionp1 =  p1.position,
	//				positionp2 = p2.position;
							
	//	Vector3d vdistance = (Vector3d)positionp2.clone();
	//	vdistance.sub(positionp1);
			
	//	double fdistance = vdistance.length();
		
		//If they are close enough check the relative velocities
	//	if(Math.abs(fdistance) <= Math.abs(p1.radius + p2.radius))
		if(p1.closestFeatures[i2].distanceTo(p2.position) <= p2.closestFeatures[i1].distanceTo(p2.position))
		{
	
			Vector3d	velocity1 = (Vector3d)p1.velocityVector.clone(),
						velocity2 = (Vector3d)p2.velocityVector.clone();
							
			Vector3d relativeVelocity = velocity1;
			relativeVelocity.sub(velocity2);
		
		
			Vector3d normal = (Vector3d)p1.position.clone();
			normal.sub(p2.position);
			
			//Vector3d normal = Feature.calculateNormal(p1.closestFeatures[i2], p2.closestFeatures[i1]);
			
			
			//Return if they are colliding
			return  (time == 1 ? relativeVelocity.dot(normal) < 0 : relativeVelocity.dot(normal) > 0);
			
		}
		return false;
	
	}
	
		
	public static boolean collideWithBounds(Particle3D p)
	{
		//If there are no bounds skip out
		if(ufrBounding == null || lblBounding == null)
			return false;
		
		//Get the normal Vector of collision
		Vector3d normal = new Vector3d(0.0,0.0,0.0);
		
		normal.x = (ufrBounding.x - p.position.x < p.radius ? 1.0 : 
								(p.position.x - lblBounding.x < p.radius ? -1.0 : 0));
		normal.y = (ufrBounding.y - p.position.y < p.radius ? 1.0 : 
								(p.position.y - lblBounding.y < p.radius ? -1.0 : 0));
		normal.z = (ufrBounding.z - p.position.z < p.radius ? 1.0 : 
								(p.position.z - lblBounding.z < p.radius ? -1.0 : 0));						

		//If there isn't a collision skip out
		if(normal.length() == 0)
			return false;
		
		//Get the relative velocity
		Vector3d pvector = (Vector3d)p.velocityVector.clone();
		Vector3d relativeVelocity = (Vector3d)pvector.clone();		
		
		//Get elasticiyty
		double e = (p.elasticity + boundsElasticity) / 2;
		
		//Get impulse
		Vector3d temp = (Vector3d)relativeVelocity.clone();
		temp.scale((-1 * (1 + e)));
		
		Vector3d temp2 = (Vector3d)normal.clone();
		temp2.scale((1 / p.mass));
		
		double j = temp.dot(normal) / normal.dot(temp2);
		//////////////////////
		
		
		System.out.println("Collision between " + p.name + " and bounding wall: impulse = " + j);
		
		//Calculate resultant velocity
		Vector3d temp3 = (Vector3d)normal.clone();
		temp3.scale((j / p.mass));
		
		pvector.add(temp3);
	
		p.velocityVector = pvector;
		
		return true;
			
	}
	
	//Handles the collision of 2 particles
	public static boolean handleCollision(Particle3D p1, Particle3D p2, int time, int i1, int i2)
	{
		
		//if they aren't colliding don't do anything
		if(!areColliding(p1, p2, time, i1, i2))
			return false;
		
		System.out.println("Collision: p1: " + p1.name + " p2: " + p2.name);
		
		//Handle exploding...
		if(collisionHandling == EXPLODE)
		{
			p1.explode();
			p2.explode();
			
			particles.remove(p1);
			particles.remove(p2);
			
			for(int i = 0; i < particles.size(); i++)
			{
				Particle3D current = (Particle3D)particles.get(i);
				
				Vector3d distance = new Vector3d(0.0,0.0,0.0);
				distance.add(current.position);
				distance.sub(p1.position);
				
				if(Math.abs(distance.length()) < p1.radius * explodeBlastRadiusMultiplier)
				{
					current.explode();
					particles.remove(current);
					i = 0;
				}
			}
		
		}
		//Handle momentum
		else if(collisionHandling == MOMENTUM)
			calculateResultantVelocities(p1, p2, time, i1, i2);

		return true;
	}
	
	//Removes a particle from the system
	public static void removeParticle(Particle3D p)
	{
		Particle3DTracker.removeReferring(p);
		particles.remove(p);
		bgRoot.removeChild(p.bg);
	
	}
	
	public static void setup()
	{
		int length = particles.size();
		for(int i = 0; i < length; i++)
		{
			((Particle3D)particles.get(i)).closestFeatures = new Feature[length];
		
		}
		
		//Get Closest Features
		for(int i = 0; i < length; i++)
		{
			for(int j = i+1; j < length; j++)
			{
				getClosest(i, j);
			}
		}
		/////
	
	}
	
	//Updates all particles in the system
	public static void updateAll(float timeDifferential)
	{
		//Get the time
		time += (long)(timeDifferential * 1000);
		int normalAdjust = 1;

		if(timeDifferential < 0)
			normalAdjust *= -1;
		//////////////////////
		
		//Update all the particles positions and collide with bounds (O(n))
		int length = particles.size();
				
		for(int i = 0; i < length; i++)
		{
			((Particle3D)particles.get(i)).update(timeDifferential);
			collideWithBounds(((Particle3D)particles.get(i)));
		}
		
		
		//Get Closest Features
		for(int i = 0; i < length; i++)
		{
			for(int j = i+1; j < length; j++)
			{
				getClosest(i, j);
			}
		}
		/////
		
		//Collide particles with each other (O(n^2) :()
		for(int i = 0; i < length; i++)
		{
			
			Particle3D current = (Particle3D)particles.get(i);
			for(int j = 0; j < length; j++)
			{					
				if(handleCollision(current, (Particle3D)particles.get(j), normalAdjust, i, j))
				{
					if(collisionHandling == EXPLODE)
					{
						length = particles.size();
						i--;
						break;				
					}
				
				}
			}
		}
		
		//Set updated back to false
		for(int i = 0; i < length; i++)
		{
			((Particle3D)particles.get(i)).updated = false;
		}
		
		//If we are traking update the current frame
		if(traceNumFrames != -1)
		{
			currentFrame = ((currentFrame >= traceNumFrames) ? 0 : currentFrame + 1);
		}
		
		//Update all the trackers
		Particle3DTracker.updateAll();
				
	}
	
	public static void getClosest(int i1, int i2)
	{
		Particle3D 	p1 = (Particle3D)particles.get(i1),
					p2 = (Particle3D)particles.get(i2);
		
		if(p1.closestFeatures[i2] == null)
		{
			p1.closestFeatures[i2] = p1.new Feature(((Point3DAdjacent)p1.p3dadj.get(0)).point, 0);
		}
		if(p2.closestFeatures[i1] == null)
		{
			p2.closestFeatures[i1] = p2.new Feature(((Point3DAdjacent)p2.p3dadj.get(0)).point, 0);
		}
		
		while(p1.getClosestFeatureTo(p2.closestFeatures[i1], i2) ||
				p2.getClosestFeatureTo(p1.closestFeatures[i2], i1));

	}
	
	public boolean getClosestFeatureTo(Feature f, int i)
	{
		Feature current = closestFeatures[i];
		
		double distance = current.distanceTo(f);
		
		Feature[] adj = null;
		
		boolean changed = true;
		boolean retVal = false;
		while(changed)
		{
			changed = false;
			adj = current.getAdjacent();
			
			for(int j = 0; j < adj.length; j++)
			{	
				double d = adj[j].distanceTo(f);
				if(d < distance)
				{
					distance = d;
					current = adj[j];
					changed = true;
					retVal = true;
				}
			}
		}
		
		closestFeatures[i] = current;
		return retVal;
	}
	
	public static void resetAll()
	{
		time = 0;
		int length = particles.size();
				
		for(int i = 0; i < length; i++)
		{
			((Particle3D)particles.get(i)).resetPosSpeed();
		}
		Particle3DTracker.updateAll();
		
	}
	
	
	
	//Resets posiiton and speed of particles to their initial position
	public void resetPosSpeed()
	{
		System.out.println("Resetting " + name + " to: " + initialPosition);
		setPosition(initialPosition);
		setSpeed(initialVelocityVector);
		setOrientation(initialOrientation);
		setAngularSpeed(initialAngularVelocity);
		bg.removeChild(bgTracer);
		
		bgTracer = new BranchGroup();
		bgTracer.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
		bgTracer.setCapability(BranchGroup.ALLOW_DETACH);		
		bg.addChild(bgTracer);
		
	}
	
	//Removes all particles from the system	
	public static void removeAll()
	{
		while(particles.size() > 0)
		{
			removeParticle((Particle3D)particles.get(0));
		}
	}
	

	/* File format:
	
	Gravity
	TraceNumFrames
	Number of Particles
	UFRBounding point x
	y
	z
	LBLBounding point x
	y
	z
	for each particle: name
	position x
	y
	z
	velocity x
	y
	z
	mass
	radius
	elasticity
	
	*/

	//Functions for loading and saving the particle to file...
	public static void saveAll(BufferedWriter w) throws IOException
	{
		w.write(gravity + "\n");
		w.write(traceNumFrames + "\n");
		w.write(particles.size() + "\n");
		if(ufrBounding == null || lblBounding == null)
		{
			w.write("null\nnull\nnull\nnull\nnull\nnull\n");
		}
		else
		{
			w.write(ufrBounding.x + "\n");
			w.write(ufrBounding.y + "\n");
			w.write(ufrBounding.z + "\n");
			w.write(lblBounding.x + "\n");
			w.write(lblBounding.y + "\n");
			w.write(lblBounding.z + "\n");
		}
		
		for(int i = 0; i < particles.size(); i++)
		{
			Particle3D current = (Particle3D)particles.get(i);
			w.write(current.name + "\n");
			w.write(current.initialPosition.x + "\n");
			w.write(current.initialPosition.y + "\n");
			w.write(current.initialPosition.z + "\n");
			w.write(current.initialVelocityVector.x + "\n");
			w.write(current.initialVelocityVector.y + "\n");
			w.write(current.initialVelocityVector.z + "\n");
			w.write(current.mass + "\n");
			w.write(current.radius + "\n");
			w.write(current.elasticity + "\n");
		}
	}
	
	//Load the particles from a buffered reader
	public static void loadAll(BufferedReader r, Physics3DThread t) throws IOException
	{
		setGravity(Double.parseDouble(r.readLine()));
		setTraceNumFrames(Integer.parseInt(r.readLine()));
		int length = Integer.parseInt(r.readLine());
		
		String line = r.readLine();
		//Read the bounding box
		if(line.equals("null"))
		{
			r.readLine();
			r.readLine();
			r.readLine();
			r.readLine();
			r.readLine();
			ufrBounding = null;
			lblBounding = null;