Ejemplo n.º 1
0
  private boolean matchPair(final ClosestPointPair cpair, final ClosestPointPair check) {
    if (check.dist <= 0) { // then just verify collison
      if (cpair.dist > 0) return false;
    } else if (check.feat1 == null) {
      final Vector3d del = new Vector3d();
      del.sub(cpair.pnt2, cpair.pnt1);

      // then just check distance and direction
      if (Math.abs(check.dist - cpair.dist) > TOL || Math.abs(del.length() - check.dist) > TOL)
        return false;
      if (!check.nrml.epsilonEquals(cpair.nrml, TOL)) return false;
    } else {
      final Vector3d del = new Vector3d();
      del.sub(cpair.pnt2, cpair.pnt1);

      if (Math.abs(check.dist - cpair.dist) > TOL || Math.abs(del.length() - check.dist) > TOL)
        return false;
      if (!cpair.pnt1.epsilonEquals(check.pnt1, TOL)) return false;
      if (!cpair.pnt2.epsilonEquals(check.pnt2, TOL)) return false;
      if (cpair.feat1.getType() != check.feat1.getType()
          || cpair.feat2.getType() != check.feat2.getType()) return false;
    }

    if (check.dist > 0) if (!cpair.nrml.epsilonEquals(check.nrml, TOL)) return false;
    return true;
  }
Ejemplo n.º 2
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);
    }
  }
Ejemplo n.º 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;
  }
Ejemplo n.º 4
0
  /**
   * Returns the normal of the plane closest to the given origin.
   *
   * @param pointOnAABB A point on the AABB
   * @param origin The origin
   * @param testX True if the x-axis should be tested
   * @param testY True if the y-axis should be tested
   * @param testZ True if the z-axis should be tested
   * @return The normal
   */
  public Vector3d normalForPlaneClosestToOrigin(
      Vector3d pointOnAABB, Vector3d origin, boolean testX, boolean testY, boolean testZ) {
    ArrayList<Vector3d> normals = new ArrayList<Vector3d>();

    if (pointOnAABB.z == minZ() && testZ) normals.add(new Vector3d(0, 0, -1));
    if (pointOnAABB.z == maxZ() && testZ) normals.add(new Vector3d(0, 0, 1));
    if (pointOnAABB.x == minX() && testX) normals.add(new Vector3d(-1, 0, 0));
    if (pointOnAABB.x == maxX() && testX) normals.add(new Vector3d(1, 0, 0));
    if (pointOnAABB.y == minY() && testY) normals.add(new Vector3d(0, -1, 0));
    if (pointOnAABB.y == maxY() && testY) normals.add(new Vector3d(0, 1, 0));

    double minDistance = Double.MAX_VALUE;
    Vector3d closestNormal = new Vector3d();

    for (int i = 0; i < normals.size(); i++) {
      Vector3d n = normals.get(i);

      Vector3d diff = new Vector3d(centerPointForNormal(n));
      diff.sub(origin);

      double distance = diff.length();

      if (distance < minDistance) {
        minDistance = distance;
        closestNormal = n;
      }
    }

    return closestNormal;
  }
Ejemplo n.º 5
0
  //
  // places triangle into canonical position
  //
  private void add(double len01, Vector3d v0, Vector3d v1, Vector3d v2) {

    // area of triangle

    p1.set(v1);
    p2.set(v2);

    p1.sub(v0);
    p2.sub(v0);

    normal.cross(p1, p2);

    // tri area
    double triWidth = len01;
    double triArea2 = normal.length();
    double triHeight = triArea2 / triWidth;

    double v1x = triWidth;
    double v2y = triHeight; // y- coordinate of 2D triangle 2
    double v2x = p1.dot(p2) / len01;

    m_ctri.add(new CanonicalTri(v1x / m_pixelSize, v2x / m_pixelSize, v2y / m_pixelSize, m_gap));

    //
    // all triangles are oriented canonically: v0,v1 is the longest side
    //
    m_tri.add(new Vector3d(v0));
    m_tri.add(new Vector3d(v1));
    m_tri.add(new Vector3d(v2));
    m_triCount++;
  }
 // 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;
 }
 // gets a point not on vector a...b; this can be used to define a plan or cross products
 private static Vector3d getNonColinearVector(Vector3d ab) {
   Vector3d cr = new Vector3d();
   cr.cross(ab, XV);
   if (cr.length() > 0.00001) {
     return XV;
   } else {
     return YV;
   }
 }
Ejemplo n.º 8
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;
    }
Ejemplo n.º 9
0
  public Vector3d getReaction(Vector3d position, Vector3d velocity, Vector3d action, double mass) {

    Vector3d relativePosition = new Vector3d(position);
    relativePosition.sub(point);
    double x = relativePosition.length();
    Vector3d reaction = new Vector3d(relativePosition);
    if (x > Teal.DoubleZero) {
      reaction.scale(-k1);
    }

    double v = velocity.length();
    Vector3d damping = new Vector3d(velocity);
    if (v > Teal.DoubleZero) {
      // damping.scale(1./v);
      damping.scale(-k2 * (1.0 + 0.0 / v) / (1. + (x / p) * (x / p)));
      reaction.add(damping);
    }
    lastReaction.set(reaction);
    return reaction;
  }
Ejemplo n.º 10
0
  public Transform3D getTransform() {
    Transform3D transform = new Transform3D();
    Point3d target = new Point3d();

    target.add(direction, position);

    Point3d zoomedPosition = new Point3d();
    zoomedPosition.scaleAdd(-zoom / direction.length(), direction, target);

    transform.lookAt(zoomedPosition, target, up);
    transform.invert();
    return transform;
  }
Ejemplo n.º 11
0
  private boolean projectToPlane(Vector3d projVec, Vector3d planeVec) {
    double dis = planeVec.dot(projVec);
    projVec.x = projVec.x - planeVec.x * dis;
    projVec.y = projVec.y - planeVec.y * dis;
    projVec.z = projVec.z - planeVec.z * dis;

    double length = projVec.length();
    if (length < EPSILON) { // projVec is parallel to planeVec
      return false;
    }
    projVec.scale(1 / length);
    return true;
  }
Ejemplo n.º 12
0
 protected void computeCircle() {
   Vector3d center = new Vector3d((Vector3d) get(0));
   Vector3d p2 = new Vector3d((Vector3d) get(1));
   p2.sub(center);
   double radius = p2.length();
   removeAllElements();
   double angle = 0;
   double incAngle = 2 * Math.PI / sides;
   for (int i = 0; i < sides; i++) {
     super.add(
         new Vector3d(
             center.x + radius * Math.cos(angle), center.y + radius * Math.sin(angle), 0));
     angle += incAngle;
   }
   super.add(new Vector3d(center.x + radius, center.y, 0));
 }
 /**
  * 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;
 }
Ejemplo n.º 14
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);
  }
  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);
  }
  /**
   * 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;
  }
Ejemplo n.º 17
0
  @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");
    }
  }
Ejemplo n.º 18
0
 public static void setRotation(Quat4d q, Vector3d axis, double angle) {
   double d = axis.length();
   assert (d != 0f);
   double s = (double) Math.sin(angle * 0.5f) / d;
   q.set(axis.x * s, axis.y * s, axis.z * s, (double) Math.cos(angle * 0.5f));
 }
Ejemplo n.º 19
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);
    }
  }