/**
  * Calculate new point(s) X in a B-A system to form B-A-X. Use C as reference for * staggering
  * about the B-A bond
  *
  * <p>(1a) 1 ligand(B) of refAtom (A) which itself has a ligand (C) (i) 1 points required; vector
  * along AB vector (ii) 2 points: 2 vectors in ABC plane, staggered and eclipsed wrt C (iii) 3
  * points: 1 staggered wrt C, the others +- gauche wrt C If C is null, a random non-colinear C is
  * generated
  *
  * @param aPoint to which substituents are added
  * @param nwanted number of points to calculate (1-3)
  * @param length A-X length
  * @param angle B-A-X angle
  * @return Point3d[] nwanted points (or zero if failed)
  */
 public static Point3d[] calculate3DCoordinates1(
     Point3d aPoint, Point3d bPoint, Point3d cPoint, int nwanted, double length, double angle) {
   Point3d points[] = new Point3d[nwanted];
   // BA vector
   Vector3d ba = new Vector3d(aPoint);
   ba.sub(bPoint);
   ba.normalize();
   // if no cPoint, generate a random reference
   if (cPoint == null) {
     Vector3d cVector = getNonColinearVector(ba);
     cPoint = new Point3d(cVector);
   }
   // CB vector
   Vector3d cb = new Vector3d(bPoint);
   cb.sub(cPoint);
   cb.normalize();
   // if A, B, C colinear, replace C by random point
   double cbdotba = cb.dot(ba);
   if (cbdotba > 0.999999) {
     Vector3d cVector = getNonColinearVector(ba);
     cPoint = new Point3d(cVector);
     cb = new Vector3d(bPoint);
     cb.sub(cPoint);
   }
   // cbxba = c x b
   Vector3d cbxba = new Vector3d();
   cbxba.cross(cb, ba);
   cbxba.normalize();
   // create three perp axes ba, cbxba, and ax
   Vector3d ax = new Vector3d();
   ax.cross(cbxba, ba);
   ax.normalize();
   double drot = Math.PI * 2.0 / (double) nwanted;
   for (int i = 0; i < nwanted; i++) {
     double rot = (double) i * drot;
     points[i] = new Point3d(aPoint);
     Vector3d vx = new Vector3d(ba);
     vx.scale(-Math.cos(angle) * length);
     Vector3d vy = new Vector3d(ax);
     vy.scale(Math.cos(rot) * length);
     Vector3d vz = new Vector3d(cbxba);
     vz.scale(Math.sin(rot) * length);
     points[i].add(vx);
     points[i].add(vy);
     points[i].add(vz);
   }
   return points;
 }
예제 #2
0
 public void setViewingDirection(double yaw, double pitch) {
   _viewingDirection.set(
       Math.sin(Math.toRadians(yaw)) * Math.cos(Math.toRadians(pitch)),
       -Math.sin(Math.toRadians(pitch)),
       -Math.cos(Math.toRadians(pitch)) * Math.cos(Math.toRadians(yaw)));
   _viewingDirection.normalize(_viewingDirection);
 }
예제 #3
0
  /**
   * Return a midpoint of a helix, calculated from three positions of three adjacent subunit
   * centers.
   *
   * @param p1 center of first subunit
   * @param p2 center of second subunit
   * @param p3 center of third subunit
   * @return midpoint of helix
   */
  private Point3d getMidPoint(Point3d p1, Point3d p2, Point3d p3) {
    Vector3d v1 = new Vector3d();
    v1.sub(p1, p2);
    Vector3d v2 = new Vector3d();
    v2.sub(p3, p2);
    Vector3d v3 = new Vector3d();
    v3.add(v1);
    v3.add(v2);
    v3.normalize();

    // calculat the total distance between to subunits
    double dTotal = v1.length();
    // calculate the rise along the y-axis. The helix axis is aligned with y-axis,
    // therfore, the rise between subunits is the y-distance
    double rise = p2.y - p1.y;
    // use phythagorean theoremm to calculate chord length between two subunit centers
    double chord = Math.sqrt(dTotal * dTotal - rise * rise);
    //		System.out.println("Chord d: " + dTotal + " rise: " + rise + "chord: " + chord);
    double angle = helixLayers.getByLargestContacts().getAxisAngle().angle;

    // using the axis angle and the chord length, we can calculate the radius of the helix
    // http://en.wikipedia.org/wiki/Chord_%28geometry%29
    double radius = chord / Math.sin(angle / 2) / 2; // can this go to zero?
    //		System.out.println("Radius: " + radius);

    // project the radius onto the vector that points toward the helix axis
    v3.scale(radius);
    v3.add(p2);
    //		System.out.println("Angle: " +
    // Math.toDegrees(helixLayers.getByLowestAngle().getAxisAngle().angle));
    Point3d cor = new Point3d(v3);
    return cor;
  }
 /**
  * Set up projection to plane in q-space
  *
  * @param qNormal normal to projection plane
  * @param qRow vector to describe direction of a row
  * @param qDeltas array of pixel sizes in q-space
  * @param rowLimits array containing start and (exclusive) stop values of row index
  * @param colLimits array containing start and (exclusive) stop values of columns index
  */
 public ProjectToQSpacePlane(
     Vector3d qNormal, Vector3d qRow, double[] qDeltas, int[] rowLimits, int[] colLimits) {
   qNormal.normalize();
   row = qRow;
   row.normalize();
   col = new Vector3d();
   col.cross(qNormal, col);
   col.normalize();
   rdel = qDeltas[0];
   cdel = qDeltas[1];
   pshape = new int[2];
   pshape[0] = rowLimits[1] - rowLimits[0];
   pshape[1] = colLimits[1] - colLimits[0];
   roff = rowLimits[0];
   coff = colLimits[0];
 }
