javax.vecmath.Vector3d Java Examples

/**
 * Calculate solid angle subtended by a plane triangle with given vertices
 * @param a
 * @param b
 * @param c
 * @return solid angle
 */
public static double calculatePlaneTriangleSolidAngle(Vector3d a, Vector3d b, Vector3d c) {
	// A. van Oosterom & J. Strackee, "The Solid Angle of a Plane Triangle", IEEE Trans.
	// Biom Eng BME-30(2) pp125-6 (1983)
	// tan(Omega/2) = a . (b x c) / [ a * b * c + (a . b) * c + (b . c) * a + (c . a) *b ]
	double al = a.length();
	double bl = b.length();
	double cl = c.length();
	double denom = al * bl * cl + a.dot(b)*cl + al * b.dot(c) + a.dot(c) * bl; 
	Vector3d bc = new Vector3d();
	bc.cross(b, c);
	double ang = Math.atan(Math.abs(a.dot(bc))/denom);
	if (ang < 0) {
		ang += Math.PI;
	}
	return 2 * ang;
}
 

@Override
public void render(GL2 gl2) {
	gl2.glPushMatrix();
	MatrixHelper.applyMatrix(gl2, pose);
	Vector3d s = size.get();
	Vector3d pos = getPosition();
	Point3d bottom = new Point3d(pos.x-s.x/2,pos.y-s.y/2,pos.z);
	Point3d top   = new Point3d(pos.x+s.x/2,pos.y+s.y/2,pos.z+s.z);
	PrimitiveSolids.drawBoxWireframe(gl2, bottom,top);
	
	Vector3d ep = ee.getPosition();
	gl2.glBegin(GL2.GL_LINES);
	gl2.glVertex3d(ep.x,ep.y,ep.z);  gl2.glVertex3d(bottom.x,bottom.y,top.z);
	gl2.glVertex3d(ep.x,ep.y,ep.z);  gl2.glVertex3d(bottom.x,top   .y,top.z);
	gl2.glVertex3d(ep.x,ep.y,ep.z);  gl2.glVertex3d(top   .x,top   .y,top.z);
	gl2.glVertex3d(ep.x,ep.y,ep.z);  gl2.glVertex3d(top   .x,bottom.y,top.z);
	gl2.glEnd();
	
	gl2.glPopMatrix();
	
	super.render(gl2);
}
 

public Ball(double x, double y, double z, double vx, double vy, double vz) {
    position = new Vector3d(x, y, z);
    speed = new Vector3d(vx, vy, vz);

    // �ړ��”\
    this.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
    this.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);

    app = new Appearance();
    app.setTexture(loadTexture("carpet.jpg"));

    // �{�[����lj�
    this.addChild(new Sphere(radius, Sphere.GENERATE_TEXTURE_COORDS, 100, app));

    move();
}
 

/**
 * Find all Entities within epsilon mm of pose.
 * TODO Much optimization could be done here to reduce the search time.
 * @param target the center of the cube around which to search.   
 * @param radius the maximum distance to search for entities.
 * @return a list of found PhysicalObjects
 */
public List<PoseEntity> findPhysicalObjectsNear(Vector3d target,double radius) {
	radius/=2;
	
	//Log.message("Finding within "+epsilon+" of " + target);
	List<PoseEntity> found = new ArrayList<PoseEntity>();
	
	// check all children
	for( Entity e : children ) {
		if(e instanceof PoseEntity) {
			// is physical, therefore has position
			PoseEntity po = (PoseEntity)e;
			//Log.message("  Checking "+po.getDisplayName()+" at "+pop);
			Vector3d pop = new Vector3d();
			pop.sub(po.getPosition(),target);
			if(pop.length()<=radius) {
				//Log.message("  in range!");
				// in range!
				found.add(po);
			}
		}
	}
	
	return found;
}
 

@Override
public void update(long t) {
    // GPS
    if (t > gpsStartTime && t > gpsLast + gpsInterval) {
        gpsLast = t;
        gpsUpdated = true;
        GNSSReport gpsCurrent = new GNSSReport();
        Vector3d pos = object.getPosition();
        gpsCurrent.position = globalProjector.reproject(new double[]{pos.x, pos.y, pos.z});
        gpsCurrent.eph = 1.0;
        gpsCurrent.epv = 1.0;
        gpsCurrent.velocity = new Vector3d(object.getVelocity());
        gpsCurrent.fix = 3;
        gpsCurrent.time = System.currentTimeMillis() * 1000;
        gps = gpsDelayLine.getOutput(t, gpsCurrent);
    }
}
 

