diff --git a/Core/Algorithms/src/MultiLayerRoughnessDWBASimulation.cpp b/Core/Algorithms/src/MultiLayerRoughnessDWBASimulation.cpp
index 0af237ac0003d1f56c8b50b0e699ca78aa9d2209..22ba9f691d4941aca18f18036df7f8145f0f05b3 100644
--- a/Core/Algorithms/src/MultiLayerRoughnessDWBASimulation.cpp
+++ b/Core/Algorithms/src/MultiLayerRoughnessDWBASimulation.cpp
@@ -94,7 +94,7 @@ double MultiLayerRoughnessDWBASimulation::evaluate(const cvector_t &k_i, const c
         }
     }
 
-    return (autocorr+crosscorr.real())*k_i.mag2().real()/16./M_PI;
+    return (autocorr+crosscorr.real())*k_i.mag2()/16./M_PI;
 }
 
 complex_t MultiLayerRoughnessDWBASimulation::get_refractive_term(size_t ilayer) const
diff --git a/Core/Core.pro b/Core/Core.pro
index 62cd6aaeee6c48e84dfea271daf43c493dab82de..29fb1e56ded09fcf8af13c0e90ccfd727743e5f1 100644
--- a/Core/Core.pro
+++ b/Core/Core.pro
@@ -12,6 +12,40 @@ CONFIG  += BORNAGAIN_PYTHON
 # source and headers
 # -----------------------------------------------------------------------------
 SOURCES += \
+    Geometry/src/BasicVector3D.cpp \
+    Geometry/src/Normal3D.cpp \
+    Geometry/src/Plane3D.cpp \
+    Geometry/src/Point3D.cpp \
+    Geometry/src/Transform3D.cpp \
+    Geometry/src/Vector3D.cpp \
+    \
+    Tools/src/AxisBin.cpp \
+    Tools/src/AxisDouble.cpp \
+    Tools/src/Convolve.cpp \
+    Tools/src/CoreOptionsDescription.cpp \
+    Tools/src/DoubleToComplexInterpolatingFunction.cpp \
+    Tools/src/Exceptions.cpp \
+    Tools/src/IFactory.cpp \
+    Tools/src/IObserver.cpp \
+    Tools/src/IParameterized.cpp \
+    Tools/src/ISingleton.cpp \
+    Tools/src/MathFunctions.cpp \
+    Tools/src/MessageSvc.cpp \
+    Tools/src/OutputData.cpp \
+    Tools/src/OutputDataFunctions.cpp \
+    Tools/src/OutputDataIOFactory.cpp \
+    Tools/src/OutputDataReadStrategy.cpp \
+    Tools/src/OutputDataReader.cpp \
+    Tools/src/OutputDataWriteStrategy.cpp \
+    Tools/src/OutputDataWriter.cpp \
+    Tools/src/ParameterPool.cpp \
+    Tools/src/ProgramOptions.cpp \
+    Tools/src/RealParameterWrapper.cpp \
+    Tools/src/StochasticGaussian.cpp \
+    Tools/src/StochasticSampledParameter.cpp \
+    Tools/src/Types.cpp \
+    Tools/src/Utils.cpp \
+    \
     Algorithms/src/Beam.cpp \
     Algorithms/src/ChiSquaredFrequency.cpp \
     Algorithms/src/ChiSquaredModule.cpp \
@@ -40,18 +74,6 @@ SOURCES += \
     Algorithms/src/ResolutionFunction2DSimple.cpp \
     Algorithms/src/SizeSpacingCorrelationApproximationStrategy.cpp \
     \
-    Fitting/src/FitObject.cpp \
-    Fitting/src/FitParameter.cpp \
-    Fitting/src/FitParameterLinked.cpp \
-    Fitting/src/FitSuite.cpp \
-    Fitting/src/FitSuiteFunctions.cpp \
-    Fitting/src/FitSuiteObjects.cpp \
-    Fitting/src/FitSuiteParameters.cpp \
-    Fitting/src/FitSuiteStrategies.cpp \
-    Fitting/src/IFitSuiteStrategy.cpp \
-    Fitting/src/MinimizerScan.cpp \
-    Fitting/src/MinimizerTest.cpp \
-    \
     FormFactors/src/FormFactorBox.cpp \
     FormFactors/src/FormFactorCone.cpp \
     FormFactors/src/FormFactorCrystal.cpp \
@@ -73,13 +95,6 @@ SOURCES += \
     FormFactors/src/FormFactorWeighted.cpp \
     FormFactors/src/IFormFactorBorn.cpp \
     \
-    Geometry/src/BasicVector3D.cpp \
-    Geometry/src/Normal3D.cpp \
-    Geometry/src/Plane3D.cpp \
-    Geometry/src/Point3D.cpp \
-    Geometry/src/Transform3D.cpp \
-    Geometry/src/Vector3D.cpp \
-    \
     Samples/src/Crystal.cpp \
     Samples/src/DiffuseParticleInfo.cpp \
     Samples/src/HomogeneousMaterial.cpp \
@@ -106,34 +121,74 @@ SOURCES += \
     Samples/src/ParticleInfo.cpp \
     Samples/src/PositionParticleInfo.cpp \
     \
-    Tools/src/AxisBin.cpp \
-    Tools/src/AxisDouble.cpp \
-    Tools/src/Convolve.cpp \
-    Tools/src/CoreOptionsDescription.cpp \
-    Tools/src/DoubleToComplexInterpolatingFunction.cpp \
-    Tools/src/Exceptions.cpp \
-    Tools/src/IFactory.cpp \
-    Tools/src/IObserver.cpp \
-    Tools/src/IParameterized.cpp \
-    Tools/src/ISingleton.cpp \
-    Tools/src/MathFunctions.cpp \
-    Tools/src/MessageSvc.cpp \
-    Tools/src/OutputData.cpp \
-    Tools/src/OutputDataFunctions.cpp \
-    Tools/src/OutputDataIOFactory.cpp \
-    Tools/src/OutputDataReadStrategy.cpp \
-    Tools/src/OutputDataReader.cpp \
-    Tools/src/OutputDataWriteStrategy.cpp \
-    Tools/src/OutputDataWriter.cpp \
-    Tools/src/ParameterPool.cpp \
-    Tools/src/ProgramOptions.cpp \
-    Tools/src/RealParameterWrapper.cpp \
-    Tools/src/StochasticGaussian.cpp \
-    Tools/src/StochasticSampledParameter.cpp \
-    Tools/src/Types.cpp \
-    Tools/src/Utils.cpp
+    Fitting/src/FitObject.cpp \
+    Fitting/src/FitParameter.cpp \
+    Fitting/src/FitParameterLinked.cpp \
+    Fitting/src/FitSuite.cpp \
+    Fitting/src/FitSuiteFunctions.cpp \
+    Fitting/src/FitSuiteObjects.cpp \
+    Fitting/src/FitSuiteParameters.cpp \
+    Fitting/src/FitSuiteStrategies.cpp \
+    Fitting/src/IFitSuiteStrategy.cpp \
+    Fitting/src/MinimizerScan.cpp \
+    Fitting/src/MinimizerTest.cpp
 
 HEADERS += \