예제 #5
0
  /** Set up coordinate system, return if successful */
  public boolean setupCS(Lineage refLin) {
    // NucLineage.Nuc nucABp = refLin.nuc.get("ABp");
    Lineage.Particle nucEMS = refLin.particle.get("EMS");

    Vector3d posABp = LineageMergeUtil.getLastPosABp(refLin);

    if (posABp == null || nucEMS == null || nucEMS.pos.isEmpty()) {
      System.out.println("Does not have enough cells marked, will not produce LR");
      return false;
    } else System.out.println("Will do LR");

    Vector3d posEMS = nucEMS.pos.get(nucEMS.pos.lastKey()).getPosCopy();

    ImVector2d dirvec = ImVector2d.polar(shell.major, shell.angle);
    Vector3d axisAP = new Vector3d(dirvec.x, dirvec.y, 0);

    // Approximate axis - but I want it perpendicular to the AP-axis defined by the shell
    Vector3d approxDV = new Vector3d();
    approxDV.sub(posEMS, posABp);

    Vector3d axisLR = new Vector3d();
    axisLR.cross(approxDV, axisAP);

    axisLR.normalize();
    double axisLength = 2 * shell.minor;
    axisLR.scale(1.0 / axisLength);
    this.axis = axisLR;

    return true;
  }
  @Override
  public void computeTransform(RigidBodyTransform currXform) {
    update();
    CameraMountInterface cameraMount = getCameraMount();
    if (isMounted() && (cameraMount != null)) {
      cameraMount.getTransformToCamera(currXform);

      return;
    }

    positionOffset.set(getCamX(), getCamY(), getCamZ());
    xAxis.set(getFixX(), getFixY(), getFixZ());

    fixPointNode.translateTo(getFixX(), getFixY(), getFixZ());

    xAxis.sub(positionOffset);
    xAxis.normalize();
    zAxis.set(0.0, 0.0, 1.0);
    yAxis.cross(zAxis, xAxis);
    zAxis.cross(xAxis, yAxis);

    rotationMatrix.setColumn(0, xAxis);
    rotationMatrix.setColumn(1, yAxis);
    rotationMatrix.setColumn(2, zAxis);

    currXform.setRotationAndZeroTranslation(rotationMatrix);
    currXform.setTranslation(positionOffset);
    currXform.normalizeRotationPart();
  }
예제 #7
0
  public void lookAt(Vector3d target) {
    Vector3d targetDirection = new Vector3d();
    targetDirection.sub(target, getPosition());
    targetDirection.normalize();

    setPitchYawFromVector(targetDirection);
  }
예제 #8
0
  /**
   * Calculates the shading in camera space
   *
   * @param p The hit point on the surface in camera space.
   * @param n The surface normal at the hit point in camera space.
   * @param eye The eye point in camera space.
   * @return
   */
  protected Color3f shade(Point3d p, Vector3d n, Point3d eye) {
    // normalize only if point is not singular
    float nLength = (float) n.length();
    if (nLength != 0.0f) n.scale(1.0f / nLength);

    // compute view vector
    Vector3d v = new Vector3d(eye);
    v.sub(p);
    v.normalize();
    /*
            // special coloring for blowup-visualization
            if( n.dot( v ) < 0.0f )
                n.negate();
            if( blowUpChooseMaterial( dcsd.rayCreator.cameraSpaceToSurfaceSpace( p ) ) )
            {
                return shadeWithMaterial( p, v, n, dcsd.frontAmbientColor, dcsd.frontLightProducts );
            }
            else
            {
                return shadeWithMaterial( p, v, n, dcsd.backAmbientColor, dcsd.backLightProducts );
            }
    */
    // compute, which material to use
    if (n.dot(v) > 0.0f) {
      return shadeWithMaterial(p, v, n, dcsd.frontAmbientColor, dcsd.frontLightProducts);
    } else {
      n.negate();
      return shadeWithMaterial(p, v, n, dcsd.backAmbientColor, dcsd.backLightProducts);
    }
  }
 public void perturb(Vector3d direction) {
   Vector3d force = new Vector3d(direction);
   if (direction.lengthSquared() > 0.0) {
     force.normalize();
     force.scale(disturbanceMagnitude.getDoubleValue());
     forcePerturbable.setForcePerturbance(force, disturbanceDuration.getDoubleValue());
   }
 }
 @Override
 public GroundProfile3D getGroundProfile() {
   Vector3d surfaceNormal = new Vector3d(0.03, 0.27, 1.0);
   surfaceNormal.normalize();
   Point3d intersectionPoint = new Point3d(0.22, 2.2, -0.4);
   double maxXY = 10.0;
   return new SlopedPlaneGroundProfile(surfaceNormal, intersectionPoint, maxXY);
 }
 // static helper method
 private static Vector3d getNormalizedVector(Vector3d v, Vector3d out) {
   out.set(v);
   out.normalize();
   if (out.length() < BulletGlobals.SIMD_EPSILON) {
     out.set(0, 0, 0);
   }
   return out;
 }
예제 #12
0
    boolean check(final ClosestPointPair cpair) {
      final Vector3d del = new Vector3d();

      del.sub(cpair.pnt2, cpair.pnt1);
      if (Math.abs(del.length() - d) > TOL) return false;
      del.normalize();
      if (!vec.epsilonEquals(del, TOL)) return false;
      return true;
    }
예제 #13
0
 private void setCameraPosition() {
   eye.set(0, 50, zTilt);
   eye.normalize();
   eye.scale(height);
   eyePoint.set(eye);
   // NOTE z axis is "up"
   up.set(0, 0, (zTilt < 0 ? 1 : -1));
   hoverTrans.lookAt(eyePoint, center, up);
   hoverTrans.invert();
   hoverTransformGroup.setTransform(hoverTrans);
 }
예제 #14
0
  /**
   * Shades a point with the same algorithm used by the {@link <a
   * href="http://surf.sourceforge.net">surf raytracer</a>}.
   *
   * @param hitPoint Intersection point.
   * @param v View vector (from intersection point to eye).
   * @param n Surface normal.
   * @param material Surface material.
   * @return
   */
  protected Color3f shadeWithMaterial(
      Point3d hitPoint,
      Vector3d v,
      Vector3d n,
      Color3f ambientColor,
      LightProducts[] lightProducts) {
    Vector3d l = new Vector3d();
    Vector3d h = new Vector3d();

    Color3f color = new Color3f(ambientColor);

    for (int i = 0; i < dcsd.lightSources.length; i++) {
      LightSource lightSource = dcsd.lightSources[i];

      l.sub(lightSource.getPosition(), hitPoint);
      l.normalize();

      float lambertTerm = (float) n.dot(l);
      if (lambertTerm > 0.0f) {
        // compute diffuse color component
        color.scaleAdd(lambertTerm, lightProducts[i].getDiffuseProduct(), color);

        // compute specular color component
        h.add(l, v);
        h.normalize();

        color.scaleAdd(
            (float)
                Math.pow(Math.max(0.0f, n.dot(h)), lightProducts[i].getMaterial().getShininess()),
            lightProducts[i].getSpecularProduct(),
            color);
      }
    }

    color.clampMax(1.0f);

    return color;
  }
