Solve a big issue with data alignment and dynamic allocation:

* add a WithAlignedOperatorNew class with overloaded operator new
* make Matrix (and Quaternion, Transform, Hyperplane, etc.) use it
  if needed such that "*(new Vector4) = xpr" does not failed anymore.
* Please: make sure your classes having fixed size Eigen's vector
  or matrice attributes inherit WithAlignedOperatorNew
* add a ei_new_allocator STL memory allocator to use with STL containers.
  This allocator really calls operator new on your types (unlike GCC's
  new_allocator). Example:
  std::vector<Vector4f> data(10);
  will segfault if the vectorization is enabled, instead use:
  std::vector<Vector4f,ei_new_allocator<Vector4f> > data(10);
NOTE: you only have to worry if you deal with fixed-size matrix types
with "sizeof(matrix_type)%16==0"...

11 files changed, 294 insertions, 10 deletions

diff --git a/Eigen/src/Core/Matrix.h b/Eigen/src/Core/Matrix.h
index cf018ffef..55c7d982a 100644
--- a/Eigen/src/Core/Matrix.h
+++ b/Eigen/src/Core/Matrix.h
@@ -98,6 +98,9 @@ struct ei_traits<Matrix<_Scalar, _Rows, _Cols, _StorageOrder, _MaxRows, _MaxCols
 template<typename _Scalar, int _Rows, int _Cols, int _StorageOrder, int _MaxRows, int _MaxCols>
 class Matrix
   : public MatrixBase<Matrix<_Scalar, _Rows, _Cols, _StorageOrder, _MaxRows, _MaxCols> >
+    #ifdef EIGEN_VECTORIZE
+    , public ei_with_aligned_operator_new<_Scalar,ei_size_at_compile_time<_Rows,_Cols>::ret>
+    #endif
 {
   public:
     EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix)
diff --git a/Eigen/src/Core/util/Memory.h b/Eigen/src/Core/util/Memory.h
index fc99962e9..734296e77 100644
--- a/Eigen/src/Core/util/Memory.h
+++ b/Eigen/src/Core/util/Memory.h
@@ -106,4 +106,128 @@ inline static int ei_alignmentOffset(const Scalar* ptr, int maxOffset)
 # define ei_free_stack(PTR,TYPE,SIZE) delete[] PTR
 #endif
 
