From 8e6f2ca5de621509935775c403c6e42114ef3156 Mon Sep 17 00:00:00 2001
From: Mark Olesen <Mark.Olesen@esi-group.com>
Date: Thu, 14 Apr 2022 09:53:20 +0200
Subject: [PATCH] ENH: additional Field normalise method (#2444)

- for most field types this is a no-op, but for a field of floatVector
  or doubleVector (eg, vector and solveVector) it will normalise each
  element with divide-by-zero protection.

  More reliable and efficient than dividing a field by the mag of itself
  (even with VSMALL protection).
  Applied to FieldField and GeometricField as well.

  Eg,
      fld.normalise();
  vs.
      fld /= mag(fld) + VSMALL;

ENH: support optional tolerance for vector::normalise

- for cases where tolerances larger than ROOTVSMALL are preferable.
  Not currently available for the field method (a templating question).

ENH: vector::removeCollinear method

- when working with geometries it is frequently necessary to have a
  normal vector without any collinear components. The removeCollinear
  method provides for clearer, compacter code.

  Eg,
      vector edgeNorm = ...;

      const vector edgeDirn = e.unitVec(points());

      edgeNorm.removeCollinear(edgeDirn);
      edgeNorm.normalise();

  vs.
      vector edgeNorm = ...;

      const vector edgeDirn = e.unitVec(points());

      edgeNorm -= edgeDirn*(edgeDirn & edgeNorm);
      edgeNorm /= mag(edgeNorm);
---
 applications/test/vector/Test-vector.C        |  46 ++++++-
 src/OpenFOAM/Make/files                       |   1 +
 .../FieldFields/FieldField/FieldField.C       |  12 +-
 .../FieldFields/FieldField/FieldField.H       |  14 ++-
 src/OpenFOAM/fields/Fields/Field/Field.C      |   6 +
 src/OpenFOAM/fields/Fields/Field/Field.H      |  14 ++-
 .../fields/Fields/vectorField/vectorField.C   | 112 ++++++++++++++++++
 .../fields/Fields/vectorField/vectorField.H   |  13 +-
 .../GeometricField/GeometricField.C           |  10 +-
 .../GeometricField/GeometricField.H           |   7 +-
 .../pointPatchField/pointPatchField.H         |  21 ++--
 src/OpenFOAM/primitives/Vector/Vector.H       |  17 ++-
 src/OpenFOAM/primitives/Vector/VectorI.H      |  17 ++-
 src/OpenFOAM/primitives/Vector2D/Vector2D.H   |  16 ++-
 src/OpenFOAM/primitives/Vector2D/Vector2DI.H  |  15 ++-
 .../primitives/VectorSpace/VectorSpaceI.H     |   2 +
 .../primitives/quaternion/quaternion.H        |  78 +++++++-----
 .../primitives/quaternion/quaternionI.H       |  74 ++++++++----
 .../primitives/septernion/septernion.C        |   2 +-
 src/OpenFOAM/primitives/triad/triad.C         |   2 +-
 src/OpenFOAM/primitives/triad/triad.H         |  28 +++--
 src/OpenFOAM/primitives/triad/triadI.H        |  10 +-
 .../rigidBodySolver/rigidBodySolver.C         |   3 +-
 23 files changed, 406 insertions(+), 114 deletions(-)
 create mode 100644 src/OpenFOAM/fields/Fields/vectorField/vectorField.C

diff --git a/applications/test/vector/Test-vector.C b/applications/test/vector/Test-vector.C
index 2cfebcd81d..417d25aad9 100644
--- a/applications/test/vector/Test-vector.C
+++ b/applications/test/vector/Test-vector.C
@@ -5,7 +5,7 @@
     \\  /    A nd           | www.openfoam.com
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
-    Copyright (C) 2018-2021 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -31,7 +31,7 @@ Description
 
 \*---------------------------------------------------------------------------*/
 
-#include "vector.H"
+#include "vectorField.H"
 #include "IOstreams.H"
 #include <algorithm>
 #include <random>
@@ -110,6 +110,16 @@ void testData(const VecSpace& vs)
 }
 
 
+template<class Type>
+void testNormalise(Field<Type>& fld)
+{
+    Info<< nl << pTraits<Type>::typeName << " Field" << nl
+        << "  orig: " << fld << nl;
+    fld.normalise();
+    Info<< "  norm: " << fld << nl;
+}
+
+
 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 //  Main program:
 
@@ -138,6 +148,38 @@ int main(int argc, char *argv[])
         Info<< "shuffled: " << vec2 << nl;
     }
 