예제 #15
0
 private Vector3d orthogonalize(Vector3d vector1, Vector3d vector2) {
   double dot = vector1.dot(vector2);
   Vector3d ref = new Vector3d(vector2);
   //		System.out.println("p.r: " + dot);
   //		System.out.println("Orig refVector: " + referenceVector);
   if (dot < 0) {
     vector2.negate();
   }
   if (Math.abs(dot) < 0.00001) {
     System.out.println("HelixAxisAligner: Warning: reference axis parallel");
   }
   vector2.cross(vector1, vector2);
   //		System.out.println("Intermed. refVector: " + vector2);
   vector2.normalize();
   //		referenceVector.cross(referenceVector, principalRotationVector);
   vector2.cross(vector1, vector2);
   vector2.normalize();
   if (ref.dot(vector2) < 0) {
     vector2.negate();
   }
   //		System.out.println("Mod. refVector: " + vector2);
   return vector2;
 }
  /**
   * Calculate new point(s) X in a B-A-C system. It forms form a B-A(-C)-X system.
   *
   * <p>(2) 2 ligands(B, C) of refAtom A (i) 1 points required; vector in ABC plane bisecting AB,
   * AC. If ABC is linear, no points (ii) 2 points: 2 points X1, X2, X1-A-X2 = angle about 2i vector
   *
   * @param aPoint to which substituents are added
   * @param bPoint first ligand of A
   * @param cPoint second ligand of A
   * @param nwanted number of points to calculate (1-2)
   * @param length A-X length
   * @param angle B-A-X angle
   * @return Point3d[] nwanted points (or zero if failed)
   */
  public static Point3d[] calculate3DCoordinates2(
      Point3d aPoint, Point3d bPoint, Point3d cPoint, int nwanted, double length, double angle) {
    Point3d newPoints[] = new Point3d[0];
    double ang2 = angle / 2.0;

    Vector3d ba = new Vector3d(aPoint);
    ba.sub(bPoint);
    Vector3d ca = new Vector3d(aPoint);
    ca.sub(cPoint);
    Vector3d baxca = new Vector3d();
    baxca.cross(ba, ca);
    if (baxca.length() < 0.00000001) {; // linear
    } else if (nwanted == 1) {
      newPoints = new Point3d[1];
      Vector3d ax = new Vector3d(ba);
      ax.add(ca);
      ax.normalize();
      ax.scale(length);
      newPoints[0] = new Point3d(aPoint);
      newPoints[0].add(ax);
    } else if (nwanted == 2) {
      newPoints = new Point3d[2];
      Vector3d ax = new Vector3d(ba);
      ax.add(ca);
      ax.normalize();
      baxca.normalize();
      baxca.scale(Math.sin(ang2) * length);
      ax.scale(Math.cos(ang2) * length);
      newPoints[0] = new Point3d(aPoint);
      newPoints[0].add(ax);
      newPoints[0].add(baxca);
      newPoints[1] = new Point3d(aPoint);
      newPoints[1].add(ax);
      newPoints[1].sub(baxca);
    }
    return newPoints;
  }
 /**
  * Rescales Point2 so that length 1-2 is sum of covalent radii. if covalent radii cannot be found,
  * use bond length of 1.0
  *
  * @param atom1 stationary atom
  * @param atom2 moveable atom
  * @param point2 coordinates for atom 2
  * @return new coords for atom 2
  */
 public static Point3d rescaleBondLength(IAtom atom1, IAtom atom2, Point3d point2) {
   Point3d point1 = atom1.getPoint3d();
   double d1 = atom1.getCovalentRadius();
   double d2 = atom2.getCovalentRadius();
   // in case we have no covalent radii, set to 1.0
   double distance =
       (d1 < 0.1 || d2 < 0.1) ? 1.0 : atom1.getCovalentRadius() + atom2.getCovalentRadius();
   Vector3d vect = new Vector3d(point2);
   vect.sub(point1);
   vect.normalize();
   vect.scale(distance);
   Point3d newPoint = new Point3d(point1);
   newPoint.add(vect);
   return newPoint;
 }
  @SuppressWarnings("all")
  protected void updateTargetPositionBasedOnCollision(
      Vector3d hitNormal, double tangentMag, double normalMag) {
    Vector3d movementDirection = new Vector3d();
    movementDirection.sub(targetPosition, currentPosition);
    double movementLength = movementDirection.length();
    if (movementLength > BulletGlobals.SIMD_EPSILON) {
      movementDirection.normalize();

      Vector3d reflectDir =
          computeReflectionDirection(movementDirection, hitNormal, new Vector3d());
      reflectDir.normalize();

      Vector3d parallelDir = parallelComponent(reflectDir, hitNormal, new Vector3d());
      Vector3d perpindicularDir = perpindicularComponent(reflectDir, hitNormal, new Vector3d());

      targetPosition.set(currentPosition);

      if (false) // tangentMag != 0.0)
      {
        Vector3d parComponent = new Vector3d();
        parComponent.scale(tangentMag * movementLength, parallelDir);
        // printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
        targetPosition.add(parComponent);
      }

      if (normalMag != 0.0f) {
        Vector3d perpComponent = new Vector3d();
        perpComponent.scale(normalMag * movementLength, perpindicularDir);
        // printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
        targetPosition.add(perpComponent);
      }
    } else {
      // printf("movementLength don't normalize a zero vector\n");
    }
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testChainAgainstCentroidalMomentumMatrix() {
    Random random = new Random(17679L);

    ArrayList<RevoluteJoint> joints = new ArrayList<RevoluteJoint>();
    RigidBody elevator = new RigidBody("elevator", world);
    int nJoints = 10;
    Vector3d[] jointAxes = new Vector3d[nJoints];
    for (int i = 0; i < nJoints; i++) {
      Vector3d jointAxis = RandomTools.generateRandomVector(random);
      jointAxis.normalize();
      jointAxes[i] = jointAxis;
    }

    ScrewTestTools.createRandomChainRobot("randomChain", joints, elevator, jointAxes, random);
    InverseDynamicsJoint[] jointsArray = new RevoluteJoint[joints.size()];
    joints.toArray(jointsArray);
    ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator);

    for (RevoluteJoint joint : joints) {
      joint.setQ(random.nextDouble());
      joint.setQd(random.nextDouble());
    }

    Momentum momentum = computeMomentum(elevator, centerOfMassFrame);
    DenseMatrix64F momentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
    momentum.getMatrix(momentumMatrix);

    CentroidalMomentumMatrix centroidalMomentumMatrix =
        new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
    centroidalMomentumMatrix.compute();
    DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getMatrix();
    DenseMatrix64F jointVelocitiesMatrix =
        new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jointsArray), 1);
    ScrewTools.getJointVelocitiesMatrix(jointsArray, jointVelocitiesMatrix);
    DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
    CommonOps.mult(
        centroidalMomentumMatrixMatrix,
        jointVelocitiesMatrix,
        momentumFromCentroidalMomentumMatrix);

    double epsilon = 1e-9;
    assertEquals(momentum.getExpressedInFrame(), centerOfMassFrame);
    JUnitTools.assertMatrixEquals(momentumFromCentroidalMomentumMatrix, momentumMatrix, epsilon);
    double norm = NormOps.normP2(momentumMatrix);
    assertTrue(norm > epsilon);
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testSingleRigidBodyRotation() {
    Random random = new Random(1766L);

    RigidBody elevator = new RigidBody("elevator", world);
    Vector3d jointAxis = RandomTools.generateRandomVector(random);
    jointAxis.normalize();
    RigidBodyTransform transformToParent = new RigidBodyTransform();
    transformToParent.setIdentity();
    RevoluteJoint joint =
        ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
    RigidBody body =
        ScrewTools.addRigidBody(
            "body",
            joint,
            RandomTools.generateRandomDiagonalMatrix3d(random),
            random.nextDouble(),
            new Vector3d());

    joint.setQ(random.nextDouble());
    joint.setQd(random.nextDouble());

    Momentum momentum = computeMomentum(elevator, world);

    momentum.changeFrame(world);
    FrameVector linearMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
    FrameVector angularMomentum =
        new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());

    FrameVector linearMomentumCheck = new FrameVector(world);
    Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
    Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
    angularMomentumCheckVector.scale(joint.getQd());
    inertia.transform(angularMomentumCheckVector);
    FrameVector angularMomentumCheck =
        new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
    angularMomentumCheck.changeFrame(world);

    double epsilon = 1e-9;
    JUnitTools.assertTuple3dEquals(
        linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
    JUnitTools.assertTuple3dEquals(
        angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
    assertTrue(angularMomentum.length() > epsilon);
  }
예제 #21
0
 public void move() {
   Vector3d velocity = new Vector3d(cameraXP - cameraXN, 0, cameraZP - cameraZN);
   velocity.normalize();
   if (Double.isNaN(velocity.x)) {
     velocity = new Vector3d();
   }
   position.add(velocity);
   if (position.x < -maxX) {
     position.x = -maxX;
   } else if (position.x > maxX) {
     position.x = maxX;
   }
   if (position.z < -maxZ) {
     position.z = -maxZ;
   } else if (position.z > maxZ) {
     position.z = maxZ;
   }
 }
    @Override
    public void performEffect(EntityLivingBase entity, int id) {
      // super magnetic - inspired by botanias code
      double x = entity.posX;
      double y = entity.posY;
      double z = entity.posZ;
      double range = 1.8d;
      range += entity.getActivePotionEffect(this).getAmplifier() * 0.3f;

      List<EntityItem> items =
          entity.worldObj.getEntitiesWithinAABB(
              EntityItem.class,
              new AxisAlignedBB(x - range, y - range, z - range, x + range, y + range, z + range));
      int pulled = 0;
      for (EntityItem item : items) {
        if (item.getEntityItem() == null || item.getEntityItem().getItem() == null || item.isDead) {
          continue;
        }

        if (pulled > 200) {
          break;
        }

        // constant force!
        float strength = 0.07f;

        // calculate direction: item -> player
        Vector3d vec = new Vector3d(x, y, z);
        vec.sub(new Vector3d(item.posX, item.posY, item.posZ));

        vec.normalize();
        vec.scale(strength);

        // we calculated the movement vector and set it to the correct strength.. now we apply it
        // \o/
        item.motionX += vec.x;
        item.motionY += vec.y;
        item.motionZ += vec.z;

        pulled++;
      }
    }
 /**
  * Calculate new point X in a B-A(-D)-C system. It forms a B-A(-D)(-C)-X system.
  *
  * <p>(3) 3 ligands(B, C, D) of refAtom A (i) 1 points required; if A, B, C, D coplanar, no
  * points. else vector is resultant of BA, CA, DA
  *
  * @param aPoint to which substituents are added
  * @param bPoint first ligand of A
  * @param cPoint second ligand of A
  * @param dPoint third ligand of A
  * @param length A-X length
  * @return Point3d nwanted points (or null if failed (coplanar))
  */
 public static Point3d calculate3DCoordinates3(
     Point3d aPoint, Point3d bPoint, Point3d cPoint, Point3d dPoint, double length) {
   Vector3d v1 = new Vector3d(aPoint);
   v1.sub(bPoint);
   Vector3d v2 = new Vector3d(aPoint);
   v2.sub(cPoint);
   Vector3d v3 = new Vector3d(aPoint);
   v3.sub(dPoint);
   Vector3d v = new Vector3d(bPoint);
   v.add(cPoint);
   v.add(dPoint);
   if (v.length() < 0.00001) {
     return null;
   }
   v.normalize();
   v.scale(length);
   Point3d point = new Point3d(aPoint);
   point.add(v);
   return point;
 }
예제 #24
0
  public void interactionForce(Particle other) {
    // ok, now for the fun stuff...
    Vector3d posDif = new Vector3d();
    posDif.sub(x, other.x);

    Vector3d velDif = new Vector3d();
    velDif.sub(v, other.v);

    double d = posDif.length();
    double dSquared = d * d;

    int m = 6;
    int n = 5;
    double r0 = 2 * PARTICLE_RADIUS;
    double cr = r0;
    double cd = r0;
    double b1 = 1;
    double b2 = b1; // *Math.pow(r0, n-m);
    double sumR = 2 * PARTICLE_RADIUS;
    double sr = 250;
    double sd = 70;

    /*sr = dSquared/(cr*cr*(sumR)*(sumR));
    sd = dSquared/(cd*cd*(sumR)*(sumR));

    sr = Math.max(0, 1 - sr);
    sd = Math.max(0, 1 - sd);*/

    Vector3d f = new Vector3d();
    f.set(posDif);
    f.normalize();

    double sf =
        -sr * (b1 / Math.pow(d / r0, m) - b2 / Math.pow(d / r0, n))
            + sd * (velDif.dot(f) / (d / r0));
    f.scale(sf);
    other.f.add(f);
  }
예제 #25
0
  /** performs the gl camera transformations for the right eye */
  public void translateRightEye(GL gl, GLU glu, GLUT glut) {
    // calculate the right vector
    Vector3d right = new Vector3d();
    Vector3d temp1 = (Vector3d) cameraTarget.clone();
    temp1.scale(-1);
    Vector3d temp2 = (Vector3d) cameraPosition.clone();

    temp2.add(temp1);
    right.cross(cameraUp, temp2);

    right.normalize();
    right.scale(eyespread / 2.);
    glu.gluLookAt(
        right.x + cameraPosition.x,
        right.y + cameraPosition.y,
        right.z + cameraPosition.z,
        right.x + cameraTarget.x,
        right.y + cameraTarget.y,
        right.z + cameraTarget.z,
        cameraUp.x,
        cameraUp.y,
        cameraUp.z);
  }
  public void doMouseDraggedMiddle(double dx, double dy) {
    // Zooms in and out

    if ((this.isMounted) && (viewportAdapter != null)) {
      cameraMount.zoom(dy * 0.1);
    } else {
      Vector3d v3d = new Vector3d(camX - fixX, camY - fixY, camZ - fixZ);

      Vector3d offsetVec = new Vector3d(v3d);

      // offsetVec.normalize();
      offsetVec.scale(dy * zoom_factor);

      // if (offsetVec.length() < v3d.length())
      // {
      if (!isDolly || (!isDollyX && !isDollyY)) {
        camX += offsetVec.getX();
        camY += offsetVec.getY();
      }

      if (!isDolly || !isDollyZ) camZ += offsetVec.getZ();

      // }

      v3d.set(camX - fixX, camY - fixY, camZ - fixZ);

      if (v3d.length() < MIN_CAMERA_POSITION_TO_FIX_DISTANCE) {
        v3d.normalize();
        v3d.scale(MIN_CAMERA_POSITION_TO_FIX_DISTANCE);
        camX = v3d.getX() + fixX;
        camY = v3d.getY() + fixY;
        camZ = v3d.getZ() + fixZ;
      }
    }

    // transformChanged(currXform);
  }
  protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
    // printf("m_normalizedDirection=%f,%f,%f\n",
    // 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
    // phase 2: forward and strafe
    Transform start = new Transform();
    Transform end = new Transform();
    targetPosition.add(currentPosition, walkMove);
    start.setIdentity();
    end.setIdentity();

    double fraction = 1.0f;
    Vector3d distance2Vec = new Vector3d();
    distance2Vec.sub(currentPosition, targetPosition);
    double distance2 = distance2Vec.lengthSquared();
    // printf("distance2=%f\n",distance2);

    /*if (touchingContact) {
    	if (normalizedDirection.dot(touchingNormal) > 0.0f) {
    		updateTargetPositionBasedOnCollision(touchingNormal);
    	}
    }*/

    int maxIter = 10;

    while (fraction > 0.01f && maxIter-- > 0) {
      start.origin.set(currentPosition);
      end.origin.set(targetPosition);

      KinematicClosestNotMeConvexResultCallback callback =
          new KinematicClosestNotMeConvexResultCallback(
              ghostObject, upAxisDirection[upAxis], -1.0f);
      callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
      callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

      double margin = convexShape.getMargin();
      convexShape.setMargin(margin + addedMargin);

      if (useGhostObjectSweepTest) {
        ghostObject.convexSweepTest(
            convexShape,
            start,
            end,
            callback,
            collisionWorld.getDispatchInfo().allowedCcdPenetration);
      } else {
        collisionWorld.convexSweepTest(convexShape, start, end, callback);
      }

      convexShape.setMargin(margin);

      fraction -= callback.closestHitFraction;

      if (callback.hasHit()) {
        // we moved only a fraction
        Vector3d hitDistanceVec = new Vector3d();
        hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
        // double hitDistance = hitDistanceVec.length();

        // if the distance is farther than the collision margin, move
        // if (hitDistance > addedMargin) {
        //	//printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
        //	currentPosition.interpolate(currentPosition, targetPosition,
        // callback.closestHitFraction);
        // }

        updateTargetPositionBasedOnCollision(callback.hitNormalWorld);

        Vector3d currentDir = new Vector3d();
        currentDir.sub(targetPosition, currentPosition);
        distance2 = currentDir.lengthSquared();
        if (distance2 > BulletGlobals.SIMD_EPSILON) {
          currentDir.normalize();
          // see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
          // oscilations in sloping corners."
          if (currentDir.dot(normalizedDirection) <= 0.0f) {
            break;
          }
        } else {
          // printf("currentDir: don't normalize a zero vector\n");
          break;
        }
      } else {
        // we moved whole way
        currentPosition.set(targetPosition);
      }

      // if (callback.m_closestHitFraction == 0.f)
      //    break;
    }
  }