+    Geometry/inc/BasicVector3D.h \
+    Geometry/inc/Normal3D.h \
+    Geometry/inc/Plane3D.h \
+    Geometry/inc/Point3D.h \
+    Geometry/inc/Transform3D.h \
+    Geometry/inc/Vector3D.h \
+    \
+    Tools/inc/AxisBin.h \
+    Tools/inc/AxisDouble.h \
+    Tools/inc/Bin.h \
+    Tools/inc/Convolve.h \
+    Tools/inc/Coordinate3D.h \
+    Tools/inc/CoreOptionsDescription.h \
+    Tools/inc/DoubleToComplexInterpolatingFunction.h \
+    Tools/inc/DoubleToComplexMap.h \
+    Tools/inc/Exceptions.h \
+    Tools/inc/FastVector.h \
+    Tools/inc/IAxis.h \
+    Tools/inc/IChangeable.h \
+    Tools/inc/ICloneable.h \
+    Tools/inc/IDoubleToComplexFunction.h \
+    Tools/inc/IFactory.h \
+    Tools/inc/INamed.h \
+    Tools/inc/IObserver.h \
+    Tools/inc/IParameterized.h \
+    Tools/inc/ISingleton.h \
+    Tools/inc/IStochasticParameter.h \
+    Tools/inc/LLData.h \
+    Tools/inc/Macros.h \
+    Tools/inc/MathFunctions.h \
+    Tools/inc/MemberComplexFunctionIntegrator.h \
+    Tools/inc/MemberFunctionIntegrator.h \
+    Tools/inc/MessageSvc.h \
+    Tools/inc/Numeric.h \
+    Tools/inc/OutputData.h \
+    Tools/inc/OutputDataFunctions.h \
+    Tools/inc/OutputDataIOFactory.h \
+    Tools/inc/OutputDataIterator.h \
+    Tools/inc/OutputDataReadStrategy.h \
+    Tools/inc/OutputDataReader.h \
+    Tools/inc/OutputDataWriteStrategy.h \
+    Tools/inc/OutputDataWriter.h \
+    Tools/inc/ParameterPool.h \
+    Tools/inc/ProgramOptions.h \
+    Tools/inc/RealParameterWrapper.h \
+    Tools/inc/SafePointerVector.h \
+    Tools/inc/StochasticDiracDelta.h \
+    Tools/inc/StochasticDoubleGate.h \
+    Tools/inc/StochasticGaussian.h \
+    Tools/inc/StochasticSampledParameter.h \
+    Tools/inc/TRange.h \
+    Tools/inc/Types.h \
+    Tools/inc/Units.h \
+    Tools/inc/Utils.h \
+    \
     Algorithms/inc/Beam.h \
     Algorithms/inc/ChiSquaredFrequency.h \
     Algorithms/inc/ChiSquaredModule.h \
@@ -173,21 +228,6 @@ HEADERS += \
     Algorithms/inc/StrategyBuilder.h \
     Algorithms/inc/ThreadInfo.h \
     \
-    Fitting/inc/AttFitting.h \
-    Fitting/inc/AttLimits.h \
-    Fitting/inc/FitObject.h \
-    Fitting/inc/FitParameter.h \
-    Fitting/inc/FitParameterLinked.h \
-    Fitting/inc/FitSuite.h \
-    Fitting/inc/FitSuiteFunctions.h \
-    Fitting/inc/FitSuiteObjects.h \
-    Fitting/inc/FitSuiteParameters.h \
-    Fitting/inc/FitSuiteStrategies.h \
-    Fitting/inc/IFitSuiteStrategy.h \
-    Fitting/inc/IMinimizer.h \
-    Fitting/inc/MinimizerScan.h \
-    Fitting/inc/MinimizerTest.h \
-    \
     FormFactors/inc/FormFactorBox.h \
     FormFactors/inc/FormFactorCone.h \
     FormFactors/inc/FormFactorCrystal.h \
@@ -219,13 +259,6 @@ HEADERS += \
     FormFactors/inc/IFormFactorBornSeparable.h \
     FormFactors/inc/IFormFactorDecorator.h \
     \
-    Geometry/inc/BasicVector3D.h \
-    Geometry/inc/Normal3D.h \
-    Geometry/inc/Plane3D.h \
-    Geometry/inc/Point3D.h \
-    Geometry/inc/Transform3D.h \
-    Geometry/inc/Vector3D.h \
-    \
     Samples/inc/Crystal.h \
     Samples/inc/DiffuseParticleInfo.h \
     Samples/inc/HomogeneousMaterial.h \
@@ -262,53 +295,20 @@ HEADERS += \
     Samples/inc/PositionParticleInfo.h \
     Samples/inc/Samples.h \
     \