static public double CPADistance(Vector3d a,Vector3d b,Vector3d da,Vector3d db) {
	// find CPA time
	Vector3d dp = new Vector3d(b);
	dp.sub(a);
	Vector3d dv = new Vector3d(db);
	db.sub(da);		
	double t = CPATime(dp,dv);

	// get both points
	Vector3d pa = new Vector3d(da);
	pa.scale(t);
	pa.add(a);
	Vector3d pb = new Vector3d(db);
	pb.scale(t);
	pb.add(b);
	// find difference
	pb.sub(pa);
	return pb.length();
}
 

/**
 * @return longest vector from beam centre to farthest corner
 */
public Vector3d getLongestVector() {
	Vector3d[] corners = cornerPositions();
	Vector3d longVec = null;
	double length = -Double.MAX_VALUE;
	for (int i = 0; i < 4; i++) {
		Vector3d c = corners[i];
		c.sub(getBeamCentrePosition());
		double l = c.length();
		if (l > length) {
			longVec = c;
			length = l;
		}
	}
	return longVec;
}
 

/**
 * Returns true if both given transform ids belong to the same crystallographic axis (a, b or c)
 * For two non-rotation transformations (i.e. identity operators) it returns true
 * @param tId1
 * @param tId2
 * @return
 */
public boolean areInSameAxis(int tId1, int tId2) {
	if (tId1==tId2) return true;

	if (axisAngles== null) calcRotAxesAndAngles();

	if (getAxisFoldType(tId1)==1 && getAxisFoldType(tId2)==1) return true;

	// we can't deal yet with improper rotations: we return false whenever either of them is improper
	if (getAxisFoldType(tId1)<0 || getAxisFoldType(tId2)<0) return false;

	Vector3d axis1 = new Vector3d(axisAngles[tId1].x, axisAngles[tId1].y, axisAngles[tId1].z);
	Vector3d axis2 = new Vector3d(axisAngles[tId2].x, axisAngles[tId2].y, axisAngles[tId2].z);

	// TODO revise: we might need to consider that the 2 are in same direction but opposite senses
	// the method is not used at the moment anyway
	if (deltaComp(axis1.angle(axis2), 0.0, DELTA)) return true;

	return false;
}
 

/**
 * Rotate all the vertexes by a given amount
 * @param arg0 amount in degrees to rotate around X,Y, and then Z. 
 */
public void adjustRotation(Vector3d arg0) {
	// generate the pose matrix
	Matrix3d pose = new Matrix3d();
	Matrix3d rotX = new Matrix3d();
	Matrix3d rotY = new Matrix3d();
	Matrix3d rotZ = new Matrix3d();
	rotX.rotX((float)Math.toRadians(arg0.x));
	rotY.rotY((float)Math.toRadians(arg0.y));
	rotZ.rotZ((float)Math.toRadians(arg0.z));
	pose.set(rotX);
	pose.mul(rotY);
	pose.mul(rotZ);
	adjust.set(pose);
	isDirty=true;
}
 

public Main() {
    setLayout(new BorderLayout());
    GraphicsConfiguration config = SimpleUniverse
            .getPreferredConfiguration();
    Canvas3D canvas = new Canvas3D(config);
    add(canvas, BorderLayout.CENTER);

    universe = new SimpleUniverse(canvas);

    // �V�[�����\�z
    BranchGroup scene = createSceneGraph();
    scene.compile();

    // ���_���Z�b�g
    Transform3D viewPlatformTransform = new Transform3D();
    viewPlatformTransform.setTranslation(new Vector3d(0.0, 0.0, 10.0));
    universe.getViewingPlatform().getViewPlatformTransform().setTransform(viewPlatformTransform);

    // �}�E�X����
    orbitControls(canvas);

    universe.addBranchGraph(scene);
}
 