예제 #28
0
  /**
   * Returns a transformation matrix that rotates refPoints to match coordPoints
   *
   * @param refPoints the points to be aligned
   * @param referenceVectors
   * @return
   */
  private Matrix4d alignAxes(Vector3d[] axisVectors, Vector3d[] referenceVectors) {
    Matrix4d m1 = new Matrix4d();
    AxisAngle4d a = new AxisAngle4d();
    Vector3d axis = new Vector3d();

    // calculate rotation matrix to rotate refPoints[0] into coordPoints[0]
    Vector3d v1 = new Vector3d(axisVectors[0]);
    Vector3d v2 = new Vector3d(referenceVectors[0]);
    double dot = v1.dot(v2);
    if (Math.abs(dot) < 0.999) {
      axis.cross(v1, v2);
      axis.normalize();
      a.set(axis, v1.angle(v2));
      m1.set(a);
      // make sure matrix element m33 is 1.0. It's 0 on Linux.
      m1.setElement(3, 3, 1.0);
    } else if (dot > 0) {
      // parallel axis, nothing to do -> identity matrix
      m1.setIdentity();
    } else if (dot < 0) {
      // anti-parallel axis, flip around x-axis
      m1.set(flipX());
    }

    // apply transformation matrix to all refPoints
    m1.transform(axisVectors[0]);
    m1.transform(axisVectors[1]);

    // calculate rotation matrix to rotate refPoints[1] into coordPoints[1]
    v1 = new Vector3d(axisVectors[1]);
    v2 = new Vector3d(referenceVectors[1]);
    Matrix4d m2 = new Matrix4d();
    dot = v1.dot(v2);
    if (Math.abs(dot) < 0.999) {
      axis.cross(v1, v2);
      axis.normalize();
      a.set(axis, v1.angle(v2));
      m2.set(a);
      // make sure matrix element m33 is 1.0. It's 0 on Linux.
      m2.setElement(3, 3, 1.0);
    } else if (dot > 0) {
      // parallel axis, nothing to do -> identity matrix
      m2.setIdentity();
    } else if (dot < 0) {
      // anti-parallel axis, flip around z-axis
      m2.set(flipZ());
    }

    // apply transformation matrix to all refPoints
    m2.transform(axisVectors[0]);
    m2.transform(axisVectors[1]);

    // combine the two rotation matrices
    m2.mul(m1);

    // the RMSD should be close to zero
    Point3d[] axes = new Point3d[2];
    axes[0] = new Point3d(axisVectors[0]);
    axes[1] = new Point3d(axisVectors[1]);
    Point3d[] ref = new Point3d[2];
    ref[0] = new Point3d(referenceVectors[0]);
    ref[1] = new Point3d(referenceVectors[1]);
    if (SuperPosition.rmsd(axes, ref) > 0.1) {
      System.out.println(
          "Warning: AxisTransformation: axes alignment is off. RMSD: "
              + SuperPosition.rmsd(axes, ref));
    }

    return m2;
  }