+    // Basic tests for fields
+    {
+        scalarField sfld1
+        ({
+            0.0,
+            1.0,
+            2.0
+        });
+
+        testNormalise(sfld1);
+
+        Field<floatVector> vfld1
+        ({
+            floatVector(0.0, 1.0, 2.0),
+            floatVector(0, 0, floatScalarSMALL),
+            floatVector(0, 0, floatScalarVSMALL),
+            floatVector(0, 2, 1),
+        });
+
+        testNormalise(vfld1);
+
+        Field<doubleVector> vfld2
+        ({
+            doubleVector(0.0, 1.0, 2.0),
+            doubleVector(0, 0, doubleScalarSMALL),
+            doubleVector(0, 0, doubleScalarVSMALL),
+            doubleVector(0, 2, 1),
+        });
+
+        testNormalise(vfld2);
+    }
+
     Info<< "\nEnd\n" << nl;
 
     return 0;
diff --git a/src/OpenFOAM/Make/files b/src/OpenFOAM/Make/files
index d08c39dadf..d58a27f7ae 100644
--- a/src/OpenFOAM/Make/files
+++ b/src/OpenFOAM/Make/files
@@ -726,6 +726,7 @@ $(Fields)/labelField/labelFieldIOField.C
 $(Fields)/scalarField/scalarField.C
 $(Fields)/scalarField/scalarIOField.C
 $(Fields)/scalarField/scalarFieldIOField.C
+$(Fields)/vectorField/vectorField.C
 $(Fields)/vectorField/vectorIOField.C
 $(Fields)/vectorField/vectorFieldIOField.C
 $(Fields)/vector2DField/vector2DIOField.C
diff --git a/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.C b/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.C
index 29f4fe8c21..4905f9c03a 100644
--- a/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.C
+++ b/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.C
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2019 OpenCFD Ltd.
+    Copyright (C) 2019-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -225,6 +225,16 @@ void FieldField<Field, Type>::negate()
 }
 
 
+template<template<class> class Field, class Type>
+void FieldField<Field, Type>::normalise()
+{
+    forAll(*this, i)
+    {
+        this->operator[](i).normalise();
+    }
+}
+
+
 template<template<class> class Field, class Type>
 tmp<FieldField<Field, typename FieldField<Field, Type>::cmptType>>
 FieldField<Field, Type>::component
diff --git a/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.H b/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.H
index d7d591de6a..7b408ceca5 100644
--- a/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.H
+++ b/src/OpenFOAM/fields/FieldFields/FieldField/FieldField.H
@@ -6,6 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
+    Copyright (C) 2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -34,8 +35,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef FieldField_H
-#define FieldField_H
+#ifndef Foam_FieldField_H
+#define Foam_FieldField_H
 
 #include "tmp.H"
 #include "PtrList.H"
@@ -48,7 +49,7 @@ SourceFiles
 namespace Foam
 {
 
-// Forward declarations
+// Forward Declarations
 
 template<template<class> class Field, class Type>
 class FieldField;
@@ -131,11 +132,14 @@ public:
         );
 
 
-    // Member functions
+    // Member Functions
 
-        //- Negate this field
+        //- Negate this field. See notes in Field
         void negate();
 
+        //- Normalise this field. See notes in Field
+        void normalise();
+
         //- Return a component field of the field
         tmp<FieldField<Field, cmptType>> component(const direction) const;
 
diff --git a/src/OpenFOAM/fields/Fields/Field/Field.C b/src/OpenFOAM/fields/Fields/Field/Field.C
index f7fd4066ad..31ad1b2286 100644
--- a/src/OpenFOAM/fields/Fields/Field/Field.C
+++ b/src/OpenFOAM/fields/Fields/Field/Field.C
@@ -533,6 +533,12 @@ void Foam::Field<Type>::negate()
 }
 
 
+// A no-op except for vector specialization
+template<class Type>
+void Foam::Field<Type>::normalise()
+{}
+
+
 template<class Type>
 Foam::tmp<Foam::Field<typename Foam::Field<Type>::cmptType>>
 Foam::Field<Type>::component
diff --git a/src/OpenFOAM/fields/Fields/Field/Field.H b/src/OpenFOAM/fields/Fields/Field/Field.H
index 55f11617b8..8ea04193fb 100644
--- a/src/OpenFOAM/fields/Fields/Field/Field.H
+++ b/src/OpenFOAM/fields/Fields/Field/Field.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2015-2021 OpenCFD Ltd.
+    Copyright (C) 2015-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -42,8 +42,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef Field_H
-#define Field_H
+#ifndef Foam_Field_H
+#define Foam_Field_H
 
 #include "tmp.H"
 #include "direction.H"
@@ -340,10 +340,16 @@ public:
             const UList<scalar>& weights
         );
 
-        //- Negate this field (negative).
+        //- Inplace negate this field (negative).
         //  Inverts the state for a bool field.
         void negate();
 