protected void updateIKEndEffector() {
	this.finger_forward.set(1,0,0);
	this.finger_left   .set(0,1,0);
	this.finger_up     .set(0,0,1);

	// roll, pitch, then yaw
	this.finger_forward = MathHelper.rotateAroundAxis(this.finger_forward,new Vector3d(1,0,0),(float)Math.toRadians(this.rotationAngleU));
	this.finger_forward = MathHelper.rotateAroundAxis(this.finger_forward,new Vector3d(0,1,0),(float)Math.toRadians(this.rotationAngleV));
	this.finger_forward = MathHelper.rotateAroundAxis(this.finger_forward,new Vector3d(0,0,1),(float)Math.toRadians(this.rotationAngleW));

	this.finger_up      = MathHelper.rotateAroundAxis(this.finger_up,     new Vector3d(1,0,0),(float)Math.toRadians(this.rotationAngleU));
	this.finger_up      = MathHelper.rotateAroundAxis(this.finger_up,     new Vector3d(0,1,0),(float)Math.toRadians(this.rotationAngleV));
	this.finger_up      = MathHelper.rotateAroundAxis(this.finger_up,     new Vector3d(0,0,1),(float)Math.toRadians(this.rotationAngleW));

	this.finger_left    = MathHelper.rotateAroundAxis(this.finger_left,   new Vector3d(1,0,0),(float)Math.toRadians(this.rotationAngleU));
	this.finger_left    = MathHelper.rotateAroundAxis(this.finger_left,   new Vector3d(0,1,0),(float)Math.toRadians(this.rotationAngleV));
	this.finger_left    = MathHelper.rotateAroundAxis(this.finger_left,   new Vector3d(0,0,1),(float)Math.toRadians(this.rotationAngleW));
}
 

protected void renderTranslationHandle(GL2 gl2,Vector3d n) {
	// draw line to box
	gl2.glBegin(GL2.GL_LINES);
	gl2.glVertex3d(0,0,0);
	gl2.glVertex3d(n.x,n.y,n.z);
	gl2.glEnd();

	// draw box
	Point3d b0 = new Point3d(+0.05,+0.05,+0.05); 
	Point3d b1 = new Point3d(-0.05,-0.05,-0.05);

	b0.scale(1);
	b1.scale(1);
	b0.add(n);
	b1.add(n);
	PrimitiveSolids.drawBox(gl2, b0,b1);
}
 

/**
 * @return a vector describing the row-wise component of a detector pixel in space.
 * I.e. the horizontal (in an image) edge of a pixel
 */
public Vector3d getPixelRow() {
	Vector3d horVec = new Vector3d(-hPxSize, 0, 0);

	if (invOrientation != null)
		invOrientation.transform(horVec);
	return horVec;
}
 

public DeltaRobot3() {
	super();
	setName(ROBOT_NAME);

	motionNow = new DeltaRobot3Keyframe();
	motionFuture = new DeltaRobot3Keyframe();
	
	setupBoundingVolumes();
	setHome(new Vector3d(0,0,0));
	
	isPortConfirmed=false;
	speed=2;
	aDir = 0.0f;
	bDir = 0.0f;
	cDir = 0.0f;
	xDir = 0.0f;
	yDir = 0.0f;
	zDir = 0.0f;
	
	try {
		modelTop = ModelEntity.createModelFromFilename("/DeltaRobot3.zip:top.STL");
		modelArm = ModelEntity.createModelFromFilename("/DeltaRobot3.zip:arm.STL");
		modelBase = ModelEntity.createModelFromFilename("/DeltaRobot3.zip:base.STL");
	} catch(Exception e) {
		e.printStackTrace();
	}
}
 

/**
 * transform a world-space point to the ball's current frame of reference
 * @param pointInWorldSpace the world space point
 * @return the transformed Vector3d
 */
public Vector3d getPickPointInFOR(Vector3d pointInWorldSpace,Matrix4d frameOfReference) {
	Matrix4d iMe = new Matrix4d(frameOfReference);
	iMe.m30=iMe.m31=iMe.m32=0;
	iMe.invert();
	Vector3d pickPointInBallSpace = new Vector3d(pointInWorldSpace);
	pickPointInBallSpace.sub(this.getPosition());
	iMe.transform(pickPointInBallSpace);
	return pickPointInBallSpace;
}
 