예제 #29
0
  /**
   * Draws the five sides of the hemicube into the provided drawable
   *
   * @param drawable
   * @param which
   * @param near
   * @param far
   * @param bCollect
   */
  public void drawHemiCube(
      GLAutoDrawable drawable, int which, double near, double far, boolean bCollect) {

    // TODO - PART 1 - DRAW HEMICUBE!

    GL gl = drawable.getGL();
    GLU glu = new GLU();

    /* Return the face based on which parameter */
    int[] vertices = scene.getObject().getFace(which);

    /* Find the center of the face */
    /* Center of the face is the average of the three vertices */

    /* Using returned list of vertex indices, find the vertices
     * corresponding to a particular face */
    Vertex v1 = scene.getObject().vertexList.get(vertices[0]);
    Vertex v2 = scene.getObject().vertexList.get(vertices[1]);
    Vertex v3 = scene.getObject().vertexList.get(vertices[2]);

    /* Locate center of face */
    /* Average of three vertices */
    Point3d centerPoint = new Point3d();
    centerPoint = new Point3d(v1.p);
    centerPoint.add(v2.p);
    centerPoint.add(v3.p);
    centerPoint.scale(0.33333);

    /* Set up camera frame */

    /* --- Surface normal --- */
    /* Declare points of vertex for face */
    Point3d p1 = new Point3d(v1.p);
    Point3d p2 = new Point3d(v2.p);
    Point3d p3 = new Point3d(v3.p);

    /* Declare vector u as p2-p1 */
    Point3d uVec = new Point3d(p2);
    uVec.sub(p1);
    Vector3d u = new Vector3d(uVec);

    /* Declare vector v as p3-p1 */
    Point3d vVec = new Point3d(p3);
    vVec.sub(p1);
    Vector3d v = new Vector3d(vVec);

    /* Make normal vector */
    Vector3d norm = new Vector3d();
    norm.cross(u, v);

    /* --- Vectors Orthonormal to Normal --- */
    Point3d vec1pt = new Point3d(p1);
    vec1pt.sub(p2);
    Vector3d vec1 = new Vector3d(vec1pt);
    vec1.cross(vec1, norm); // Cross surface normal with vec1 to get orthogonal vector

    Vector3d vec2 = new Vector3d();
    vec2.cross(
        norm,
        vec1); // Cross product of surface normal with new vector vec1 to get 2nd orthogonal vector

    /* Make unit vectors */
    norm.normalize();
    vec1.normalize();
    vec2.normalize();

    /* Set up the five different frustums, and stitch together using viewPort */
    /* Viewport to set up the view of the scene */

    /* ----- FRONT FACE ----- */
    gl.glPushMatrix();
    gl.glViewport(0, 0, drawable.getWidth(), drawable.getHeight() / 3);

    /* Set up frustums for this face */

    gl.glMatrixMode(gl.GL_PROJECTION);
    gl.glLoadIdentity();
    gl.glFrustum(-near, near, -near, near, near, far);

    /* Position camera at center of specified patch (which) */
    gl.glMatrixMode(gl.GL_MODELVIEW);
    gl.glLoadIdentity();
    glu.gluLookAt(
        centerPoint.x,
        centerPoint.y,
        centerPoint.z,
        centerPoint.x + norm.x,
        centerPoint.y + norm.y,
        centerPoint.z + norm.z,
        centerPoint.x + vec1.x,
        centerPoint.y + vec1.y,
        centerPoint.z + vec1.z);

    /* Draw the frustum to screen */
    draw(drawable, scene.drawStyle.IDENT);
    gl.glPopMatrix();

    /* ----- BOTTOM FACE ----- */
    gl.glPushMatrix();
    gl.glViewport(0, drawable.getHeight() / 3, drawable.getWidth(), drawable.getHeight() / 6);

    gl.glMatrixMode(gl.GL_PROJECTION);
    gl.glLoadIdentity();
    gl.glFrustum(-near, near, 0, near, near, far);

    /* Position camera at center of specified patch (which) */
    gl.glMatrixMode(gl.GL_MODELVIEW);
    gl.glLoadIdentity();
    glu.gluLookAt(
        centerPoint.x,
        centerPoint.y,
        centerPoint.z,
        centerPoint.x + (-vec1.x),
        centerPoint.y + (-vec1.y),
        centerPoint.z + (-vec1.z),
        centerPoint.x + norm.x,
        centerPoint.y + norm.y,
        centerPoint.z + norm.z);

    /* Draw the frustum to screen */
    draw(drawable, scene.drawStyle.IDENT);
    gl.glPopMatrix();

    /* ----- TOP FACE ----- */
    gl.glPushMatrix();
    gl.glViewport(
        0,
        (drawable.getHeight() / 3) + (drawable.getHeight() / 6),
        drawable.getWidth(),
        drawable.getHeight() / 6);

    gl.glMatrixMode(gl.GL_PROJECTION);
    gl.glLoadIdentity();
    gl.glFrustum(-near, near, -near, 0, near, far);

    /* Position camera at center of specified patch (which) */
    gl.glMatrixMode(gl.GL_MODELVIEW);
    gl.glLoadIdentity();
    glu.gluLookAt(
        centerPoint.x,
        centerPoint.y,
        centerPoint.z,
        centerPoint.x + vec1.x,
        centerPoint.y + vec1.y,
        centerPoint.z + vec1.z,
        centerPoint.x - norm.x,
        centerPoint.y - norm.y,
        centerPoint.z - norm.z);

    /* Draw the frustum to screen */
    draw(drawable, scene.drawStyle.IDENT);
    gl.glPopMatrix();

    /* ----- LEFT FACE ----- */
    gl.glPushMatrix();
    gl.glViewport(
        0,
        (drawable.getHeight() / 3) + 2 * (drawable.getHeight() / 6),
        drawable.getWidth(),
        drawable.getHeight() / 6);

    gl.glMatrixMode(gl.GL_PROJECTION);
    gl.glLoadIdentity();
    gl.glFrustum(0, near, -near, near, near, far);

    /* Position camera at center of specified patch (which) */
    gl.glMatrixMode(gl.GL_MODELVIEW);
    gl.glLoadIdentity();
    glu.gluLookAt(
        centerPoint.x,
        centerPoint.y,
        centerPoint.z,
        centerPoint.x + vec2.x,
        centerPoint.y + vec2.y,
        centerPoint.z + vec2.z,
        centerPoint.x + vec1.x,
        centerPoint.y + vec1.y,
        centerPoint.z + vec1.z);

    /* Draw the frustum to screen */
    draw(drawable, scene.drawStyle.IDENT);
    gl.glPopMatrix();

    /* ----- RIGHT FACE ----- */
    gl.glPushMatrix();
    gl.glViewport(
        0,
        (drawable.getHeight() / 3) + 3 * (drawable.getHeight() / 6),
        drawable.getWidth(),
        drawable.getHeight() / 6);

    gl.glMatrixMode(gl.GL_PROJECTION);
    gl.glLoadIdentity();
    gl.glFrustum(near, 0, -near, near, near, far);

    /* Position camera at center of specified patch (which) */
    gl.glMatrixMode(gl.GL_MODELVIEW);
    gl.glLoadIdentity();
    glu.gluLookAt(
        centerPoint.x,
        centerPoint.y,
        centerPoint.z,
        centerPoint.x + (-vec2.x),
        centerPoint.y + (-vec2.y),
        centerPoint.z + (-vec2.z),
        centerPoint.x + vec1.x,
        centerPoint.y + vec1.y,
        centerPoint.z + vec1.z);

    /* Draw the frustum to screen */
    draw(drawable, scene.drawStyle.IDENT);
    gl.glPopMatrix();

    /* ---- End Frustums ---- */

    // if collecting the form factors, then read back and process the data
    if (bCollect) {
      gl.glFlush();
      gl.glFinish();
      gl.glReadPixels(
          0, 0, divisions * 2, divisions * 6, GL.GL_RGBA, GL.GL_UNSIGNED_BYTE, pixelDataBuffer);
      collectData(which);
    }
  }