+        //- Inplace normalise this field.
+        //- Generally a no-op except for vector fields.
+        //  Vector fields are normalised by their magnitude.
+        //  Small vectors (mag less than ROOTVSMALL) are set to zero.
+        void normalise();
+
         //- Return a component field of the field
         tmp<Field<cmptType>> component(const direction) const;
 
diff --git a/src/OpenFOAM/fields/Fields/vectorField/vectorField.C b/src/OpenFOAM/fields/Fields/vectorField/vectorField.C
new file mode 100644
index 0000000000..bc759e49a6
--- /dev/null
+++ b/src/OpenFOAM/fields/Fields/vectorField/vectorField.C
@@ -0,0 +1,112 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | www.openfoam.com
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+    Copyright (C) 2022 OpenCFD Ltd.
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: 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 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM 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 General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "vectorField.H"
+
+// * * * * * * * * * * * * * * * Specializations * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+// Note: the mag of vector and division is written out to avoid any casting
+// between float and double.
+//
+// This enables specialization for floatVector and doubleVector independent
+// of the definition of 'scalar' or 'vector' - useful for mixed-precision
+// operation.
+
+template<>
+void Field<Vector<float>>::normalise()
+{
+    typedef float cmptType;
+
+    constexpr float tol = floatScalarROOTVSMALL;
+
+    for (Vector<cmptType>& v : *this)
+    {
+        // Foam::mag but using cmptType instead of scalar
+        cmptType s
+        (
+            ::sqrt(magSqr(v.x()) + magSqr(v.y()) + magSqr(v.z()))
+        );
+
+        if (s < tol)
+        {
+            v.x() = 0;
+            v.y() = 0;
+            v.z() = 0;
+        }
+        else
+        {
+            v.x() /= s;
+            v.y() /= s;
+            v.z() /= s;
+        }
+    }
+}
+
+
+template<>
+void Field<Vector<double>>::normalise()
+{
+    typedef double cmptType;
+
+    constexpr double tol = doubleScalarROOTVSMALL;
+
+    for (Vector<cmptType>& v : *this)
+    {
+        // Foam::mag but using cmptType instead of scalar
+        cmptType s
+        (
+            ::sqrt(magSqr(v.x()) + magSqr(v.y()) + magSqr(v.z()))
+        );
+
+        if (s < tol)
+        {
+            v.x() = 0;
+            v.y() = 0;
+            v.z() = 0;
+        }
+        else
+        {
+            v.x() /= s;
+            v.y() /= s;
+            v.z() /= s;
+        }
+    }
+}
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+// ************************************************************************* //
diff --git a/src/OpenFOAM/fields/Fields/vectorField/vectorField.H b/src/OpenFOAM/fields/Fields/vectorField/vectorField.H
index 48c2144929..a0fbed9c1c 100644
--- a/src/OpenFOAM/fields/Fields/vectorField/vectorField.H
+++ b/src/OpenFOAM/fields/Fields/vectorField/vectorField.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2017 OpenFOAM Foundation
-    Copyright (C) 2019 OpenCFD Ltd.
+    Copyright (C) 2019-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -35,8 +35,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef vectorField_H
-#define vectorField_H
+#ifndef Foam_vectorField_H
+#define Foam_vectorField_H
 
 #include "scalarField.H"
 #include "vector.H"
@@ -52,6 +52,13 @@ typedef Field<vector> vectorField;
 
 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
+//- Inplace normalise (float) vector field
+template<> void Field<Vector<float>>::normalise();
+
+//- Inplace normalise (double) vector field
+template<> void Field<Vector<double>>::normalise();
+
+
 //- Zip together vector field from components
 template<class Cmpt>
 void zip
diff --git a/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.C b/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.C
index 1532c6fffd..b93df0811c 100644
--- a/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.C
+++ b/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.C
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2017 OpenFOAM Foundation
-    Copyright (C) 2015-2021 OpenCFD Ltd.
+    Copyright (C) 2015-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -1193,6 +1193,14 @@ void Foam::GeometricField<Type, PatchField, GeoMesh>::negate()
 }
 
 
+template<class Type, template<class> class PatchField, class GeoMesh>
+void Foam::GeometricField<Type, PatchField, GeoMesh>::normalise()
+{
+    primitiveFieldRef().normalise();
+    boundaryFieldRef().normalise();
+}
+
+
 // * * * * * * * * * * * * * * * Member Operators  * * * * * * * * * * * * * //
 
 template<class Type, template<class> class PatchField, class GeoMesh>
diff --git a/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.H b/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.H
index 24401b338d..db89a75277 100644
--- a/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.H
+++ b/src/OpenFOAM/fields/GeometricFields/GeometricField/GeometricField.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2017 OpenFOAM Foundation
-    Copyright (C) 2015-2020 OpenCFD Ltd.
+    Copyright (C) 2015-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -527,9 +527,12 @@ public:
 
     // Member Function *this Operators
 
