Public Pastes
SHARE
TWEET

Untitled

a guest Oct 27th, 2015 104 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import java.lang.Math;
  2.  
  3. public class Test{
  4.  
  5.         public static class Vector3{
  6.                 public double x, y, z;
  7.  
  8.                 public void normalize(){
  9.                         double mag = Math.sqrt(x*x+y*y+z*z);
  10.                         x = x/mag;
  11.                         y = y/mag;
  12.                         z = z/mag;
  13.                 }
  14.  
  15.                 public void printVector(){
  16.                         System.out.println((Double.toString(x) + " " + Double.toString(y) + " " + Double.toString(z)));
  17.                 }
  18.         }
  19.  
  20.         public static class Quaternion{
  21.                 public double qi, qj, qk, qr;
  22.  
  23.                 public void vectorToQuaternion(double angle, Vector3 in){
  24.                         angle = angle*3.141592654/180;
  25.                         qr = Math.cos(angle/2);
  26.                         qi = in.x*Math.sin(angle/2);
  27.                         qj = in.y*Math.sin(angle/2);
  28.                         qk = in.z*Math.sin(angle/2);
  29.                 }
  30.  
  31.                 public Vector3 rotateVector(Vector3 in){
  32.                         Vector3 out = new Vector3();
  33.  
  34.                         double M1, M2, M3,
  35.                                         M4, M5, M6,
  36.                                         M7, M8, M9;
  37.  
  38.                         M1 = 1 - 2*qj*qj - 2*qk*qk;
  39.                         M2 = 2*(qi*qj - qk*qr);
  40.                         M3 = 2*(qi*qk + qj*qr);
  41.                         M4 = 2*(qi*qj + qk*qr);
  42.                         M5 = 1 - 2*qi*qi - 2*qk*qk;
  43.                         M6 = 2*(qj*qk - qi*qr);
  44.                         M7 = 2*(qi*qk - qj*qr);
  45.                         M8 = 2*(qj*qk + qi*qr);
  46.                         M9 = 1 - 2*qi*qi - 2*qj*qj;
  47.  
  48.                         out.x = M1*in.x + M2*in.y + M3*in.z;
  49.                         out.y = M4*in.x + M5*in.y + M6*in.z;
  50.                         out.z = M7*in.x + M8*in.y + M9*in.z;
  51.  
  52.                         return out;
  53.                 }
  54.  
  55.                 public void printQuaternion(){
  56.                        
  57.                 }
  58.         }
  59.  
  60.  
  61.         public static void main(String args[]) {
  62.                 Vector3 forward, other;
  63.                 Quaternion myQ;
  64.  
  65.                 forward = new Vector3();
  66.                 myQ = new Quaternion();
  67.  
  68.                 other = new Vector3();
  69.  
  70.                 // Set the vector you rotate about:
  71.                 other.x = 0.0;
  72.                 other.y = 0.0;
  73.                 other.z = 1.0;
  74.  
  75.                 //  Set the vector you are trying to rotate:
  76.                 forward.x = 1.0;
  77.                 forward.y = 0.0;
  78.                 forward.z = 0.0;
  79.  
  80.                 System.out.println("Initially:");
  81.                 forward.printVector();
  82.  
  83.                 System.out.println("\nFinally: ");
  84.  
  85.                 myQ.vectorToQuaternion(30.0, other);
  86.                
  87.                 other = myQ.rotateVector(forward);
  88.  
  89.                 other.printVector();
  90.  
  91.  
  92.         }
  93.  
  94. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top