Vector3f.java
Go to the documentation of this file.
00001 package edu.tum.cs.ias.knowrob.util.datastructures;
00002 
00003 import javax.vecmath.Matrix4f;
00004 
00005 public class Vector3f extends javax.vecmath.Vector3f {
00006         private static final long serialVersionUID = 1L;
00007 
00008         public Vector3f() {
00009                 super();
00010         }
00011         
00012         public Vector3f(float x, float y, float z) {
00013                 super(x,y,z);
00014         }
00015         
00016         public Vector3f(javax.vecmath.Vector3f v) {
00017                 super(v);
00018         }
00019         
00020         public double distance(Vector3f other) {
00021                 float xd = x-other.x;
00022                 float yd = y-other.y;
00023                 float zd = z-other.z;
00024                 return Math.sqrt(xd*xd+yd*yd+zd*zd);
00025         }
00026         
00027         public void rotate(double theta, javax.vecmath.Vector3f axis) {
00028            Vector3f p = this;
00029            Vector3f q = new Vector3f(0.0f,0.0f,0.0f);
00030            double costheta, sintheta;
00031 
00032            Vector3f r = new Vector3f(axis);
00033            r.normalize();
00034            
00035            costheta = Math.cos(theta);
00036            sintheta = Math.sin(theta);
00037 
00038            q.x += (costheta + (1 - costheta) * r.x * r.x) * p.x;
00039            q.x += ((1 - costheta) * r.x * r.y - r.z * sintheta) * p.y;
00040            q.x += ((1 - costheta) * r.x * r.z + r.y * sintheta) * p.z;
00041 
00042            q.y += ((1 - costheta) * r.x * r.y + r.z * sintheta) * p.x;
00043            q.y += (costheta + (1 - costheta) * r.y * r.y) * p.y;
00044            q.y += ((1 - costheta) * r.y * r.z - r.x * sintheta) * p.z;
00045 
00046            q.z += ((1 - costheta) * r.x * r.z - r.y * sintheta) * p.x;
00047            q.z += ((1 - costheta) * r.y * r.z + r.x * sintheta) * p.y;
00048            q.z += (costheta + (1 - costheta) * r.z * r.z) * p.z;
00049 
00050            this.x = q.x;
00051            this.y = q.y;
00052            this.z = q.z;
00053         }
00054         
00055         public static Matrix4f getRotationMatrix(double theta, javax.vecmath.Vector3f axis) {
00056                 Matrix4f ret = new Matrix4f();
00057                 float costheta, sintheta;
00058                 
00059                 Vector3f r = new Vector3f(axis);
00060                 r.normalize();
00061 
00062                 costheta = (float)Math.cos(theta);
00063                 sintheta = (float)Math.sin(theta);
00064 
00065                 ret.m00 = (costheta + (1 - costheta) * r.x * r.x);
00066                 ret.m10 = ((1 - costheta) * r.x * r.y - r.z * sintheta);
00067                 ret.m20 = ((1 - costheta) * r.x * r.z + r.y * sintheta);
00068                 ret.m30 = 0;
00069 
00070                 ret.m01 = ((1 - costheta) * r.x * r.y + r.z * sintheta);
00071                 ret.m11 = (costheta + (1 - costheta) * r.y * r.y);
00072                 ret.m21 = ((1 - costheta) * r.y * r.z - r.x * sintheta);
00073                 ret.m31 = 0;
00074 
00075                 ret.m02 = ((1 - costheta) * r.x * r.z - r.y * sintheta);
00076                 ret.m12 = ((1 - costheta) * r.y * r.z + r.x * sintheta);
00077                 ret.m22 = (costheta + (1 - costheta) * r.z * r.z);
00078                 ret.m32 = 0;
00079                 
00080                 ret.m03 = 0;
00081                 ret.m13 = 0;
00082                 ret.m23 = 0;
00083                 ret.m33 = 1;
00084 
00085                 return ret;
00086         }
00087 }


mod_vis
Author(s): Moritz Tenorth, Jakob Engel
autogenerated on Mon Oct 6 2014 01:30:00