-        //- Negate the field inplace
+        //- Negate the field inplace. See notes in Field
         void negate();
 
+        //- Normalise the field inplace. See notes in Field
+        void normalise();
+
         //- Replace specified field component with content from another field
         void replace
         (
diff --git a/src/OpenFOAM/fields/pointPatchFields/pointPatchField/pointPatchField.H b/src/OpenFOAM/fields/pointPatchFields/pointPatchField/pointPatchField.H
index 15e2af8241..92aee089a6 100644
--- a/src/OpenFOAM/fields/pointPatchFields/pointPatchField/pointPatchField.H
+++ b/src/OpenFOAM/fields/pointPatchFields/pointPatchField/pointPatchField.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2017 OpenFOAM Foundation
-    Copyright (C) 2019 OpenCFD Ltd.
+    Copyright (C) 2019-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -42,8 +42,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef pointPatchField_H
-#define pointPatchField_H
+#ifndef Foam_pointPatchField_H
+#define Foam_pointPatchField_H
 
 #include "pointPatch.H"
 #include "DimensionedField.H"
@@ -54,7 +54,7 @@ SourceFiles
 namespace Foam
 {
 
-// Forward declarations
+// Forward Declarations
 
 class objectRegistry;
 class dictionary;
@@ -75,7 +75,7 @@ Ostream& operator<<(Ostream&, const pointPatchField<Type>&);
 template<class Type>
 class pointPatchField
 {
-    // Private data
+    // Private Data
 
         //- Reference to patch
         const pointPatch& patch_;
@@ -471,14 +471,13 @@ public:
         );
 
 
-    // Housekeeping
+    // Other Methods
 
-    // Includes some dummy methods to ensure that various
-    // FieldFieldFunctions will be definable.
-
-        //- Negate the field inplace
-        //  [dummy placeholder for FieldField] (2019-11)
+        //- Negate the field inplace. Dummy placeholder for FieldField
         void negate() {}
+
+        //- Normalise the field inplace. Dummy placeholder for FieldField
+        void normalise() {}
 };
 
 
diff --git a/src/OpenFOAM/primitives/Vector/Vector.H b/src/OpenFOAM/primitives/Vector/Vector.H
index b222616520..05ae0934c1 100644
--- a/src/OpenFOAM/primitives/Vector/Vector.H
+++ b/src/OpenFOAM/primitives/Vector/Vector.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2018-2019 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -41,8 +41,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef Vector_H
-#define Vector_H
+#ifndef Foam_Vector_H
+#define Foam_Vector_H
 
 #include "contiguous.H"
 #include "VectorSpace.H"
@@ -130,15 +130,20 @@ public:
         //- Access to the vector z component
         inline Cmpt& z();
 
+        //- Inplace normalise the vector by its magnitude
+        //  For small magnitudes (less than ROOTVSMALL) set to zero.
+        //  Will not be particularly useful for a vector of labels
+        inline Vector<Cmpt>& normalise(const scalar tol = ROOTVSMALL);
 
-        //- Normalise the vector by its magnitude
-        inline Vector<Cmpt>& normalise();
+        //- Inplace removal of components that are collinear to the given
+        //- unit vector.
+        inline Vector<Cmpt>& removeCollinear(const Vector<Cmpt>& unitVec);
 
 
         //- Return *this (used for point which is a typedef to Vector<scalar>.
         inline const Vector<Cmpt>& centre
         (
-            const Foam::List<Vector<Cmpt>>&
+            const Foam::UList<Vector<Cmpt>>&  /* (unused) */
         ) const;
 };
 