+/** \class WithAlignedOperatorNew
+  *
+  * \brief Enforces inherited classes to be 16 bytes aligned when dynamicalled allocated with operator new
+  *
+  * When Eigen's explicit vectorization is enabled, Eigen assumes that some fixed sizes types are aligned
+  * on a 16 bytes boundary. Such types include:
+  *  - Vector2d, Vector4f, Vector4i, Vector4d,
+  *  - Matrix2d, Matrix4f, Matrix4i, Matrix4d,
+  *  - etc.
+  * When objects are statically allocated, the compiler will automatically and always enforces 16 bytes
+  * alignment of the data. However some troubles might appear when data are dynamically allocated.
+  * Let's pick an example:
+  * \code
+  * struct Foo {
+  *   char dummy;
+  *   Vector4f some_vector;
+  * };
+  * Foo obj1;                           // static allocation
+  * obj1.some_vector = Vector4f(..);    // =>   OK
+  *
+  * Foo *pObj2 = new Foo;               // dynamic allocation
+  * pObj2->some_vector = Vector4f(..);  // =>  !! might segfault !!
+  * \endcode
+  * Here, the problem is that operator new is not aware of the compile time alignment requirement of the
+  * type Vector4f (and hence of the type Foo). Therefore "new Foo" does not necessarily returned a 16 bytes
+  * aligned pointer. The purpose of the class WithAlignedOperatorNew is exacly to overcome this issue, by
+  * overloading the operator new to return aligned data when the vectorization is enabled.
+  * Here is a similar safe example:
+  * \code
+  * struct Foo : WithAlignedOperatorNew {
+  *   char dummy;
+  *   Vector4f some_vector;
+  * };
+  * Foo obj1;                           // static allocation
+  * obj1.some_vector = Vector4f(..);    // =>   OK
+  *
+  * Foo *pObj2 = new Foo;               // dynamic allocation
+  * pObj2->some_vector = Vector4f(..);  // =>  SAFE !
+  * \endcode 
+  *
+  * \sa class ei_new_allocator
+  */
+struct WithAlignedOperatorNew
+{
+  #ifdef EIGEN_VECTORIZE
+
+  void *operator new(size_t size) throw()
+  {
+    void* ptr = 0;
+    if (posix_memalign(&ptr, 16, size)==0)
+      return ptr;
+    else
+      return 0;
+  }
+
+  void *operator new[](size_t size) throw()
+  {
+    void* ptr = 0;
+    if (posix_memalign(&ptr, 16, size)==0)
+      return ptr;
+    else
+      return 0;
+  }
+
+  void operator delete(void * ptr) { free(ptr); }
+  void operator delete[](void * ptr) { free(ptr); }
+  
+  #endif
+};
+
+template<typename T, int SizeAtCompileTime,
+         bool NeedsToAlign = (SizeAtCompileTime!=Dynamic) && ((sizeof(T)*SizeAtCompileTime)%16==0)>
+struct ei_with_aligned_operator_new : WithAlignedOperatorNew {};
+
+template<typename T, int SizeAtCompileTime>
+struct ei_with_aligned_operator_new<T,SizeAtCompileTime,false> {};
+
+/** \class ei_new_allocator
+  *
+  * \brief stl compatible allocator to use with with fixed-size vector and matrix types
+  *
+  * STL allocator simply wrapping operators new[] and delete[]. Unlike GCC's default new_allocator,
+  * ei_new_allocator call operator new on the type \a T and not the general new operator ignoring
+  * overloaded version of operator new.
+  * 
+  * Example:
+  * \code
+  * // Vector4f requires 16 bytes alignment:
+  * std::vector<Vector4f,ei_new_allocator<Vector4f> > dataVec4;
+  * // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
+  * std::vector<Vector3f> dataVec3;
+  * 
+  * struct Foo : WithAlignedOperatorNew {
+  *   char dummy;
+  *   Vector4f some_vector;
+  * };
+  * std::vector<Foo,ei_new_allocator<Foo> > dataFoo;
+  * \endcode
+  *
+  * \sa class WithAlignedOperatorNew
+  */
+template<typename T> class ei_new_allocator
+{
+  public:
+    typedef T         value_type;
+    typedef T*        pointer;
+    typedef const T*  const_pointer;
+    typedef T&        reference;
+    typedef const T&  const_reference;
+
+    template<typename OtherType>
+    struct rebind
+    { typedef ei_new_allocator<OtherType> other; };
+
+    T* address(T& ref) const { return &ref; }
+    const T* address(const T& ref) const { return &ref; }
+    T* allocate(size_t size, const void* = 0) { return new T[size]; }
+    void deallocate(T* ptr, size_t) { delete[] ptr; }
+    size_t max_size() const { return size_t(-1) / sizeof(T); }
+    // FIXME I'm note sure about this construction...
+    void construct(T* ptr, const T& refObj) { ::new(ptr) T(refObj); }
+    void destroy(T* ptr) { ptr->~T(); }
+};
+
 #endif // EIGEN_MEMORY_H
diff --git a/Eigen/src/Geometry/Hyperplane.h b/Eigen/src/Geometry/Hyperplane.h
index 6cf128712..faf2cf3ed 100644
--- a/Eigen/src/Geometry/Hyperplane.h
+++ b/Eigen/src/Geometry/Hyperplane.h
@@ -38,6 +38,9 @@
   */
 template <typename _Scalar, int _AmbientDim>
 class ParametrizedLine