-    Tools/inc/AxisBin.h \
-    Tools/inc/AxisDouble.h \
-    Tools/inc/Bin.h \
-    Tools/inc/Convolve.h \
-    Tools/inc/Coordinate3D.h \
-    Tools/inc/CoreOptionsDescription.h \
-    Tools/inc/DoubleToComplexInterpolatingFunction.h \
-    Tools/inc/DoubleToComplexMap.h \
-    Tools/inc/Exceptions.h \
-    Tools/inc/FastVector.h \
-    Tools/inc/IAxis.h \
-    Tools/inc/IChangeable.h \
-    Tools/inc/ICloneable.h \
-    Tools/inc/IDoubleToComplexFunction.h \
-    Tools/inc/IFactory.h \
-    Tools/inc/INamed.h \
-    Tools/inc/IObserver.h \
-    Tools/inc/IParameterized.h \
-    Tools/inc/ISingleton.h \
-    Tools/inc/IStochasticParameter.h \
-    Tools/inc/LLData.h \
-    Tools/inc/Macros.h \
-    Tools/inc/MathFunctions.h \
-    Tools/inc/MemberComplexFunctionIntegrator.h \
-    Tools/inc/MemberFunctionIntegrator.h \
-    Tools/inc/MessageSvc.h \
-    Tools/inc/Numeric.h \
-    Tools/inc/OutputData.h \
-    Tools/inc/OutputDataFunctions.h \
-    Tools/inc/OutputDataIOFactory.h \
-    Tools/inc/OutputDataIterator.h \
-    Tools/inc/OutputDataReadStrategy.h \
-    Tools/inc/OutputDataReader.h \
-    Tools/inc/OutputDataWriteStrategy.h \
-    Tools/inc/OutputDataWriter.h \
-    Tools/inc/ParameterPool.h \
-    Tools/inc/ProgramOptions.h \
-    Tools/inc/RealParameterWrapper.h \
-    Tools/inc/SafePointerVector.h \
-    Tools/inc/StochasticDiracDelta.h \
-    Tools/inc/StochasticDoubleGate.h \
-    Tools/inc/StochasticGaussian.h \
-    Tools/inc/StochasticSampledParameter.h \
-    Tools/inc/TRange.h \
-    Tools/inc/Types.h \
-    Tools/inc/Units.h \
-    Tools/inc/Utils.h
+    Fitting/inc/AttFitting.h \
+    Fitting/inc/AttLimits.h \
+    Fitting/inc/FitObject.h \
+    Fitting/inc/FitParameter.h \
+    Fitting/inc/FitParameterLinked.h \
+    Fitting/inc/FitSuite.h \
+    Fitting/inc/FitSuiteFunctions.h \
+    Fitting/inc/FitSuiteObjects.h \
+    Fitting/inc/FitSuiteParameters.h \
+    Fitting/inc/FitSuiteStrategies.h \
+    Fitting/inc/IFitSuiteStrategy.h \
+    Fitting/inc/IMinimizer.h \
+    Fitting/inc/MinimizerScan.h \
+    Fitting/inc/MinimizerTest.h
 
 contains(CONFIG, BORNAGAIN_PYTHON) {
    include($$PWD/python_module.pri)
diff --git a/Core/Geometry/inc/BasicVector3D.h b/Core/Geometry/inc/BasicVector3D.h
index 1921c30888a28ed968246fd20499fe1c86df25a2..1fa97496eff920ac405075ee99ec8cd6312eb2c1 100755
--- a/Core/Geometry/inc/BasicVector3D.h
+++ b/Core/Geometry/inc/BasicVector3D.h
@@ -36,470 +36,422 @@ namespace Geometry {
 
 class Transform3D;
 
-    //! Base class for Point3D<T>, Vector3D<T> and Normal3D<T>.
+//! Base class for Point3D<T>, Vector3D<T> and Normal3D<T>.
 
-    //! It defines only common functionality for those classes and
-    //! should not be used as separate class.
-    //!
-    //! @author Evgeni Chernyaev 1996-2003
-    //!
-    template<class T> class BasicVector3D {
-      protected:
-        T v_[3];
-        
-      public:
-        //! Default constructor.
-        //! It is protected - this class should not be instantiated directly.
-        BasicVector3D() { v_[0] = 0.0; v_[1] = 0.0; v_[2] = 0.0; }
-
-        //! Safe indexing of coordinates, for matrices, arrays, etc.
-        enum {
-            X = 0,                  //!< index for x-component
-            Y = 1,                  //!< index for y-component
-            Z = 2,                  //!< index for z-component
-            NUM_COORDINATES = 3,    //!< number of components
-            SIZE = NUM_COORDINATES  //!< number of components
-        };
-
-        //! Constructor from three numbers.
-        BasicVector3D(const T&x1, const T&y1, const T&z1) {
-            v_[0] = x1; v_[1] = y1; v_[2] = z1; }
-
-        //! Destructor.
-        virtual ~BasicVector3D() {}
-
-        // -----------------------------
-        // General arithmetic operations
-        // -----------------------------
-
-        //! Assignment.
-        inline BasicVector3D<T>& operator= (const BasicVector3D<T>& v) {
-            v_[0] = v.v_[0]; v_[1] = v.v_[1]; v_[2] = v.v_[2]; return *this;
-        }
-        //! Addition.
-        inline BasicVector3D<T>& operator+=(const BasicVector3D<T>& v) {
-            v_[0] += v.v_[0]; v_[1] += v.v_[1]; v_[2] += v.v_[2]; return *this;
-        }
-        //! Subtraction.
-        inline BasicVector3D<T>& operator-=(const BasicVector3D<T>& v) {
-            v_[0] -= v.v_[0]; v_[1] -= v.v_[1]; v_[2] -= v.v_[2]; return *this;
-        }
-        //! Multiplication by scalar.
-        inline BasicVector3D<T>& operator*=(double a) {
-            v_[0] *= a; v_[1] *= a; v_[2] *= a; return *this;
-        }
-        //! Division by scalar.
-        inline BasicVector3D<T>& operator/=(double a) {
-            v_[0] /= a; v_[1] /= a; v_[2] /= a; return *this;
-        }
+//! It defines only common functionality for those classes and
+//! should not be used as separate class.
+//!
+//! @author Evgeni Chernyaev 1996-2003
+//!
+template<class T> class BasicVector3D {
+  protected:
+    T v_[3];
+    
+  public:
+    //! Default constructor.
+    //! It is protected - this class should not be instantiated directly.
+    BasicVector3D() { v_[0] = 0.0; v_[1] = 0.0; v_[2] = 0.0; }
+
+    //! Safe indexing of coordinates, for matrices, arrays, etc.
+    enum {
+        X = 0,                  //!< index for x-component
+        Y = 1,                  //!< index for y-component
+        Z = 2,                  //!< index for z-component
+        NUM_COORDINATES = 3,    //!< number of components
+        SIZE = NUM_COORDINATES  //!< number of components
+    };
 
-        // ------------
-        // Subscripting
-        // ------------
-
-        //! Returns components by index.
-        inline T operator[](int i) const { return v_[i]; }
-
-        //! Sets components by index.
-        inline T& operator[](int i) { return v_[i]; }
-
-        // ------------------------------------
-        // Cartesian coordinate system: x, y, z
-        // ------------------------------------
-
-        //! Returns x-component in cartesian coordinate system.
-        inline T x() const { return v_[0]; }
-        //! Returns y-component in cartesian coordinate system.
-        inline T y() const { return v_[1]; }
-        //! Returns z-component in cartesian coordinate system.
-        inline T z() const { return v_[2]; }
-
-        //! Sets x-component in cartesian coordinate system.
-        inline void setX(const T&a) { v_[0] = a; }
-        //! Sets y-component in cartesian coordinate system.
-        inline void setY(const T&a) { v_[1] = a; }
-        //! Sets z-component in cartesian coordinate system.
-        inline void setZ(const T&a) { v_[2] = a; }
-
-        //! Sets components in cartesian coordinate system.
-        inline void setXYZ(const T&x1, const T&y1, const T&z1) {
-            v_[0] = x1; v_[1] = y1; v_[2] = z1; }
-
-        // ---------------------------------------------------------------
-        // Cylindrical coordinate system: rho (phi see below, z see above)
-        // ---------------------------------------------------------------
-
-        //! Returns squared rho-component in cylindrical coordinate system.
-        inline T magxy2() const { return x()*x()+y()*y(); }
-        //! Returns rho-component in cylindrical coordinate system.
-        inline T magxy() const { return std::sqrt(magxy2()); }
-
-        // ------------------------------------------
-        // Spherical coordinate system: r, phi, theta
-        // ------------------------------------------
-
-        //! Returns magnitude squared of the vector.
-        inline T mag2() const { return x()*x()+y()*y()+z()*z(); }
-        //! Returns magnitude of the vector.
-        inline T mag() const { return std::sqrt(mag2()); }
-        //! Returns azimuth angle.
-        inline T phi() const {
-            return x() == 0.0&& y() == 0.0 ? 0.0 : std::atan2(y(),x());
-        }
-        //! Returns polar angle.
-        inline T theta() const {
-            return x() == 0.0&& y() == 0.0&& z() == 0.0 ?
-                0.0 : std::atan2(magxy(),z());
-        }
-        //! Returns cosine of polar angle.
-        inline T cosTheta() const {
-            T ma = mag();
-            return std::abs(ma) == 0 ? 1 : z()/ma;
-        }
+    //! Constructor from three numbers.
+    BasicVector3D(const T&x1, const T&y1, const T&z1) {
+        v_[0] = x1; v_[1] = y1; v_[2] = z1; }
 
-        //! Sets magnitude.
-        inline void setMag(T ma) {
-            T factor = mag();
-            if (factor > 0.0) {
-                factor = ma/factor;
-                v_[0] *= factor; v_[1] *= factor; v_[2] *= factor;
-            }
-        }
-        //! Sets r-component in spherical coordinate system.
-        inline void setR(T ma) { setMag(ma); }
-        //! Sets phi-component in cylindrical or spherical coordinate system.
-        inline void setPhi(T ph) {
-            T xy = magxy();
-            setX(xy*std::cos(ph));
-            setY(xy*std::sin(ph));
-        }
-        //! Sets theta-component in spherical coordinate system.
-        inline void setTheta(T th) {
-            T ma = mag();
-            T ph = phi();
-            setXYZ(ma*std::sin(th)*std::cos(ph),
-                   ma*std::sin(th)*std::sin(ph),
-                   ma*std::cos(th));
-        }
+    //! Destructor.
+    virtual ~BasicVector3D() {}
 
-        // -------------------
-        // Combine two vectors
-        // -------------------
+    // -----------------------------
+    // General arithmetic operations
+    // -----------------------------
 
-        //! Scalar product.
-        inline T dot(const BasicVector3D<T>& v) const {
-            return x()*v.x()+y()*v.y()+z()*v.z();
-        }
-        //! Vector product.
-        inline BasicVector3D<T> cross(const BasicVector3D<T>& v) const {
-            return BasicVector3D<T>(y()*v.z()-v.y()*z(),
-                                    z()*v.x()-v.z()*x(),
-                                    x()*v.y()-v.x()*y());
-        }
-        //! Returns square of transverse component with respect to given axis.
-        inline T perp2(const BasicVector3D<T>& v) const {
-            T tot = v.mag2(), s = dot(v);
-            return tot > 0.0 ? mag2()-s*s/tot : mag2();
-        }
-        //! Returns transverse component with respect to given axis.
-        inline T perp(const BasicVector3D<T>& v) const {
-            return std::sqrt(perp2(v));
-        }
-        //! Returns angle with respect to another vector.
-        T angle(const BasicVector3D<T>& v) const;
-
-        // ---------------
-        // Related vectors
-        // ---------------
-
-        //! Returns unit vector parallel to this.
-        inline BasicVector3D<T> unit() const {
-            T len = mag();
-            return (len > 0.0) ?
-                BasicVector3D<T>(x()/len, y()/len, z()/len) :
-                BasicVector3D<T>();
-        }
+    //! Assignment.
+    inline BasicVector3D<T>& operator= (const BasicVector3D<T>& v) {
+        v_[0] = v.v_[0]; v_[1] = v.v_[1]; v_[2] = v.v_[2]; return *this;
+    }
+    //! Addition.
+    inline BasicVector3D<T>& operator+=(const BasicVector3D<T>& v) {
+        v_[0] += v.v_[0]; v_[1] += v.v_[1]; v_[2] += v.v_[2]; return *this;
+    }
+    //! Subtraction.
+    inline BasicVector3D<T>& operator-=(const BasicVector3D<T>& v) {
+        v_[0] -= v.v_[0]; v_[1] -= v.v_[1]; v_[2] -= v.v_[2]; return *this;
+    }
+    //! Multiplication by scalar.
+    inline BasicVector3D<T>& operator*=(double a) {
+        v_[0] *= a; v_[1] *= a; v_[2] *= a; return *this;
+    }
+    //! Division by scalar.
+    inline BasicVector3D<T>& operator/=(double a) {
+        v_[0] /= a; v_[1] /= a; v_[2] /= a; return *this;
+    }
 
-        //! Returns somewhat arbitrarily chosen orthogonal vector.
-        BasicVector3D<T> orthogonal() const {
-            T dx = x() < 0.0 ? -x() : x();
-            T dy = y() < 0.0 ? -y() : y();
-            T dz = z() < 0.0 ? -z() : z();
-            if (dx < dy) {
-                return dx < dz ?
-                    BasicVector3D<T>(0.0,z(),-y()) :
-                    BasicVector3D<T>(y(),-x(),0.0);
-            } else {
-                return dy < dz ?
-                    BasicVector3D<T>(-z(),0.0,x()) :
-                    BasicVector3D<T>(y(),-x(),0.0);
-            }
-        }
+    // ------------
+    // Subscripting
+    // ------------
 
-        // ---------
-        // Rotations
-        // ---------
-
-        //! Rotates around x-axis.
-        BasicVector3D<T>& rotateX(T a);
-        //! Rotates around y-axis.
-        BasicVector3D<T>& rotateY(T a);
-        //! Rotates around z-axis.
-        BasicVector3D<T>& rotateZ(T a);
-        //! Rotates around the axis specified by another vector.
-        BasicVector3D<T>& rotate(T a, const BasicVector3D<T>& v);
-
-        // ---------------------------------------------
-        // Specifically for grazing-incidence scattering
-        // ---------------------------------------------
-
-        //! Set wave vector for given wavelength and angles/
-        inline void setLambdaAlphaPhi(
-            const T&_lambda, const T&_alpha, const T&_phi)
-            {
-                T k = 2.*M_PI/_lambda;
-                v_[0] = k*std::cos(_alpha) * std::cos(_phi);
-                v_[1] = k*std::cos(_alpha) * std::sin(_phi);
-                v_[2] = k*std::sin(_alpha);
-            }
-
-        //! Transformation by Transform3D, as function.
-        //!
-        //! This is an alternative to operator*(const Transform3D&,&T) below.
-        BasicVector3D<T>&transform(const Transform3D&m);
-    };
+    //! Return components by index.
+    inline T operator[](int i) const { return v_[i]; }
 
-    // =========================================================================
-    // Non-member functions for BasicVector3D<double>
-    // =========================================================================
-
-    //! Output to stream.
-    //! @relates BasicVector3D
-    std::ostream&
-    operator<<(std::ostream&, const BasicVector3D<double>&);
-
-    // -----------------
-    // Scalar operations
-    // -----------------
-
-    //! Unary plus.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator+(const BasicVector3D<double>& v) { return v; }
-
-    //! Unary minus.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator-(const BasicVector3D<double>& v) {
-        return BasicVector3D<double>(-v.x(), -v.y(), -v.z()); }
-
-    //! Multiplication vector by scalar.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator*(const BasicVector3D<double>& v, double a) {
-        return BasicVector3D<double>(v.x()*a, v.y()*a, v.z()*a); }
-
-    //! Multiplication scalar by vector.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator*(double a, const BasicVector3D<double>& v) {
-        return BasicVector3D<double>(a*v.x(), a*v.y(), a*v.z()); }
-
-    //! Division vector by scalar.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator/(const BasicVector3D<double>& v, double a) {
-        return BasicVector3D<double>(v.x()/a, v.y()/a, v.z()/a); }
-
-    // -----------------------------------
-    // Operations involving another vector
-    // -----------------------------------
-
-    //! Addition of two vectors.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator+(const BasicVector3D<double>& a,const BasicVector3D<double>& b) {
-        return BasicVector3D<double>(a.x()+b.x(), a.y()+b.y(), a.z()+b.z()); }
-
-    //! Subtraction of two vectors.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    operator-(const BasicVector3D<double>& a,const BasicVector3D<double>& b) {
-        return BasicVector3D<double>(a.x()-b.x(), a.y()-b.y(), a.z()-b.z());
-    }
+    //! Set components by index.
+    inline T& operator[](int i) { return v_[i]; }
 