diff --git a/src/OpenFOAM/primitives/Vector/VectorI.H b/src/OpenFOAM/primitives/Vector/VectorI.H
index ad2f5e3cd9..3f697e1844 100644
--- a/src/OpenFOAM/primitives/Vector/VectorI.H
+++ b/src/OpenFOAM/primitives/Vector/VectorI.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2018-2020 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -112,7 +112,7 @@ inline Cmpt& Foam::Vector<Cmpt>::z()
 template<class Cmpt>
 inline const Foam::Vector<Cmpt>& Foam::Vector<Cmpt>::centre
 (
-    const Foam::List<Vector<Cmpt>>&
+    const Foam::UList<Vector<Cmpt>>&
 ) const
 {
     return *this;
@@ -120,11 +120,11 @@ inline const Foam::Vector<Cmpt>& Foam::Vector<Cmpt>::centre
 
 
 template<class Cmpt>
-inline Foam::Vector<Cmpt>& Foam::Vector<Cmpt>::normalise()
+inline Foam::Vector<Cmpt>& Foam::Vector<Cmpt>::normalise(const scalar tol)
 {
     const scalar s(Foam::mag(*this));
 
-    if (s < ROOTVSMALL)
+    if (s < tol)
     {
         *this = Zero;
     }
@@ -137,6 +137,15 @@ inline Foam::Vector<Cmpt>& Foam::Vector<Cmpt>::normalise()
 }
 
 
+template<class Cmpt>
+inline Foam::Vector<Cmpt>&
+Foam::Vector<Cmpt>::removeCollinear(const Vector<Cmpt>& unitVec)
+{
+    *this -= (*this & unitVec) * unitVec;
+    return *this;
+}
+
+
 // * * * * * * * * * * * * * * * Global Operators  * * * * * * * * * * * * * //
 
 namespace Foam
diff --git a/src/OpenFOAM/primitives/Vector2D/Vector2D.H b/src/OpenFOAM/primitives/Vector2D/Vector2D.H
index 23664464d3..f8126bbf49 100644
--- a/src/OpenFOAM/primitives/Vector2D/Vector2D.H
+++ b/src/OpenFOAM/primitives/Vector2D/Vector2D.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2018-2021 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -37,8 +37,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef Vector2D_H
-#define Vector2D_H
+#ifndef Foam_Vector2D_H
+#define Foam_Vector2D_H
 
 #include "contiguous.H"
 #include "VectorSpace.H"
@@ -116,8 +116,16 @@ public:
         //- Access to the vector y component
         inline Cmpt& y();
 
+
         //- Normalise the vector by its magnitude
-        inline Vector2D<Cmpt>& normalise();
+        //  For small magnitudes (less than ROOTVSMALL) set to zero.
+        //  Will not be particularly useful for a vector of labels
+        inline Vector2D<Cmpt>& normalise(const scalar tol = ROOTVSMALL);
+
+        //- Inplace removal of components that are collinear to the given
+        //- unit vector.
+        inline Vector2D<Cmpt>& removeCollinear(const Vector2D<Cmpt>& unitVec);
+
 
         //- Perp dot product (dot product with perpendicular vector)
         inline scalar perp(const Vector2D<Cmpt>& b) const;
diff --git a/src/OpenFOAM/primitives/Vector2D/Vector2DI.H b/src/OpenFOAM/primitives/Vector2D/Vector2DI.H
index 46599dfc48..a5c63b818d 100644
--- a/src/OpenFOAM/primitives/Vector2D/Vector2DI.H
+++ b/src/OpenFOAM/primitives/Vector2D/Vector2DI.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2018-2021 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -91,11 +91,11 @@ inline Cmpt& Foam::Vector2D<Cmpt>::y()
 
 
 template<class Cmpt>
-inline Foam::Vector2D<Cmpt>& Foam::Vector2D<Cmpt>::normalise()
+inline Foam::Vector2D<Cmpt>& Foam::Vector2D<Cmpt>::normalise(const scalar tol)
 {
     const scalar s(Foam::mag(*this));
 
-    if (s < ROOTVSMALL)
+    if (s < tol)
     {
         *this = Zero;
     }
@@ -108,6 +108,15 @@ inline Foam::Vector2D<Cmpt>& Foam::Vector2D<Cmpt>::normalise()
 }
 
 
+template<class Cmpt>
+inline Foam::Vector2D<Cmpt>&
+Foam::Vector2D<Cmpt>::removeCollinear(const Vector2D<Cmpt>& unitVec)
+{
+    *this -= (*this & unitVec) * unitVec;
+    return *this;
+}
+
+
 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
 namespace Foam
diff --git a/src/OpenFOAM/primitives/VectorSpace/VectorSpaceI.H b/src/OpenFOAM/primitives/VectorSpace/VectorSpaceI.H
index 5bde1e96e8..68a75cf0ff 100644
--- a/src/OpenFOAM/primitives/VectorSpace/VectorSpaceI.H
+++ b/src/OpenFOAM/primitives/VectorSpace/VectorSpaceI.H
@@ -482,6 +482,8 @@ inline scalar mag
 }
 
 