boolean Sit_Down(double dt) {
  int i;
  int legup=0;
  float scale=1.0f;

  // we've planted all feet, lower the body to the ground
  if( body.pos.z > 0 ) body.pos.z -= 2 * scale * dt;
  else {
    for( i = 0; i < 6; ++i ) {

      // raise feet
      Vector3d ls = new Vector3d( legs[i].ankle_joint.pos );
      ls.sub( legs[i].pan_joint.pos );
      if( ls.length() < 16 ) {
        ls.z=0;
        ls.normalize();
        ls.scale( 4 * scale * dt );
        legs[i].ankle_joint.pos.add( ls );
      } else ++legup;

      if( legs[i].ankle_joint.pos.z-legs[i].pan_joint.pos.z < 5.5 ) legs[i].ankle_joint.pos.z += 4 * scale * dt;
      else ++legup;

      if( legs[i].knee_joint.pos.z-legs[i].pan_joint.pos.z < 5.5 ) legs[i].knee_joint.pos.z += 4 * scale * dt;
      else ++legup;
    }
    if( legup == 6*3 ) return true;
  }

  return false;
}
 

void Stand_Up(double dt) {
  int i;
  int onfloor = 0;
  float scale = 2.0f;

  // touch the feet to the floor
  for(i=0;i<6;++i) {
    if(legs[i].ankle_joint.pos.z>0) legs[i].ankle_joint.pos.z-=4*scale*dt;
    else ++onfloor;

    // contract - put feet closer to shoulders
    Vector3d df = new Vector3d(legs[i].ankle_joint.pos);
    df.sub(body.pos);
    df.z=0;
    if(df.length()>standing_radius) {
      df.normalize();
      df.scale(6*scale*dt);
      legs[i].ankle_joint.pos.sub(df);
    }
  }

  if(onfloor==6) {
    // we've planted all feet, raise the body a bit
    if( body.pos.z < standing_height ) body.pos.z+=2*scale*dt;

    for(i=0;i<6;++i) {
      Vector3d ds = new Vector3d( legs[i].pan_joint.pos );
      ds.sub( body.pos );
      ds.normalize();
      ds.scale(standing_radius);
      legs[i].npoc.set(body.pos.x+ds.x,
    		  			body.pos.y+ds.y,
    		  			0);
    }
  }
}
 

@Override
public void setPosition(Vector3d p) {
	super.setPosition(p);
	position[0] = (float)p.x;
	position[1] = (float)p.y;
	position[2] = (float)p.z;
}
 

public void setupModels(DHRobotEntity robot) {
	material = new MaterialEntity();
	float r=1;
	float g=0f/255f;
	float b=0f/255f;
	material.setDiffuseColor(r,g,b,1);
	
	try {
		robot.links.get(0).setModelFilename("/Thor/Thor0.stl");
		robot.links.get(1).setModelFilename("/Thor/Thor1.stl");
		robot.links.get(2).setModelFilename("/Thor/Thor2.stl");
		robot.links.get(3).setModelFilename("/Thor/Thor3.stl");
		robot.links.get(5).setModelFilename("/Thor/Thor4.stl");
		robot.links.get(6).setModelFilename("/Thor/Thor5.stl");
		robot.links.get(7).setModelFilename("/Thor/Thor6.stl");

		robot.links.get(1).getModel().adjustOrigin(new Vector3d(0,0,-15.35));
		robot.links.get(1).getModel().adjustRotation(new Vector3d(0,0,90));
		robot.links.get(2).getModel().adjustOrigin(new Vector3d(0,0,-6.5));
		robot.links.get(2).getModel().adjustRotation(new Vector3d(0,0,90));
		robot.links.get(3).getModel().adjustRotation(new Vector3d(90,0,90));
		robot.links.get(3).getModel().adjustOrigin(new Vector3d(0,6,0));
		robot.links.get(5).getModel().adjustOrigin(new Vector3d(0,0,0));
		robot.links.get(5).getModel().adjustRotation(new Vector3d(0,0,90));
		robot.links.get(6).getModel().adjustOrigin(new Vector3d(0,0,-4.75));
		robot.links.get(6).getModel().adjustRotation(new Vector3d(0,0,90));
		robot.links.get(7).getModel().adjustOrigin(new Vector3d(0,0,0));
	} catch (Exception e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
	}
}
 

