Beispiel #1
0
def computeWorldCorrection():
  x0, y0, z0 = 43324.0, 41836.0, 0.0
  x1, y1, z1 = 46780.0, 45532.0, 22950.0
  x2, y2, z2 = x0, y0, z1
  
  # A point in the first section that should be directly on top of the x0,y0,z0
  x3, y3, z3 = 48164, 23372, 0

  trans = Transform3D()
  trans.setTranslation(Vector3d(-x0, -y0, 0))

  rot = Transform3D()

  p0 = Vector3d(0, 0, z1)
  p1 = Vector3d(x1 - x0, y1 - y0, z1 - z0)
  pc = Vector3d()
  pc.cross(p1, p0)

  angle = Math.atan(Math.sqrt(Math.pow(x1 - x0, 2) + Math.pow(y1 - y0, 2)) / (z1 - z0))
  print angle * 180 / Math.PI

  rot.setRotation(AxisAngle4d(pc, angle))

  t = Transform3D()
  t.mul(rot)
  t.mul(trans)

  # Transform the third point
  p3 = Point3d(x3, y3, z3)
  t.transform(p3)
  print "A p3:", p3

  # test
  p0 = Point3d(x0, y0, z0)
  t.transform(p0)
  print "A: p0", p0

  p1 = Point3d(x1, y1, z1)
  t.transform(p1)
  print "A: p1", p1

  # Compute rotation of third point around Z axis
  rotZ = Transform3D()
  angleZ = Math.atan((p3.x - 0) / (p3.y - 0))
  print "angleZ", angleZ
  rotZ.setRotation(AxisAngle4d(Vector3d(0,0,1), angleZ))

  t.mul(rotZ, t)
  
  
  # test
  p0 = Point3d(x0, y0, z0)
  t.transform(p0)
  print p0

  p1 = Point3d(x1, y1, z1)
  t.transform(p1)
  print p1

  p3 = Point3d(x3, y3, z3)
  t.transform(p3)
  print p3

  return t