+//- Return the vector type normalised by its magnitude
+//  For small magnitudes (less than ROOTVSMALL) return zero.
 template<class Form, class Cmpt, direction Ncmpts>
 inline VectorSpace<Form, Cmpt, Ncmpts> normalised
 (
diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.H b/src/OpenFOAM/primitives/quaternion/quaternion.H
index 9935d4c8cb..4f0ab6b903 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternion.H
+++ b/src/OpenFOAM/primitives/quaternion/quaternion.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2019-2020 OpenCFD Ltd.
+    Copyright (C) 2019-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -36,14 +36,15 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef quaternion_H
-#define quaternion_H
+#ifndef Foam_quaternion_H
+#define Foam_quaternion_H
 
 #include "scalar.H"
 #include "vector.H"
 #include "tensor.H"
 #include "word.H"
 #include "Enum.H"
+#include "stdFoam.H"  // For deprecation macros
 
 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
@@ -141,7 +142,7 @@ public:
 
     // Constructors
 
-        //- Construct zero initialized
+        //- Construct initialized to zero
         inline quaternion(const Foam::zero);
 
         //- Construct given scalar and vector parts
@@ -151,12 +152,12 @@ public:
         inline quaternion(const vector& d, const scalar theta);
 
         //- Construct a rotation quaternion given direction d
-        //- and cosine angle cosTheta and flag if d is normalized
+        //- and cosine angle cosTheta and flag if d is normalised
         inline quaternion
         (
             const vector& d,
             const scalar cosTheta,
-            const bool normalized
+            const bool isNormalised
         );
 
         //- Construct a real quaternion from the given scalar part,
@@ -185,33 +186,31 @@ public:
 
     // Access
 
-       //- Scalar part of the quaternion ( = cos(theta/2) for rotation)
-       inline scalar w() const;
+        //- Scalar part of the quaternion ( = cos(theta/2) for rotation)
+        inline scalar w() const noexcept;
 
-       //- Vector part of the quaternion ( = axis of rotation)
-       inline const vector& v() const;
+        //- Vector part of the quaternion ( = axis of rotation)
+        inline const vector& v() const noexcept;
 
-       //- The rotation tensor corresponding to the quaternion
-       inline tensor R() const;
+        //- The rotation tensor corresponding to the quaternion
+        inline tensor R() const;
 
-       //- Return the Euler rotation angles corresponding to the
-       //- specified rotation order
-       inline vector eulerAngles(const eulerOrder order) const;
-
-       //- Return the quaternion normalized by its magnitude
-       inline quaternion normalized() const;
+        //- Return the Euler rotation angles corresponding to the
+        //- specified rotation order
+        inline vector eulerAngles(const eulerOrder order) const;
 
 
     // Edit
 
-       //- Scalar part of the quaternion ( = cos(theta/2) for rotation)
-       inline scalar& w();
+        //- Scalar part of the quaternion ( = cos(theta/2) for rotation)
+        inline scalar& w() noexcept;
 
-       //- Vector part of the quaternion ( = axis of rotation)
-       inline vector& v();
+        //- Vector part of the quaternion ( = axis of rotation)
+        inline vector& v() noexcept;
 
-       //- Normalize the quaternion by its magnitude
-       inline void normalize();
+        //- Inplace normalise the quaternion by its magnitude
+        //  For small magnitudes (less than ROOTVSMALL) set to zero.
+        inline quaternion& normalise();
 
 
     // Transform
@@ -222,10 +221,10 @@ public:
        //- Rotate the given vector anti-clockwise
        inline vector invTransform(const vector& v) const;
 
-       //- Rotate the given quaternion (and normalize)
+       //- Rotate the given quaternion (and normalise)
        inline quaternion transform(const quaternion& q) const;
 
-       //- Rotate the given quaternion anti-clockwise (and normalize)
+       //- Rotate the given quaternion anti-clockwise (and normalise)
        inline quaternion invTransform(const quaternion& q) const;
 
 
@@ -242,8 +241,27 @@ public:
         //- Change vector portion only
         inline void operator=(const vector& v);
 
+        //- Assign scalar and vector to zero
+        inline void operator=(const Foam::zero);
+
         inline void operator*=(const scalar s);
         inline void operator/=(const scalar s);
+
+
+    // Housekeeping
+
+        //- Inplace normalise the quaternion by its magnitude
+        FOAM_DEPRECATED_FOR(2022-04, "normalise() method")
+        void normalize() { this->normalise(); }
+
+        //- Return the quaternion normalised by its magnitude
+        FOAM_DEPRECATED_FOR(2022-04, "Use Foam::normalised() function")
+        quaternion normalized() const
+        {
+            quaternion q(*this);
+            q.normalise();
+            return q;
+        }
 };
 
 
@@ -264,8 +282,12 @@ inline scalar mag(const quaternion& q);
 //- Return the conjugate of the given quaternion
 inline quaternion conjugate(const quaternion& q);
 
-//- Return the normalized (unit) quaternion of the given quaternion
-inline quaternion normalize(const quaternion& q);
+//- Return the normalised (unit) quaternion of the given quaternion
+inline quaternion normalised(const quaternion& q);
+
+//- Return the normalised (unit) quaternion of the given quaternion
+FOAM_DEPRECATED_FOR(2022-04, "Use Foam::normalised() function")
+inline quaternion normalize(const quaternion& q) { return Foam::normalised(q); }
 
 //- Return the inverse of the given quaternion
 inline quaternion inv(const quaternion& q);