+  #ifdef EIGEN_VECTORIZE
+  : public ei_with_aligned_operator_new<_Scalar,_AmbientDim>
+  #endif
 {
   public:
 
@@ -84,6 +87,9 @@ class ParametrizedLine
   */
 template <typename _Scalar, int _AmbientDim>
 class Hyperplane
+  #ifdef EIGEN_VECTORIZE
+  : public ei_with_aligned_operator_new<_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1>
+  #endif
 {
   public:
 
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 970280d33..876524cc0 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -59,6 +59,9 @@ template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
 
 template<typename _Scalar>
 class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
+  #ifdef EIGEN_VECTORIZE
+  , public ei_with_aligned_operator_new<_Scalar,4>
+  #endif
 {
   typedef RotationBase<Quaternion<_Scalar>,3> Base;
   typedef Matrix<_Scalar, 4, 1> Coefficients;
diff --git a/Eigen/src/Geometry/Scaling.h b/Eigen/src/Geometry/Scaling.h
index ded1b6220..da7cca68e 100644
--- a/Eigen/src/Geometry/Scaling.h
+++ b/Eigen/src/Geometry/Scaling.h
@@ -41,6 +41,9 @@
   */
 template<typename _Scalar, int _Dim>
 class Scaling
+  #ifdef EIGEN_VECTORIZE
+  : public ei_with_aligned_operator_new<_Scalar,_Dim>
+  #endif
 {
 public:
   /** dimension of the space */
diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h
index 37ac00d75..ee7bdce83 100644
--- a/Eigen/src/Geometry/Transform.h
+++ b/Eigen/src/Geometry/Transform.h
@@ -61,6 +61,9 @@ struct ei_transform_product_impl;
   */
 template<typename _Scalar, int _Dim>
 class Transform
+  #ifdef EIGEN_VECTORIZE
+  : public ei_with_aligned_operator_new<_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1)>
+  #endif
 {
 public:
 
diff --git a/Eigen/src/Geometry/Translation.h b/Eigen/src/Geometry/Translation.h
index 056fd2818..a322205a4 100644
--- a/Eigen/src/Geometry/Translation.h
+++ b/Eigen/src/Geometry/Translation.h
@@ -41,6 +41,9 @@
   */
 template<typename _Scalar, int _Dim>
 class Translation
+  #ifdef EIGEN_VECTORIZE
+  : public ei_with_aligned_operator_new<_Scalar,_Dim>
+  #endif
 {
 public:
   /** dimension of the space */
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index b1e206096..64298baf4 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,3 +1,4 @@
 IF(BUILD_DEMOS)
 ADD_SUBDIRECTORY(mandelbrot)
+ADD_SUBDIRECTORY(opengl)
 ENDIF(BUILD_DEMOS)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5453bfcf0..505cfd70d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -94,6 +94,7 @@ ENDIF(TEST_LIB)
 
 EI_ADD_TEST(meta)
 EI_ADD_TEST(sizeof)
+EI_ADD_TEST(dynalloc)
 EI_ADD_TEST(nomalloc)
 EI_ADD_TEST(packetmath)
 EI_ADD_TEST(basicstuff)
diff --git a/test/dynalloc.cpp b/test/dynalloc.cpp
new file mode 100644
index 000000000..287dce0d2
--- /dev/null
+++ b/test/dynalloc.cpp
@@ -0,0 +1,138 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#include "main.h"
+
+#include <ext/malloc_allocator.h>
+
+// test compilation with both a struct and a class...
+struct MyStruct : WithAlignedOperatorNew
+{
+  char dummychar;
+  Vector4f avec;
+};
+
+class MyClassA : public WithAlignedOperatorNew
+{
+  public:
+    char dummychar;
+    Vector4f avec;
+};
+
+// ..as well as with some other base classes
+
+class MyBaseClass
+{
+  public:
+    char dummychar;
+    float afloat;
+};
+
+class MyClassB : public WithAlignedOperatorNew, public MyBaseClass
+{
+  public:
+    char dummychar;
+    Vector4f avec;
+};
+
+class MyClassC : public MyBaseClass, public WithAlignedOperatorNew
+{
+  public:
+    char dummychar;
+    Vector4f avec;
+};
+
+template<typename T> void check_dynaligned()
+{
+  T* obj = new T;
+  VERIFY(size_t(obj)%16==0);
+  delete obj;
+}
+
+void test_dynalloc()
+{
+
+#ifdef EIGEN_VECTORIZE
+  for (int i=0; i<g_repeat*100; ++i)
+  {
+    CALL_SUBTEST( check_dynaligned<Vector4f>() );
+    CALL_SUBTEST( check_dynaligned<Vector2d>() );
+    CALL_SUBTEST( check_dynaligned<Matrix4f>() );
+    CALL_SUBTEST( check_dynaligned<Vector4d>() );
+    CALL_SUBTEST( check_dynaligned<Vector4i>() );
+  }
+  
+  // check static allocation, who knows ?
+  {
+    MyStruct foo0;  VERIFY(size_t(foo0.avec.data())%16==0);
+    MyClassA fooA;  VERIFY(size_t(fooA.avec.data())%16==0);
+    MyClassB fooB;  VERIFY(size_t(fooB.avec.data())%16==0);
+    MyClassC fooC;  VERIFY(size_t(fooC.avec.data())%16==0);
+  }
+
+  // dynamic allocation, single object
+  for (int i=0; i<g_repeat*100; ++i)
+  {
+    MyStruct *foo0 = new MyStruct();  VERIFY(size_t(foo0->avec.data())%16==0);
+    MyClassA *fooA = new MyClassA();  VERIFY(size_t(fooA->avec.data())%16==0);
+    MyClassB *fooB = new MyClassB();  VERIFY(size_t(fooB->avec.data())%16==0);
+    MyClassC *fooC = new MyClassC();  VERIFY(size_t(fooC->avec.data())%16==0);
+    delete foo0;
+    delete fooA;
+    delete fooB;
+    delete fooC;
+  }
+
+  // dynamic allocation, array
+  const int N = 10;
+  for (int i=0; i<g_repeat*100; ++i)
+  {
+    MyStruct *foo0 = new MyStruct[N];  VERIFY(size_t(foo0->avec.data())%16==0);
+    MyClassA *fooA = new MyClassA[N];  VERIFY(size_t(fooA->avec.data())%16==0);
+    MyClassB *fooB = new MyClassB[N];  VERIFY(size_t(fooB->avec.data())%16==0);
+    MyClassC *fooC = new MyClassC[N];  VERIFY(size_t(fooC->avec.data())%16==0);
+    delete[] foo0;
+    delete[] fooA;
+    delete[] fooB;
+    delete[] fooC;
+  }
+
+  // std::vector
+  for (int i=0; i<g_repeat*100; ++i)
+  {
+    std::vector<Vector4f, ei_new_allocator<Vector4f> > vecs(N);
+    for (int j=0; j<N; ++j)
+    {
+      VERIFY(size_t(vecs[j].data())%16==0);
+    }
+    std::vector<MyStruct,ei_new_allocator<MyStruct> > foos(N);
+    for (int j=0; j<N; ++j)
+    {
+      VERIFY(size_t(foos[j].avec.data())%16==0);
+    }
+  }
+  
+#endif // EIGEN_VECTORIZE
+
+}
diff --git a/test/nomalloc.cpp b/test/nomalloc.cpp
index a1b12f57c..78bb12a8a 100644
--- a/test/nomalloc.cpp
+++ b/test/nomalloc.cpp
@@ -1,6 +1,7 @@
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra. Eigen itself is part of the KDE project.
 //
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
 // Copyright (C) 2006-2008 Benoit Jacob <jacob@math.jussieu.fr>
 //
 // Eigen is free software; you can redistribute it and/or
@@ -29,14 +30,14 @@
 #include "main.h"
 
 void* operator new[] (size_t n)
-  {
-    ei_assert(false && "operator new should never be called with fixed size path");
-    // the following is in case assertion are disabled
-    std::cerr << "operator new should never be called with fixed size path" << std::endl;
-    exit(2);
-    void* p = malloc(n);
-    return p;
-  }
+{
+  ei_assert(false && "operator new should never be called with fixed size path");
+  // the following is in case assertion are disabled
+  std::cerr << "operator new should never be called with fixed size path" << std::endl;
+  exit(2);
+  void* p = malloc(n);
+  return p;
+}
 
 void operator delete[](void* p) throw()
 {
@@ -54,8 +55,6 @@ template<typename MatrixType> void nomalloc(const MatrixType& m)
   int rows = m.rows();
   int cols = m.cols();
 
-  // this test relies a lot on Random.h, and there's not much more that we can do
-  // to test it, hence I consider that we will have tested Random.h
   MatrixType m1 = MatrixType::Random(rows, cols),
              m2 = MatrixType::Random(rows, cols),
              m3(rows, cols),