public EarthVector findPointReverseDirection(final EarthVector nextCoord, final double fraction) {
  // check for same point first
  if (equals(nextCoord)) {
    return new EarthVector(this);
  }

  // compute the vector normal to this vector and the input vector
  final Vector3d nextVector = nextCoord.getVector();
  final Vector3d vec = new Vector3d();
  vec.cross(ecfVector, nextVector);
  vec.negate();

  // compute the fractional angle between this vector and the input vector
  final double phi =
      fraction
          * Math.acos(ecfVector.dot(nextVector) / (ecfVector.length() * nextVector.length()));

  // create the vector rotated through phi about the normal vector
  final Vector3d output = rotate(vec, phi);
  // now scale the output vector by interpolating the magnitudes
  // of this vector and the input vector
  output.normalize();
  final double size =
      ((nextVector.length() - ecfVector.length()) * fraction) + ecfVector.length();
  output.scale(size);

  return new EarthVector(output);
}
 

public static EarthVector translateDegrees(
    final double lat,
    final double lon,
    final Vector3d translation) {
  final EarthVector result = EarthVector.fromDegrees(lat, lon);
  result.getVector().add(translation);
  return new EarthVector(result.getVector());
}
 

/**
 * Given a rotation matrix calculates the rotation axis and angle for it.
 * The angle is calculated from the trace, the axis from the eigenvalue
 * decomposition.
 * If given matrix is improper rotation or identity matrix then
 * axis (0,0,0) and angle 0 are returned.
 * @param m
 * @return
 * @throws IllegalArgumentException if given matrix is not a rotation matrix (determinant not 1 or -1)
 */
public static AxisAngle4d getRotAxisAndAngle(Matrix3d m) {
	double determinant = m.determinant();

	if (!(Math.abs(determinant)-1.0<DELTA)) throw new IllegalArgumentException("Given matrix is not a rotation matrix");

	AxisAngle4d axisAndAngle = new AxisAngle4d(new Vector3d(0,0,0),0);

	double[] d = {m.m00,m.m10,m.m20,
			m.m01,m.m11,m.m21,
			m.m02,m.m12,m.m22};

	Matrix r = new Matrix(d,3);

	if (!deltaComp(r.det(), 1.0, DELTA)) {
		// improper rotation: we return axis 0,0,0 and angle 0
		return axisAndAngle;
	}

	EigenvalueDecomposition evd = new EigenvalueDecomposition(r);

	Matrix eval = evd.getD();
	if (deltaComp(eval.get(0, 0),1.0,DELTA) && deltaComp(eval.get(1, 1),1.0,DELTA) && deltaComp(eval.get(2, 2),1.0,DELTA)) {
		// the rotation is an identity: we return axis 0,0,0 and angle 0
		return axisAndAngle;
	}
	int indexOfEv1;
	for (indexOfEv1=0;indexOfEv1<3;indexOfEv1++) {
		if (deltaComp(eval.get(indexOfEv1, indexOfEv1),1,DELTA)) break;
	}
	Matrix evec = evd.getV();
	axisAndAngle.set(new Vector3d(evec.get(0,indexOfEv1), evec.get(1, indexOfEv1), evec.get(2, indexOfEv1)),
			Math.acos((eval.trace()-1.0)/2.0));

	return axisAndAngle;
}
 

public RotaryStewartPlatform3() {
	super();
	dimensions = new RotaryStewartPlatform3Dimensions();
	setName(dimensions.ROBOT_NAME);

	motionNow = new RotaryStewartPlatformKeyframe(dimensions);
	motionFuture = new RotaryStewartPlatformKeyframe(dimensions);
	
	setupBoundingVolumes();
	setHome(new Vector3d(0,0,0));
	
	// set up the initial state of the machine
	isPortConfirmed=false;
	hasArmMoved = false;
	xDir = 0.0f;
	yDir = 0.0f;
	zDir = 0.0f;
	uDir = 0.0f;
	vDir = 0.0f;
	wDir = 0.0f;

	matBase.setDiffuseColor(69.0f/255.0f,115.0f/255.0f,133.0f/255.0f,1);
	matBicep.setDiffuseColor(2.0f/255.0f,39.0f/255.0f,53.0f/255.0f,1);
	matForearm.setDiffuseColor(39.0f/255.0f,88.0f/255.0f,107.0f/255.0f,1);
	matTop.setDiffuseColor(16.0f/255.0f,62.0f/255.0f,80.0f/255.0f,1);

	try {
		modelTop = ModelEntity.createModelFromFilename("/StewartPlatform3.zip:top.stl");
		modelBicep = ModelEntity.createModelFromFilename("/StewartPlatform3.zip:bicep.stl");
		modelBase = ModelEntity.createModelFromFilename("/StewartPlatform3.zip:base.stl");
		modelForearm = ModelEntity.createModelFromFilename("/StewartPlatform3.zip:forearm.stl");
	} catch(Exception e) {
		e.printStackTrace();
	}
}
 