-    //! Scalar product of two vectors, as operator.
-    //! @relates BasicVector3D
-    inline double
-    operator*(const BasicVector3D<double>& a,const BasicVector3D<double>& b) {
-        return a.dot(b); }
-
-    //! Scalar product of two vectors, as function.
-    //! @relates BasicVector3D
-    inline double DotProduct(const BasicVector3D<double>&left,
-                             const BasicVector3D<double>&right)
-    {
-        return left.x()*right.x() + left.y()*right.y() + left.z()*right.z();
-    }
+    // ------------------------------------
+    // Cartesian coordinate system: x, y, z
+    // ------------------------------------
 
-    //! Cross product.
-    //! @relates BasicVector3D
-    inline BasicVector3D<double>
-    CrossProduct(const BasicVector3D<double>&left,
-                 const BasicVector3D<double>&right)
-    {
-        double x = left.y()*right.z() - left.z()*right.y();
-        double y = left.z()*right.x() - left.x()*right.z();
-        double z = left.x()*right.y() - left.y()*right.x();
-        return BasicVector3D<double> (x, y, z);
-    }
+    //! Return x-component in cartesian coordinate system.
+    inline T x() const { return v_[0]; }
+    //! Return y-component in cartesian coordinate system.
+    inline T y() const { return v_[1]; }
+    //! Return z-component in cartesian coordinate system.
+    inline T z() const { return v_[2]; }
 