diff --git a/src/OpenFOAM/primitives/quaternion/quaternionI.H b/src/OpenFOAM/primitives/quaternion/quaternionI.H
index 51efba620e..84592253e5 100644
--- a/src/OpenFOAM/primitives/quaternion/quaternionI.H
+++ b/src/OpenFOAM/primitives/quaternion/quaternionI.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2011-2016 OpenFOAM Foundation
-    Copyright (C) 2019-2020 OpenCFD Ltd.
+    Copyright (C) 2019-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -45,7 +45,7 @@ inline Foam::quaternion::quaternion(const scalar w, const vector& v)
 inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
 :
     w_(cos(0.5*theta)),
-    v_((sin(0.5*theta)/mag(d))*d)
+    v_(sin(0.5*theta)*Foam::normalised(d))
 {}
 
 
@@ -53,19 +53,19 @@ inline Foam::quaternion::quaternion
 (
     const vector& d,
     const scalar cosTheta,
-    const bool normalized
+    const bool isNormalised
 )
 {
-    scalar cosHalfTheta2 = 0.5*(cosTheta + 1);
+    const scalar cosHalfTheta2 = 0.5*(cosTheta + 1);
     w_ = sqrt(cosHalfTheta2);
 
-    if (normalized)
+    if (isNormalised)
     {
         v_ = sqrt(1 - cosHalfTheta2)*d;
     }
     else
     {
-        v_ = (sqrt(1 - cosHalfTheta2)/mag(d))*d;
+        v_ = sqrt(1 - cosHalfTheta2)*Foam::normalised(d);
     }
 }
 
@@ -282,39 +282,43 @@ inline Foam::quaternion::quaternion
 
 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 
-inline Foam::scalar Foam::quaternion::w() const
+inline Foam::scalar Foam::quaternion::w() const noexcept
 {
     return w_;
 }
 
 
-inline const Foam::vector& Foam::quaternion::v() const
+inline const Foam::vector& Foam::quaternion::v() const noexcept
 {
     return v_;
 }
 
 
-inline Foam::scalar& Foam::quaternion::w()
+inline Foam::scalar& Foam::quaternion::w() noexcept
 {
     return w_;
 }
 
 
-inline Foam::vector& Foam::quaternion::v()
+inline Foam::vector& Foam::quaternion::v() noexcept
 {
     return v_;
 }
 
 
-inline Foam::quaternion Foam::quaternion::normalized() const
+inline Foam::quaternion& Foam::quaternion::normalise()
 {
-    return *this/mag(*this);
-}
+    const scalar s(Foam::mag(*this));
 
-
-inline void Foam::quaternion::normalize()
-{
-    operator/=(mag(*this));
+    if (s < ROOTVSMALL)
+    {
+        *this = Zero;
+    }
+    else
+    {
+        *this /= s;
+    }
+    return *this;
 }
 
 
@@ -338,7 +342,7 @@ inline Foam::vector Foam::quaternion::invTransform(const vector& u) const
 
 inline Foam::quaternion Foam::quaternion::transform(const quaternion& q) const
 {
-    return Foam::normalize((*this)*q);
+    return Foam::normalised((*this)*q);
 }
 
 
@@ -347,7 +351,7 @@ inline Foam::quaternion Foam::quaternion::invTransform
     const quaternion& q
 ) const
 {
-    return Foam::normalize(conjugate(*this)*q);
+    return Foam::normalised(conjugate(*this)*q);
 }
 
 
@@ -618,6 +622,13 @@ inline void Foam::quaternion::operator=(const vector& v)
 }
 
 
+inline void Foam::quaternion::operator=(const Foam::zero)
+{
+    w_ = 0;
+    v_ = Zero;
+}
+
+
 inline void Foam::quaternion::operator*=(const scalar s)
 {
     w_ *= s;
@@ -653,14 +664,31 @@ inline Foam::quaternion Foam::conjugate(const quaternion& q)
 
 inline Foam::quaternion Foam::inv(const quaternion& q)
 {
-    scalar magSqrq = magSqr(q);
-    return quaternion(q.w()/magSqrq, -q.v()/magSqrq);
+    const scalar s(Foam::magSqr(q));
+
+    if (s < VSMALL)
+    {
+        return Zero;
+    }
+    else
+    {
+        return quaternion(q.w()/s, -q.v()/s);
+    }
 }
 
 
-inline Foam::quaternion Foam::normalize(const quaternion& q)
+inline Foam::quaternion Foam::normalised(const quaternion& q)
 {
-    return q/mag(q);
+    const scalar s(Foam::mag(q));
+
+    if (s < ROOTVSMALL)
+    {
+        return Zero;
+    }
+    else
+    {
+        return q/s;
+    }
 }
 
 
diff --git a/src/OpenFOAM/primitives/septernion/septernion.C b/src/OpenFOAM/primitives/septernion/septernion.C
index a291622c25..10bad87954 100644
--- a/src/OpenFOAM/primitives/septernion/septernion.C
+++ b/src/OpenFOAM/primitives/septernion/septernion.C
@@ -94,7 +94,7 @@ Foam::septernion Foam::average
         }
     }
 