/**
 * Translates a group object, given a Vector3d (i.e. the vecmath library
 * double-precision 3-d vector)
 *
 * @param group
 * @param v
 */
public static final void translate (Group group, Vector3d v) {

	for (Atom atom : group.getAtoms()) {
		translate(atom,v);
	}
	for (Group altG : group.getAltLocs()) {
		translate(altG, v);
	}
}
 

public void renderLineage(GL2 gl2) {
	boolean isTex = gl2.glIsEnabled(GL2.GL_TEXTURE_2D);
	gl2.glDisable(GL2.GL_TEXTURE_2D);

	// save the lighting mode
	boolean lightWasOn = gl2.glIsEnabled(GL2.GL_LIGHTING);
	gl2.glDisable(GL2.GL_LIGHTING);

	IntBuffer depthFunc = IntBuffer.allocate(1);
	gl2.glGetIntegerv(GL2.GL_DEPTH_FUNC, depthFunc);
	gl2.glDepthFunc(GL2.GL_ALWAYS);
	//boolean depthWasOn = gl2.glIsEnabled(GL2.GL_DEPTH_TEST);
	//gl2.glDisable(GL2.GL_DEPTH_TEST);

	gl2.glColor4d(1,1,1,1);
	gl2.glBegin(GL2.GL_LINES);
	// connection to children
	for(Entity e : children ) {
		if(e instanceof PoseEntity) {					
			Vector3d p = ((PoseEntity)e).getPosition();
			gl2.glVertex3d(0, 0, 0);
			gl2.glVertex3d(p.x,p.y,p.z);
		}
	}
	gl2.glEnd();

	//if(depthWasOn) gl2.glEnable(GL2.GL_DEPTH_TEST);
	gl2.glDepthFunc(depthFunc.get());
	// restore lighting
	if(lightWasOn) gl2.glEnable(GL2.GL_LIGHTING);
	if(isTex) gl2.glDisable(GL2.GL_TEXTURE_2D);
}
 

@Override
public Vector3d getAcc() {
    Vector3d accBody = new Vector3d(object.getAcceleration());
    accBody.sub(object.getWorld().getEnvironment().getG());
    Matrix3d rot = new Matrix3d(object.getRotation());
    rot.transpose();
    rot.transform(accBody);
    return accBody;
}
 

/**
 * Set distance from sample to beam centre.
 * <p>
 * Can throw an exception if direct beam does not intersect detector.
 * @param distance
 */
public void setBeamCentreDistance(double distance) {
	distance -= getBeamCentrePosition().length();
	Vector3d b = new Vector3d(beamVector);
	b.scale(distance);
	origin.add(b);
	fireDetectorPropertyListeners(new DetectorPropertyEvent(this, EventType.ORIGIN));
}
 

public void setRotation(Matrix3d m) {
	setChanged();
	t.setRotation(m);
	notifyObservers();
	
	Vector3d r = MatrixHelper.matrixToEuler(t);
	rot.set(Math.toDegrees(r.x),
			Math.toDegrees(r.y),
			Math.toDegrees(r.z));
}
 

/**
 * interpolate from a to b
 * @param a
 * @param b
 * @param t [0...1]
 * @return
 */
static public Vector3d interpolate(Vector3d a,Vector3d b,double t) {
	Vector3d n = new Vector3d(b);
	n.sub(a);
	n.scale((float)t);
	n.add(a);
	
	return n;
}
 

/**
 * Calculate solid angle subtended by pixel
 * @param x
 * @param y
 * @return solid angle
 */
public double calculateSolidAngle(final int x, final int y) {
	Vector3d a = pixelPosition(x, y);
	Vector3d ab = getPixelRow();
	Vector3d ac = getPixelColumn();

	Vector3d b = new Vector3d();
	Vector3d c = new Vector3d();
	b.add(a, ab);
	c.add(a, ac);
	double s = calculatePlaneTriangleSolidAngle(a, b, c);

	a.add(b, ac);
	return s + calculatePlaneTriangleSolidAngle(b, a, c); // order is important
}