-    //! Comparison of two vectors for equality.
-    //! @relates BasicVector3D
-    inline bool
-    operator==(const BasicVector3D<double>& a, const BasicVector3D<double>& b)
-        { return (a.x()==b.x()&& a.y()==b.y()&& a.z()==b.z()); }
-
-    //! Comparison of two vectors for inequality.
-    //! @relates BasicVector3D
-    inline bool
-    operator!=(const BasicVector3D<double>& a, const BasicVector3D<double>& b)
-        { return (a.x()!=b.x() || a.y()!=b.y() || a.z()!=b.z()); }
-
-    // ----------
-    // Transforms
-    // ----------
-
-    //! Transformation of BasicVector3D<double> by Transform3D.
-    //! @relates BasicVector3D
-    BasicVector3D<double>
-    operator*(const Transform3D& m, const BasicVector3D<double>& v);
-
-    // =========================================================================
-    // Non-member functions for BasicVector3D<std::complex<double> >
-    // =========================================================================
-
-    //! Output to stream.
-    //! @relates BasicVector3D
-    std::ostream&
-    operator<<(std::ostream&, const BasicVector3D<std::complex<double> >&);
-
-    // -----------------
-    // Scalar operations
-    // -----------------
-
-    //! Unary plus.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator+(const BasicVector3D<std::complex<double> >& v) { return v; }
-
-    //! Unary minus.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator-(const BasicVector3D<std::complex<double> >& v) {
-        return BasicVector3D<std::complex<double> >(-v.x(), -v.y(), -v.z());
-    }
+    //! Set x-component in cartesian coordinate system.
+    inline void setX(const T&a) { v_[0] = a; }
+    //! Set y-component in cartesian coordinate system.
+    inline void setY(const T&a) { v_[1] = a; }
+    //! Set z-component in cartesian coordinate system.
+    inline void setZ(const T&a) { v_[2] = a; }
 
-    //! Multiplication vector by scalar.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator*(const BasicVector3D<const std::complex<double> >& v,
-              const std::complex<double>&a) {
-        return BasicVector3D<std::complex<double> >(v.x()*a, v.y()*a, v.z()*a);
-    }
+    //! Set components in cartesian coordinate system.
+    inline void setXYZ(const T&x1, const T&y1, const T&z1) {
+        v_[0] = x1; v_[1] = y1; v_[2] = z1; }
 
-    //! Multiplication scalar by vector.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator*(const std::complex<double>&a,
-              const BasicVector3D<std::complex<double> >& v) {
-        return BasicVector3D<std::complex<double> >(a*v.x(), a*v.y(), a*v.z());
-    }
+    // ----
+    // Norm
+    // ----
 
-    //! Division vector by scalar.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator/(const BasicVector3D<std::complex<double> >& v,
-              const std::complex<double>&a) {
-        return BasicVector3D<std::complex<double> >(v.x()/a, v.y()/a, v.z()/a);
-    }
+    //! Return squared magnitude squared of the vector.
+    double mag2() const;
 
-    // -----------------------------------
-    // Operations involving another vector
-    // -----------------------------------
-
-    //! Addition of two vectors.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator+(const BasicVector3D<std::complex<double> >& a,
-              const BasicVector3D<std::complex<double> >& b) {
-        return BasicVector3D<std::complex<double> >(
-            a.x()+b.x(), a.y()+b.y(), a.z()+b.z() );
-    }
+    //! Return magnitude of the vector.
+    double mag() const;
 
