00001
00002
00004
00005 #ifndef GEOPRIMITIVES_GEOPRIMITIVESHELPERS_H
00006 #define GEOPRIMITIVES_GEOPRIMITIVESHELPERS_H
00007
00008 #include "GeoPrimitives/GeoPrimitives.h"
00009 #include "GeoPrimitives/GeoPrimitivesCompare.h"
00010 #include "CxxUtils/sincos.h"
00011
00012 #include "cmath"
00013
00014 #include <vector>
00015 #include <set>
00016 #include <iostream>
00017
00018
00026 namespace Amg {
00027
00028
00029
00030 typedef std::set<Amg::Vector3D, Vector3DComparer> SetVector3D;
00031 typedef std::set< std::vector< Amg::Vector3D>, VectorVector3DComparer> SetVectorVector3D;
00032
00033
00034
00036 inline double angle(const Amg::Vector3D& v1, const Amg::Vector3D& v2) {
00037 double dp = v1.dot(v2);
00038 dp /= v1.mag() * v2.mag();
00039 if (dp > 1)
00040 dp = 1;
00041 if (dp < -1)
00042 dp = -1;
00043 return acos(dp);
00044 }
00045
00046
00048 inline float distance2(const Amg::Vector3D& p1, const Amg::Vector3D& p2) {
00049 float dx = p2.x()-p1.x(), dy = p2.y()-p1.y(), dz = p2.z()-p1.z();
00050 return dx*dx + dy*dy + dz*dz;
00051 }
00052
00054 inline float distance(const Amg::Vector3D& p1, const Amg::Vector3D& p2) {
00055 return std::sqrt( distance2(p1, p2) );
00056 }
00057
00058
00059
00060
00062 inline void setPhi(Amg::Vector3D& v, double phi) {
00063 double xy = v.perp();
00064 CxxUtils::sincos sc(phi);
00065 v[0] = xy * sc.cs;
00066 v[1] = xy * sc.sn;
00067 }
00068
00070 inline void setThetaPhi(Amg::Vector3D& v, double theta, double phi) {
00071 double mag = v.mag();
00072 CxxUtils::sincos sc(phi);
00073 CxxUtils::sincos sct(theta);
00074 v[0] = mag * sct.sn * sc.cs;
00075 v[1] = mag * sct.sn * sc.sn;
00076 v[2] = mag * sct.cs;
00077 }
00078
00080 inline void setRThetaPhi(Amg::Vector3D& v, double r, double theta, double phi) {
00081 CxxUtils::sincos sc(phi);
00082 CxxUtils::sincos sct(theta);
00083 v[0] = r * sct.sn * sc.cs;
00084 v[1] = r * sct.sn * sc.sn;
00085 v[2] = r * sct.cs;
00086 }
00087
00089 inline void setTheta(Amg::Vector3D& v, double theta) {
00090 setThetaPhi(v, theta, v.phi());
00091 }
00092
00094 inline void setPerp(Amg::Vector3D& v, double perp) {
00095 double p = v.perp();
00096 if (p != 0.0) {
00097 double scale = perp / p;
00098 v[0] *= scale;
00099 v[1] *= scale;
00100 }
00101 }
00102
00104 inline void setMag(Amg::Vector3D& v, double mag) {
00105 double p = v.mag();
00106 if (p != 0.0) {
00107 double scale = mag / p;
00108 v[0] *= scale;
00109 v[1] *= scale;
00110 v[2] *= scale;
00111 }
00112 }
00113 inline double deltaPhi(const Amg::Vector3D& v1, const Amg::Vector3D& v2) {
00114 double dphi = v2.phi() - v1.phi();
00115 if (dphi > M_PI) {
00116 dphi -= M_PI*2;
00117 } else if (dphi <= -M_PI) {
00118 dphi += M_PI*2;
00119 }
00120 return dphi;
00121 }
00122 inline double deltaR(const Amg::Vector3D& v1, const Amg::Vector3D& v2){
00123 double a = v1.eta() - v2.eta();
00124 double b = deltaPhi(v1,v2);
00125 return sqrt(a*a + b*b);
00126 }
00127
00128
00129
00130
00131
00132
00136 inline void setVector3DCartesian(Amg::Vector3D& v1, double x1, double y1, double z1) { v1[0] = x1; v1[1] = y1; v1[2] = z1; }
00140 inline double mag2Vector3D(const Amg::Vector3D& v1) { return v1.x()*v1.x() + v1.y()*v1.y() + v1.z()*v1.z(); }
00144 inline double magVector3D(const Amg::Vector3D& v1) { return std::sqrt(mag2Vector3D(v1)); }
00148 inline double rVector3D(const Amg::Vector3D& v1) { return magVector3D(v1); }
00149
00156 inline Amg::Vector3D transform( Amg::Vector3D& v, Amg::Transform3D& tr ) {
00157 Amg::Vector3D vect;
00158 double vx = v.x(), vy = v.y(), vz = v.z();
00159 setVector3DCartesian( vect,
00160 tr(0,0)*vx + tr(0,1)*vy + tr(0,2)*vz + tr(0,3),
00161 tr(1,0)*vx + tr(1,1)*vy + tr(1,2)*vz + tr(1,3),
00162 tr(2,0)*vx + tr(2,1)*vy + tr(2,2)*vz + tr(2,3));
00163 return vect;
00164 }
00165
00166
00167
00168
00169
00170
00171
00172 inline Amg::Transform3D getTransformFromRotTransl(Amg::RotationMatrix3D rot, Amg::Vector3D transl_vec )
00173 {
00174 Amg::Transform3D trans = Amg::Transform3D::Identity();
00175 trans = trans * rot;
00176 trans.translation() = transl_vec;
00177 return trans;
00178 }
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192 inline void getAngleAxisFromRotation(Amg::RotationMatrix3D& rotation, double& rotationAngle, Amg::Vector3D& rotationAxis)
00193 {
00194 rotationAngle = 0.;
00195
00196 double xx = rotation(0,0);
00197 double yy = rotation(1,1);
00198 double zz = rotation(2,2);
00199
00200 double cosa = 0.5 * (xx + yy + zz - 1);
00201 double cosa1 = 1 - cosa;
00202
00203 if (cosa1 <= 0) {
00204 rotationAngle = 0;
00205 rotationAxis = Amg::Vector3D(0,0,1);
00206 }
00207 else{
00208 double x=0, y=0, z=0;
00209 if (xx > cosa) x = sqrt((xx-cosa)/cosa1);
00210 if (yy > cosa) y = sqrt((yy-cosa)/cosa1);
00211 if (zz > cosa) z = sqrt((zz-cosa)/cosa1);
00212 if (rotation(2,1) < rotation(1,2)) x = -x;
00213 if (rotation(0,2) < rotation(2,0)) y = -y;
00214 if (rotation(1,0) < rotation(0,1)) z = -z;
00215 rotationAngle = (cosa < -1.) ? acos(-1.) : acos(cosa);
00216 rotationAxis = Amg::Vector3D(x,y,z);
00217 }
00218
00219 return;
00220 }
00221
00225 inline Amg::Vector3D getTranslationVectorFromTransform(const Amg::Transform3D& tr) {
00226 return Amg::Vector3D(tr(0,3),tr(1,3),tr(2,3));
00227 }
00228
00229
00230
00237 inline Amg::Rotation3D getRotation3DfromAngleAxis(double angle, Amg::Vector3D& axis)
00238 {
00239 AngleAxis3D t;
00240 t = Eigen::AngleAxis<double>(angle,axis);
00241
00242 Amg::Rotation3D rot;
00243 rot = t;
00244
00245 return rot;
00246 }
00247
00248
00252 inline Amg::Transform3D getRotateX3D(double angle) {
00253 Amg::Transform3D transf;
00254 Amg::AngleAxis3D angleaxis(angle, Amg::Vector3D(1.,0.,0.));
00255 transf = angleaxis;
00256 return transf;
00257 }
00261 inline Amg::Transform3D getRotateY3D(double angle) {
00262 Amg::Transform3D transf;
00263 Amg::AngleAxis3D angleaxis(angle, Amg::Vector3D(0.,1.,0.));
00264 transf = angleaxis;
00265 return transf;
00266 }
00270 inline Amg::Transform3D getRotateZ3D(double angle) {
00271 Amg::Transform3D transf;
00272 Amg::AngleAxis3D angleaxis(angle, Amg::Vector3D(0.,0.,1.));
00273 transf = angleaxis;
00274 return transf;
00275 }
00276
00277
00278
00279 }
00280
00281 #endif