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 }