-    //! Subtraction of two vectors.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    operator-(const BasicVector3D<std::complex<double> >& a,
-              const BasicVector3D<std::complex<double> >& b) {
-        return BasicVector3D<std::complex<double> >(
-            a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
-    }
+    // --------------------------------------------
+    // Cylindrical and spherical coordinate systems
+    // --------------------------------------------
 
-    //! Scalar product of two vectors, as operator.
-    //! @relates BasicVector3D
-    inline std::complex<double>
-    operator*(const BasicVector3D<std::complex<double> >& a,
-              const BasicVector3D<std::complex<double> >& b) {
-        return a.dot(b);
-    }
+    //! Return squared distance from z axis.
+    double magxy2() const;
 
-    //! Scalar product of two vectors, as function.
-    //! @relates BasicVector3D
-    inline std::complex<double>
-    DotProduct(const BasicVector3D<std::complex<double> >&left,
-               const BasicVector3D<std::complex<double> >&right)
-    {
-        return left.x()*right.x() + left.y()*right.y() + left.z()*right.z();
-    }
+    //! Return distance from z axis.
+    double magxy() const;
 
-    //! Cross product.
-    //! @relates BasicVector3D
-    inline BasicVector3D<std::complex<double> >
-    CrossProduct(const BasicVector3D<std::complex<double> >&left,
-                 const BasicVector3D<std::complex<double> >&right)
-    {
-        std::complex<double> x = left.y()*right.z() - left.z()*right.y();
-        std::complex<double> y = left.z()*right.x() - left.x()*right.z();
-        std::complex<double> z = left.x()*right.y() - left.y()*right.x();
-        return BasicVector3D<std::complex<double> > (x, y, z);
+    //! Return azimuth angle.
+    double phi() const;
+
+    //! Return polar angle.
+    double theta() const;
+
+    //! Return cosine of polar angle.
+    double cosTheta() const;
+
+    //! Scale to given magnitude.
+    void setMag(double ma);
+
+    // -------------------
+    // Combine two vectors
+    // -------------------
+
+    //! Scalar product.
+    double dot(const BasicVector3D<T>& v) const;
+
+    //! Vector product.
+    BasicVector3D<T> cross(const BasicVector3D<T>& v) const;
+
+    //! Return square of transverse component with respect to given axis.
+    double perp2(const BasicVector3D<T>& v) const;
+
+    //! Return transverse component with respect to given axis.
+    inline T perp(const BasicVector3D<T>& v) const {
+        return std::sqrt(perp2(v));
     }
 
-    //! Comparison of two vectors for equality.
-    //! @relates BasicVector3D
-    inline bool
-    operator==(const BasicVector3D<std::complex<double> >& a,
-               const BasicVector3D<std::complex<double> >& b) {
-        return (a.x()==b.x()&& a.y()==b.y()&& a.z()==b.z());
+    //! Return angle with respect to another vector.
+    T angle(const BasicVector3D<T>& v) const;
+
+    // ---------------
+    // Related vectors
+    // ---------------
+
+    //! Return unit vector parallel to this.
+    inline BasicVector3D<T> unit() const {
+        T len = mag();
+        return (len > 0.0) ?
+            BasicVector3D<T>(x()/len, y()/len, z()/len) :
+            BasicVector3D<T>();
     }
 
-    //! Comparison of two vectors for inequality.
-    //! @relates BasicVector3D
-    inline bool
-    operator!=(const BasicVector3D<std::complex<double> >& a,
-               const BasicVector3D<std::complex<double> >& b) {
-        return (a.x()!=b.x() || a.y()!=b.y() || a.z()!=b.z());
+    //! Return somewhat arbitrarily chosen orthogonal vector.
+    BasicVector3D<T> orthogonal() const {
+        T dx = x() < 0.0 ? -x() : x();
+        T dy = y() < 0.0 ? -y() : y();
+        T dz = z() < 0.0 ? -z() : z();
+        if (dx < dy) {
+            return dx < dz ?
+                BasicVector3D<T>(0.0,z(),-y()) :
+                BasicVector3D<T>(y(),-x(),0.0);
+        } else {
+            return dy < dz ?
+                BasicVector3D<T>(-z(),0.0,x()) :
+                BasicVector3D<T>(y(),-x(),0.0);
+        }
     }
 
-    // ----------
-    // Transforms
-    // ----------
+    // ---------
+    // Rotations
+    // ---------
+
+    //! Rotates around x-axis.
+    BasicVector3D<T>& rotateX(T a);
+    //! Rotates around y-axis.
+    BasicVector3D<T>& rotateY(T a);
+    //! Rotates around z-axis.
+    BasicVector3D<T>& rotateZ(T a);
+    //! Rotates around the axis specified by another vector.
+    BasicVector3D<T>& rotate(T a, const BasicVector3D<T>& v);
+
+    // ---------------------------------------------
+    // Specifically for grazing-incidence scattering
+    // ---------------------------------------------
+
+    //! Set wave vector for given wavelength and angles/
+    inline void setLambdaAlphaPhi(
+        const T&_lambda, const T&_alpha, const T&_phi)
+        {
+            T k = 2.*M_PI/_lambda;
+            v_[0] = k*std::cos(_alpha) * std::cos(_phi);
+            v_[1] = k*std::cos(_alpha) * std::sin(_phi);
+            v_[2] = k*std::sin(_alpha);
+        }
 
-    //! Transformation of BasicVector3D<double> by Transform3D.
-    //! @relates BasicVector3D
-    BasicVector3D<std::complex<double> >
-    operator*(const Transform3D& m,
-              const BasicVector3D<std::complex<double> >& v);
+    //! Transformation by Transform3D, as function.
+    //!
+    //! This is an alternative to operator*(const Transform3D&,&T) below.
+    BasicVector3D<T>&transform(const Transform3D&m);
+};
+
+// =========================================================================
+// Non-member functions for BasicVector3D<double>
+// =========================================================================
+
+//! Output to stream.
+//! @relates BasicVector3D
+std::ostream&
+operator<<(std::ostream&, const BasicVector3D<double>&);
+
+// -----------------
+// Scalar operations
+// -----------------
+
+//! Unary plus.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator+(const BasicVector3D<double>& v) { return v; }
+
+//! Unary minus.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator-(const BasicVector3D<double>& v) {
+    return BasicVector3D<double>(-v.x(), -v.y(), -v.z()); }
+
+//! Multiplication vector by scalar.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator*(const BasicVector3D<double>& v, double a) {
+    return BasicVector3D<double>(v.x()*a, v.y()*a, v.z()*a); }
+
+//! Multiplication scalar by vector.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator*(double a, const BasicVector3D<double>& v) {
+    return BasicVector3D<double>(a*v.x(), a*v.y(), a*v.z()); }
+
+//! Division vector by scalar.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator/(const BasicVector3D<double>& v, double a) {
+    return BasicVector3D<double>(v.x()/a, v.y()/a, v.z()/a); }
+
+// -----------------------------------
+// Operations involving another vector
+// -----------------------------------
+
+//! Addition of two vectors.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator+(const BasicVector3D<double>& a,const BasicVector3D<double>& b) {
+    return BasicVector3D<double>(a.x()+b.x(), a.y()+b.y(), a.z()+b.z()); }
+
+//! Subtraction of two vectors.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+operator-(const BasicVector3D<double>& a,const BasicVector3D<double>& b) {
+    return BasicVector3D<double>(a.x()-b.x(), a.y()-b.y(), a.z()-b.z());
+}
+
+//! Scalar product of two vectors, as operator.
+//! @relates BasicVector3D
+double operator*(const BasicVector3D<double>& a,
+                 const BasicVector3D<double>& b);
+
+//! Scalar product of two vectors, as function.
+//! @relates BasicVector3D
+inline double DotProduct(const BasicVector3D<double>&left,
+                         const BasicVector3D<double>&right)
+{
+    return left.x()*right.x() + left.y()*right.y() + left.z()*right.z();
+}
+
+//! Cross product.
+//! @relates BasicVector3D
+inline BasicVector3D<double>
+CrossProduct(const BasicVector3D<double>&left,
+             const BasicVector3D<double>&right)
+{
+    double x = left.y()*right.z() - left.z()*right.y();
+    double y = left.z()*right.x() - left.x()*right.z();
+    double z = left.x()*right.y() - left.y()*right.x();
+    return BasicVector3D<double> (x, y, z);
+}
+
+//! Comparison of two vectors for equality.
+//! @relates BasicVector3D
+inline bool
+operator==(const BasicVector3D<double>& a, const BasicVector3D<double>& b)
+    { return (a.x()==b.x()&& a.y()==b.y()&& a.z()==b.z()); }
+
+//! Comparison of two vectors for inequality.
+//! @relates BasicVector3D
+inline bool
+operator!=(const BasicVector3D<double>& a, const BasicVector3D<double>& b)
+    { return (a.x()!=b.x() || a.y()!=b.y() || a.z()!=b.z()); }
+
+// ----------
+// Transforms
+// ----------
+
+//! Transformation of BasicVector3D<double> by Transform3D.
+//! @relates BasicVector3D
+BasicVector3D<double>
+operator*(const Transform3D& m, const BasicVector3D<double>& v);
+
+// =========================================================================
+// Non-member functions for BasicVector3D<std::complex<double> >
+// =========================================================================
+
+//! Output to stream.
+//! @relates BasicVector3D
+std::ostream&
+operator<<(std::ostream&, const BasicVector3D<std::complex<double> >&);
+
+// -----------------
+// Scalar operations
+// -----------------
+
+//! Unary plus.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator+(const BasicVector3D<std::complex<double> >& v) { return v; }
+
+//! Unary minus.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator-(const BasicVector3D<std::complex<double> >& v) {
+    return BasicVector3D<std::complex<double> >(-v.x(), -v.y(), -v.z());
+}
+
+//! Multiplication vector by scalar.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator*(const BasicVector3D<const std::complex<double> >& v,
+          const std::complex<double>&a) {
+    return BasicVector3D<std::complex<double> >(v.x()*a, v.y()*a, v.z()*a);
+}
+
+//! Multiplication scalar by vector.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator*(const std::complex<double>&a,
+          const BasicVector3D<std::complex<double> >& v) {
+    return BasicVector3D<std::complex<double> >(a*v.x(), a*v.y(), a*v.z());
+}
+
+//! Division vector by scalar.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator/(const BasicVector3D<std::complex<double> >& v,
+          const std::complex<double>&a) {
+    return BasicVector3D<std::complex<double> >(v.x()/a, v.y()/a, v.z()/a);
+}
+
+// -----------------------------------
+// Operations involving another vector
+// -----------------------------------
+
+//! Addition of two vectors.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator+(const BasicVector3D<std::complex<double> >& a,
+          const BasicVector3D<std::complex<double> >& b) {
+    return BasicVector3D<std::complex<double> >(
+        a.x()+b.x(), a.y()+b.y(), a.z()+b.z() );
+}
+
+//! Subtraction of two vectors.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+operator-(const BasicVector3D<std::complex<double> >& a,
+          const BasicVector3D<std::complex<double> >& b) {
+    return BasicVector3D<std::complex<double> >(
+        a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
+}
+
+//! Cross product.
+//! @relates BasicVector3D
+inline BasicVector3D<std::complex<double> >
+CrossProduct(const BasicVector3D<std::complex<double> >&left,
+             const BasicVector3D<std::complex<double> >&right)
+{
+    std::complex<double> x = left.y()*right.z() - left.z()*right.y();
+    std::complex<double> y = left.z()*right.x() - left.x()*right.z();
+    std::complex<double> z = left.x()*right.y() - left.y()*right.x();
+    return BasicVector3D<std::complex<double> > (x, y, z);
+}
+
+//! Comparison of two vectors for equality.
+//! @relates BasicVector3D
+inline bool
+operator==(const BasicVector3D<std::complex<double> >& a,
+           const BasicVector3D<std::complex<double> >& b) {
+    return (a.x()==b.x()&& a.y()==b.y()&& a.z()==b.z());
+}
+
+//! Comparison of two vectors for inequality.
+//! @relates BasicVector3D
+inline bool
+operator!=(const BasicVector3D<std::complex<double> >& a,
+           const BasicVector3D<std::complex<double> >& b) {
+    return (a.x()!=b.x() || a.y()!=b.y() || a.z()!=b.z());
+}
+
+// ----------
+// Transforms
+// ----------
+
+//! Transformation of BasicVector3D<double> by Transform3D.
+//! @relates BasicVector3D
+BasicVector3D<std::complex<double> >
+operator*(const Transform3D& m,
+          const BasicVector3D<std::complex<double> >& v);
 
 }  // namespace Geometry
 