예제 #30
0
  // called on the parent object
  // Should be synchronized so that the user thread does not modify the
  // OrientedShape3D params while computing the transform
  synchronized void updateOrientedTransform(Canvas3D canvas, int viewIndex) {
    double angle = 0.0;
    double sign;
    boolean status;

    Transform3D orientedxform = getOrientedTransform(viewIndex);
    //  get viewplatforms's location in virutal world
    if (mode == OrientedShape3D.ROTATE_ABOUT_AXIS) { // rotate about axis
      canvas.getCenterEyeInImagePlate(viewPosition);
      canvas.getImagePlateToVworld(xform); // xform is imagePlateToLocal
      xform.transform(viewPosition);

      // get billboard's transform
      xform.set(getCurrentLocalToVworld());
      xform.invert(); // xform is now vWorldToLocal

      // transform the eye position into the billboard's coordinate system
      xform.transform(viewPosition);

      // eyeVec is a vector from the local origin to the eye pt in local
      eyeVec.set(viewPosition);
      eyeVec.normalize();

      // project the eye into the rotation plane
      status = projectToPlane(eyeVec, nAxis);

      if (status) {
        // project the z axis into the rotation plane
        zAxis.x = 0.0;
        zAxis.y = 0.0;
        zAxis.z = 1.0;
        status = projectToPlane(zAxis, nAxis);
      }
      if (status) {

        // compute the sign of the angle by checking if the cross product
        // of the two vectors is in the same direction as the normal axis
        vector.cross(eyeVec, zAxis);
        if (vector.dot(nAxis) > 0.0) {
          sign = 1.0;
        } else {
          sign = -1.0;
        }

        // compute the angle between the projected eye vector and the
        // projected z

        double dot = eyeVec.dot(zAxis);
        if (dot > 1.0f) {
          dot = 1.0f;
        } else if (dot < -1.0f) {
          dot = -1.0f;
        }

        angle = sign * Math.acos(dot);

        // use -angle because xform is to *undo* rotation by angle
        aa.x = nAxis.x;
        aa.y = nAxis.y;
        aa.z = nAxis.z;
        aa.angle = -angle;
        orientedxform.set(aa);
      } else {
        orientedxform.setIdentity();
      }

    } else if (mode == OrientedShape3D.ROTATE_ABOUT_POINT) { // rotate about point
      // Need to rotate Z axis to point to eye, and Y axis to be
      // parallel to view platform Y axis, rotating around rotation pt

      // get the eye point
      canvas.getCenterEyeInImagePlate(viewPosition);

      // derive the yUp point
      yUpPoint.set(viewPosition);
      yUpPoint.y += 0.01; // one cm in Physical space

      // transform the points to the Billboard's space
      canvas.getImagePlateToVworld(xform); // xform is ImagePlateToVworld
      xform.transform(viewPosition);
      xform.transform(yUpPoint);

      // get billboard's transform
      xform.set(getCurrentLocalToVworld());
      xform.invert(); // xform is vWorldToLocal

      // transfom points to local coord sys
      xform.transform(viewPosition);
      xform.transform(yUpPoint);

      // Make a vector from viewPostion to 0,0,0 in the BB coord sys
      eyeVec.set(viewPosition);
      eyeVec.normalize();

      // create a yUp vector
      yUp.set(yUpPoint);
      yUp.sub(viewPosition);
      yUp.normalize();

      // find the plane to rotate z
      zAxis.x = 0.0;
      zAxis.y = 0.0;
      zAxis.z = 1.0;

      // rotation axis is cross product of eyeVec and zAxis
      vector.cross(eyeVec, zAxis); // vector is cross product

      // if cross product is non-zero, vector is rotation axis and
      // rotation angle is acos(eyeVec.dot(zAxis)));
      double length = vector.length();
      if (length > 0.0001) {
        double dot = eyeVec.dot(zAxis);
        if (dot > 1.0f) {
          dot = 1.0f;
        } else if (dot < -1.0f) {
          dot = -1.0f;
        }
        angle = Math.acos(dot);
        aa.x = vector.x;
        aa.y = vector.y;
        aa.z = vector.z;
        aa.angle = -angle;
        zRotate.set(aa);
      } else {
        // no rotation needed, set to identity (scale = 1.0)
        zRotate.set(1.0);
      }

      // Transform the yAxis by zRotate
      yAxis.x = 0.0;
      yAxis.y = 1.0;
      yAxis.z = 0.0;
      zRotate.transform(yAxis);

      // project the yAxis onto the plane perp to the eyeVec
      status = projectToPlane(yAxis, eyeVec);

      if (status) {
        // project the yUp onto the plane perp to the eyeVec
        status = projectToPlane(yUp, eyeVec);
      }

      if (status) {
        // rotation angle is acos(yUp.dot(yAxis));
        double dot = yUp.dot(yAxis);

        // Fix numerical error, otherwise acos return NULL
        if (dot > 1.0f) {
          dot = 1.0f;
        } else if (dot < -1.0f) {
          dot = -1.0f;
        }

        angle = Math.acos(dot);

        // check the sign by looking a the cross product vs the eyeVec
        vector.cross(yUp, yAxis); // vector is cross product
        if (eyeVec.dot(vector) < 0) {
          angle *= -1;
        }
        aa.x = eyeVec.x;
        aa.y = eyeVec.y;
        aa.z = eyeVec.z;
        aa.angle = -angle;
        xform.set(aa); // xform is now yRotate

        // rotate around the rotation point
        vector.x = rotationPoint.x;
        vector.y = rotationPoint.y;
        vector.z = rotationPoint.z; // vector to translate to RP
        orientedxform.set(vector); // translate to RP
        orientedxform.mul(xform); // yRotate
        orientedxform.mul(zRotate); // zRotate
        vector.scale(-1.0); // vector to translate back
        xform.set(vector); // xform to translate back
        orientedxform.mul(xform); // translate back
      } else {
        orientedxform.setIdentity();
      }
    }
    // Scale invariant computation
    if (constantScale) {
      // Back Xform a unit vector to local world coords
      canvas.getInverseVworldProjection(left_xform, right_xform);

      // the two endpts of the vector have to be transformed
      // individually because the Xform is not affine
      im_vec[0].set(0.0, 0.0, 0.0, 1.0);
      im_vec[1].set(1.0, 0.0, 0.0, 1.0);
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);

      left_xform.set(getCurrentLocalToVworld());
      left_xform.invert();
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);
      lvec.set(im_vec[1]);
      lvec.sub(im_vec[0]);

      // We simply need the direction of this vector
      lvec.normalize();
      im_vec[0].set(0.0, 0.0, 0.0, 1.0);
      im_vec[1].set(lvec);
      im_vec[1].w = 1.0;

      // Forward Xfrom to clip coords
      left_xform.set(getCurrentLocalToVworld());
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);

      canvas.getVworldProjection(left_xform, right_xform);
      left_xform.transform(im_vec[0]);
      left_xform.transform(im_vec[1]);

      // Perspective division
      im_vec[0].x /= im_vec[0].w;
      im_vec[0].y /= im_vec[0].w;
      im_vec[0].z /= im_vec[0].w;

      im_vec[1].x /= im_vec[1].w;
      im_vec[1].y /= im_vec[1].w;
      im_vec[1].z /= im_vec[1].w;

      lvec.set(im_vec[1]);
      lvec.sub(im_vec[0]);

      // Use the length of this vector to determine the scaling
      // factor
      double scale = 1 / lvec.length();

      // Convert to meters
      scale *= scaleFactor * canvas.getPhysicalWidth() / 2;

      // Scale object so that it appears the same size
      scaleXform.setScale(scale);
      orientedxform.mul(scaleXform);
    }
  }