-    sa.r().normalize();
+    sa.r().normalise();
 
     return sa;
 }
diff --git a/src/OpenFOAM/primitives/triad/triad.C b/src/OpenFOAM/primitives/triad/triad.C
index 018e721c0f..f8036b7b50 100644
--- a/src/OpenFOAM/primitives/triad/triad.C
+++ b/src/OpenFOAM/primitives/triad/triad.C
@@ -99,7 +99,7 @@ Foam::triad::triad(const quaternion& q)
 
 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 
-void Foam::triad::orthogonalize()
+void Foam::triad::orthogonalise()
 {
     // Hack for 2D z-slab cases
     // if (!set(2))
diff --git a/src/OpenFOAM/primitives/triad/triad.H b/src/OpenFOAM/primitives/triad/triad.H
index b158b42531..8ed84b6bce 100644
--- a/src/OpenFOAM/primitives/triad/triad.H
+++ b/src/OpenFOAM/primitives/triad/triad.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2012-2016 OpenFOAM Foundation
-    Copyright (C) 2018-2019 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -40,8 +40,8 @@ SourceFiles
 
 \*---------------------------------------------------------------------------*/
 
-#ifndef triad_H
-#define triad_H
+#ifndef Foam_triad_H
+#define Foam_triad_H
 
 #include "vector.H"
 #include "tensor.H"
@@ -70,7 +70,7 @@ public:
 
     // Constructors
 
-        //- Construct null
+        //- Default construct
         inline triad();
 
         //- Construct from components
@@ -112,11 +112,12 @@ public:
         //- Return the vector orthogonal to the two provided
         static inline vector orthogonal(const vector& v1, const vector& v2);
 
-        //- Orthogonalize this triad so that it is ortho-normal
-        void orthogonalize();
+        //- Inplace orthogonalise so that it is ortho-normal
+        void orthogonalise();
 
-        //- Normalize each set axis vector to have a unit magnitude
-        void normalize();
+        //- Normalise each set axis vector to have a unit magnitude
+        //  Uses vector::normalise with handling of small magnitudes.
+        void normalise();
 
         //- Align this triad with the given vector v
         //  by rotating the most aligned axis to be coincident with v
@@ -151,7 +152,7 @@ public:
         inline void operator=(const tensor& t);
 
         //- Add the triad t2 to this triad
-        //  without normalizing or orthogonalizing
+        //  without normalising or orthogonalising
         void operator+=(const triad& t2);
 
 
@@ -159,6 +160,15 @@ public:
 
         friend Istream& operator>>(Istream&, triad&);
         friend Ostream& operator<<(Ostream&, const triad&);
+
+
+    // Housekeeping
+
+        //- Same as orthogonalise
+        void orthogonalize() { this->orthogonalise(); }
+
+        //- Same as normalise
+        void normalize() { this->normalise(); }
 };
 
 
diff --git a/src/OpenFOAM/primitives/triad/triadI.H b/src/OpenFOAM/primitives/triad/triadI.H
index 3cde6246b1..8debae6ce1 100644
--- a/src/OpenFOAM/primitives/triad/triadI.H
+++ b/src/OpenFOAM/primitives/triad/triadI.H
@@ -6,7 +6,7 @@
      \\/     M anipulation  |
 -------------------------------------------------------------------------------
     Copyright (C) 2012-2016 OpenFOAM Foundation
-    Copyright (C) 2018 OpenCFD Ltd.
+    Copyright (C) 2018-2022 OpenCFD Ltd.
 -------------------------------------------------------------------------------
 License
     This file is part of OpenFOAM.
@@ -117,11 +117,11 @@ inline Foam::vector Foam::triad::orthogonal
 }
 
 
-inline void Foam::triad::normalize()
+inline void Foam::triad::normalise()
 {
-    if (set(0)) operator[](0) /= mag(operator[](0));
-    if (set(1)) operator[](1) /= mag(operator[](1));
-    if (set(2)) operator[](2) /= mag(operator[](2));
+    if (set(0)) (*this)[0].normalise();
+    if (set(1)) (*this)[1].normalise();
+    if (set(2)) (*this)[2].normalise();
 }
 
 
diff --git a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
index a4f9579133..1a08097d7b 100644
--- a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
+++ b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
@@ -78,8 +78,9 @@ void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
                     // Transform the previous time unit quaternion
                     quaternion quat
                     (
-                        normalize(model_.joints()[i].unitQuaternion(q0())*dQuat)
+                        model_.joints()[i].unitQuaternion(q0())*dQuat
                     );
+                    quat.normalise();
 
                     // Update the joint unit quaternion
                     model_.joints()[i].unitQuaternion(quat, q());