diff --git a/Core/Geometry/inc/Point3D.h b/Core/Geometry/inc/Point3D.h
index 60981e90b63a9fa141f5ab28bccf2ee0b8930eb2..639d2631ac86aabc2a573bdbb6f878df7fccf29f 100755
--- a/Core/Geometry/inc/Point3D.h
+++ b/Core/Geometry/inc/Point3D.h
@@ -66,17 +66,11 @@ public:
     Point3D<double> & operator=(const BasicVector3D<double> & v) {
         setXYZ(v.x(),v.y(),v.z()); return *this; }
 
-    //! Returns distance to the origin squared.
-    double distance2() const {
-        return mag2(); }
-    //! Returns distance to the point squared.
+    //! Returns squared distance to a given point.
     double distance2(const Point3D<double> & p) const {
         double dx = p.x()-x(), dy = p.y()-y(), dz = p.z()-z();
         return dx*dx + dy*dy + dz*dz; }
-    //! Returns distance to the origin.
-    double distance() const {
-        return std::sqrt(distance2()); }
-    //! Returns distance to the point.
+    //! Returns distance to a given point.
     double distance(const Point3D<double> & p) const {
         return std::sqrt(distance2(p)); }
 
diff --git a/Core/Geometry/inc/Transform3D.h b/Core/Geometry/inc/Transform3D.h
index f2d18c16cc6204f35a0c6e4f1448d2123a620efd..d5dd7f408fc2a2eca19b56c7db0e2c6c485af261 100755
--- a/Core/Geometry/inc/Transform3D.h
+++ b/Core/Geometry/inc/Transform3D.h
@@ -32,10 +32,6 @@
 
 namespace Geometry {
 
-template<class T> class Point3D;
-template<class T> class Vector3D;
-template<class T> class Normal3D;
-    
 class Translate3D;
 class Rotate3D;
 class Scale3D;
@@ -126,8 +122,8 @@ class Scale3D;
 //!
 //! Example of the usage:
 //!
-//!   Transform3D m1, m2, m3;
-//!   HepVector3D    v2, v1(0,0,0);
+//!   Transform3D   m1, m2, m3;
+//!   BasicVector3D v2, v1(0,0,0);
 //!
 //!   m1 = Rotate3D(angle, Vector3D(1,1,1));
 //!   m2 = Translate3D(dx,dy,dz);
@@ -147,8 +143,8 @@ class Scale3D;
 //!
 //! Example:
 //! @code
-//!   HepGeom::Transform3D m;
-//!   m = HepGeom::TranslateX3D(10.*cm);
+//!   Transform3D m;
+//!   m = TranslateX3D(10.*cm);
 //! @endcode
 //!
 //! @author <Evgueni.Tcherniaev@cern.ch> 1996-2003
diff --git a/Core/Geometry/src/BasicVector3D.cpp b/Core/Geometry/src/BasicVector3D.cpp
index bb9547caceadabc02c444539f4f31aacd34d332c..54d55b7be2e286bc3c2eb50e075de5dd902407a3 100755
--- a/Core/Geometry/src/BasicVector3D.cpp
+++ b/Core/Geometry/src/BasicVector3D.cpp
@@ -19,7 +19,7 @@
 //! Changes w.r.t. CLHEP:
 //! - eliminated support for type float
 //! - eliminated support for istream
-//! - added support for type complex<double>
+//! - added support for type complex<double> (but certain methods make no sense)
 //! - reworked doxygen comments
 //
 // ************************************************************************** //
@@ -33,8 +33,109 @@ typedef std::complex<double> complex_t;
 
 namespace Geometry {
 
+// -----------------------------------------------------------------------------
+// Norm
+// -----------------------------------------------------------------------------
+
+//! Return squared magnitude of the vector.
+template<>
+double BasicVector3D<double>::mag2() const
+{
+    return x()*x()+y()*y()+z()*z();
+}
+
+//! Return magnitude of the vector.
+template<class T>
+double BasicVector3D<T>::mag() const
+{
+    return std::sqrt(mag2());
+}
+
+// -----------------------------------------------------------------------------
+// Cylindrical and spherical coordinate systems
+// -----------------------------------------------------------------------------
+
+//! Return squared distance from z axis.
+template<>
+double BasicVector3D<double>::magxy2() const
+{
+    return x()*x()+y()*y();
+}
+
+//! Return distance from z axis.
+template<class T>
+double BasicVector3D<T>::magxy() const
+{
+    return std::sqrt(magxy2());
+}
+
+//! Return azimuth angle.
+template<>
+double BasicVector3D<double>::phi() const
+{
+    return x() == 0.0&& y() == 0.0 ? 0.0 : std::atan2(y(),x());
+}
+
+//! Return polar angle.
+template<>
+double BasicVector3D<double>::theta() const
+{
+    return x() == 0.0&& y() == 0.0&& z() == 0.0 ?
+        0.0 : std::atan2(magxy(),z());
+}
+
+//! Return cosine of polar angle.
+template<>
+double BasicVector3D<double>::cosTheta() const
+{
+    double ma = mag();
+    return std::abs(ma) == 0 ? 1 : z()/ma;
+}
+
+//! Scale to given magnitude.
+template<class T>
+void BasicVector3D<T>::setMag(double ma)
+{
+    double factor = mag();
+    if (factor > 0.0) {
+        factor = ma/factor;
+        v_[0] *= factor; v_[1] *= factor; v_[2] *= factor;
+    }
+}
+
+// -----------------------------------------------------------------------------
+// Combine two vectors
+// -----------------------------------------------------------------------------
+
+//! Scalar product.
+template<>
+double BasicVector3D<double>::dot(
+    const BasicVector3D<double>& v) const
+{
+    return x()*v.x()+y()*v.y()+z()*v.z();
+}
+
+//! Vector product.
+template<>
+BasicVector3D<double> BasicVector3D<double>::cross(
+    const BasicVector3D<double>& v) const
+{
+    return BasicVector3D<double>(y()*v.z()-v.y()*z(),
+                                 z()*v.x()-v.z()*x(),
+                                 x()*v.y()-v.x()*y());
+}
+
+//! Return square of transverse component with respect to given axis.
 template<>
-double BasicVector3D<double>::angle (const BasicVector3D<double>& v) const
+double BasicVector3D<double>::perp2(const BasicVector3D<double>& v) const
+{
+    double tot = v.mag2(), s = dot(v);
+    return tot > 0.0 ? mag2()-s*s/tot : mag2();
+}
+
+//! Return angle with respect to another vector.
+template<>
+double BasicVector3D<double>::angle(const BasicVector3D<double>& v) const
 {
     double cosa = 0;
     double ptot = mag()*v.mag();
@@ -46,6 +147,10 @@ double BasicVector3D<double>::angle (const BasicVector3D<double>& v) const
     return std::acos(cosa);
 }
 
+// -----------------------------------------------------------------------------
+// Rotations
+// -----------------------------------------------------------------------------
+
 template<>
 BasicVector3D<double>& BasicVector3D<double>::rotateX (double a)
 {
@@ -104,6 +209,10 @@ BasicVector3D<double>& BasicVector3D<double>::rotate (
     return *this;
 }
 
+// =========================================================================
+// Non-member functions for BasicVector3D<double>
+// =========================================================================
+
 std::ostream& operator<< (
         std::ostream& os, const BasicVector3D<double>& a)
 {
@@ -116,14 +225,27 @@ std::ostream& operator<< (
     return os << "(" << a.x() << "," << a.y() << "," << a.z() << ")";
 }
 
+// -----------------------------------------------------------------------------
+// Operations involving another vector
+// -----------------------------------------------------------------------------
+
+//! Scalar product of two vectors, as operator.
+double operator*(const BasicVector3D<double>& a,const BasicVector3D<double>& b)
+{
+    return a.dot(b);
+}
+
+// -----------------------------------------------------------------------------
+// Transforms
+// -----------------------------------------------------------------------------
+
 template<>
 BasicVector3D<double>& BasicVector3D<double>::transform (
             const Transform3D& m)
 {
-    double vx = x(), vy = y(), vz = z();
-    setXYZ(m.xx()*vx + m.xy()*vy + m.xz()*vz,
-           m.yx()*vx + m.yy()*vy + m.yz()*vz,
-           m.zx()*vx + m.zy()*vy + m.zz()*vz);
+    setXYZ(m.xx()*x() + m.xy()*y() + m.xz()*z(),
+           m.yx()*x() + m.yy()*y() + m.yz()*z(),
+           m.zx()*x() + m.zy()*y() + m.zz()*z());
     return *this;
 }
 
@@ -131,21 +253,19 @@ template<>
 BasicVector3D<complex_t>& BasicVector3D<complex_t>::transform (
             const Transform3D & m)
 {
-    complex_t vx = x(), vy = y(), vz = z();
-    setXYZ(m.xx()*vx + m.xy()*vy + m.xz()*vz,
-           m.yx()*vx + m.yy()*vy + m.yz()*vz,
-           m.zx()*vx + m.zy()*vy + m.zz()*vz);
+    setXYZ(m.xx()*x() + m.xy()*y() + m.xz()*z(),
+           m.yx()*x() + m.yy()*y() + m.yz()*z(),
+           m.zx()*x() + m.zy()*y() + m.zz()*z());
     return *this;
 }
 
 BasicVector3D<double> operator* (
             const Transform3D& m, const BasicVector3D<double>& v)
 {
-    double vx = v.x(), vy = v.y(), vz = v.z();
     return BasicVector3D<double>
-            (m.xx()*vx + m.xy()*vy + m.xz()*vz,
-             m.yx()*vx + m.yy()*vy + m.yz()*vz,
-             m.zx()*vx + m.zy()*vy + m.zz()*vz);
+            (m.xx()*v.x() + m.xy()*v.y() + m.xz()*v.z(),
+             m.yx()*v.x() + m.yy()*v.y() + m.yz()*v.z(),
+             m.zx()*v.x() + m.zy()*v.y() + m.zz()*v.z());
 }
 
 }  // namespace Geometry