vhacd: Recommit unmodified upstream code without style changes

Godot-specific changes will then be redone without touching upstream formatting.
Also documented current state in thirdparty/README.md and added LICENSE.

Add vhacd to COPYRIGHT.txt.

6 files changed, 1247 insertions, 1200 deletions

diff --git a/thirdparty/vhacd/inc/btAlignedAllocator.h b/thirdparty/vhacd/inc/btAlignedAllocator.h
index 4f534e43f0..11f6e12dca 100644
--- a/thirdparty/vhacd/inc/btAlignedAllocator.h
+++ b/thirdparty/vhacd/inc/btAlignedAllocator.h
@@ -21,27 +21,22 @@ subject to the following restrictions:
 ///that is better portable and more predictable
 
 #include "btScalar.h"
-
-//GODOT ADDITION
-namespace VHACD {
-//
-
 //#define BT_DEBUG_MEMORY_ALLOCATIONS 1
 #ifdef BT_DEBUG_MEMORY_ALLOCATIONS
 
 #define btAlignedAlloc(a, b) \
-	btAlignedAllocInternal(a, b, __LINE__, __FILE__)
+    btAlignedAllocInternal(a, b, __LINE__, __FILE__)
 
 #define btAlignedFree(ptr) \
-	btAlignedFreeInternal(ptr, __LINE__, __FILE__)
+    btAlignedFreeInternal(ptr, __LINE__, __FILE__)
 
-void *btAlignedAllocInternal(size_t size, int32_t alignment, int32_t line, char *filename);
+void* btAlignedAllocInternal(size_t size, int32_t alignment, int32_t line, char* filename);
 
-void btAlignedFreeInternal(void *ptr, int32_t line, char *filename);
+void btAlignedFreeInternal(void* ptr, int32_t line, char* filename);
 
 #else
-void *btAlignedAllocInternal(size_t size, int32_t alignment);
-void btAlignedFreeInternal(void *ptr);
+void* btAlignedAllocInternal(size_t size, int32_t alignment);
+void btAlignedFreeInternal(void* ptr);
 
 #define btAlignedAlloc(size, alignment) btAlignedAllocInternal(size, alignment)
 #define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
@@ -49,63 +44,61 @@ void btAlignedFreeInternal(void *ptr);
 #endif
 typedef int32_t size_type;
 
-typedef void *(btAlignedAllocFunc)(size_t size, int32_t alignment);
-typedef void(btAlignedFreeFunc)(void *memblock);
-typedef void *(btAllocFunc)(size_t size);
-typedef void(btFreeFunc)(void *memblock);
+typedef void*(btAlignedAllocFunc)(size_t size, int32_t alignment);
+typedef void(btAlignedFreeFunc)(void* memblock);
+typedef void*(btAllocFunc)(size_t size);
+typedef void(btFreeFunc)(void* memblock);
 
 ///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
-void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+void btAlignedAllocSetCustom(btAllocFunc* allocFunc, btFreeFunc* freeFunc);
 ///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
-void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc* allocFunc, btAlignedFreeFunc* freeFunc);
 
 ///The btAlignedAllocator is a portable class for aligned memory allocations.
 ///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
 template <typename T, unsigned Alignment>
 class btAlignedAllocator {
 
-	typedef btAlignedAllocator<T, Alignment> self_type;
+    typedef btAlignedAllocator<T, Alignment> self_type;
 
 public:
-	//just going down a list:
-	btAlignedAllocator() {}
-	/*
+    //just going down a list:
+    btAlignedAllocator() {}
+    /*
 	btAlignedAllocator( const self_type & ) {}
 	*/
 
-	template <typename Other>
-	btAlignedAllocator(const btAlignedAllocator<Other, Alignment> &) {}
-
-	typedef const T *const_pointer;
-	typedef const T &const_reference;
-	typedef T *pointer;
-	typedef T &reference;
-	typedef T value_type;
-
-	pointer address(reference ref) const { return &ref; }
-	const_pointer address(const_reference ref) const { return &ref; }
-	pointer allocate(size_type n, const_pointer *hint = 0) {
-		(void)hint;
-		return reinterpret_cast<pointer>(btAlignedAlloc(sizeof(value_type) * n, Alignment));
-	}
-	void construct(pointer ptr, const value_type &value) { new (ptr) value_type(value); }
-	void deallocate(pointer ptr) {
-		btAlignedFree(reinterpret_cast<void *>(ptr));
-	}
-	void destroy(pointer ptr) { ptr->~value_type(); }
-
-	template <typename O>
-	struct rebind {
-		typedef btAlignedAllocator<O, Alignment> other;
-	};
-	template <typename O>
-	self_type &operator=(const btAlignedAllocator<O, Alignment> &) { return *this; }
-
-	friend bool operator==(const self_type &, const self_type &) { return true; }
+    template <typename Other>
+    btAlignedAllocator(const btAlignedAllocator<Other, Alignment>&) {}
+
+    typedef const T* const_pointer;
+    typedef const T& const_reference;
+    typedef T* pointer;
+    typedef T& reference;
+    typedef T value_type;
+
+    pointer address(reference ref) const { return &ref; }
+    const_pointer address(const_reference ref) const { return &ref; }
+    pointer allocate(size_type n, const_pointer* hint = 0)
+    {
+        (void)hint;
+        return reinterpret_cast<pointer>(btAlignedAlloc(sizeof(value_type) * n, Alignment));
+    }
+    void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
+    void deallocate(pointer ptr)
+    {
+        btAlignedFree(reinterpret_cast<void*>(ptr));
+    }
+    void destroy(pointer ptr) { ptr->~value_type(); }
+
+    template <typename O>
+    struct rebind {
+        typedef btAlignedAllocator<O, Alignment> other;
+    };
+    template <typename O>
+    self_type& operator=(const btAlignedAllocator<O, Alignment>&) { return *this; }
+
+    friend bool operator==(const self_type&, const self_type&) { return true; }
 };
 
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #endif //BT_ALIGNED_ALLOCATOR
diff --git a/thirdparty/vhacd/inc/btAlignedObjectArray.h b/thirdparty/vhacd/inc/btAlignedObjectArray.h
index d82449e8fd..e6620adf6f 100644
--- a/thirdparty/vhacd/inc/btAlignedObjectArray.h
+++ b/thirdparty/vhacd/inc/btAlignedObjectArray.h
@@ -38,383 +38,411 @@ subject to the following restrictions:
 #include <new> //for placement new
 #endif //BT_USE_PLACEMENT_NEW
 
-//GODOT ADDITION
-namespace VHACD {
-//
-
 ///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
 ///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
 template <typename T>
 //template <class T>
 class btAlignedObjectArray {
-	btAlignedAllocator<T, 16> m_allocator;
+    btAlignedAllocator<T, 16> m_allocator;
 
-	int32_t m_size;
-	int32_t m_capacity;
-	T *m_data;
-	//PCK: added this line
-	bool m_ownsMemory;
+    int32_t m_size;
+    int32_t m_capacity;
+    T* m_data;
+    //PCK: added this line
+    bool m_ownsMemory;
 
 #ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
 public:
-	SIMD_FORCE_INLINE btAlignedObjectArray<T> &operator=(const btAlignedObjectArray<T> &other) {
-		copyFromArray(other);
-		return *this;
-	}
+    SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other)
+    {
+        copyFromArray(other);
+        return *this;
+    }
 #else //BT_ALLOW_ARRAY_COPY_OPERATOR
 private:
-	SIMD_FORCE_INLINE btAlignedObjectArray<T> &operator=(const btAlignedObjectArray<T> &other);
+    SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other);
 #endif //BT_ALLOW_ARRAY_COPY_OPERATOR
 
 protected:
-	SIMD_FORCE_INLINE int32_t allocSize(int32_t size) {
-		return (size ? size * 2 : 1);
-	}
-	SIMD_FORCE_INLINE void copy(int32_t start, int32_t end, T *dest) const {
-		int32_t i;
-		for (i = start; i < end; ++i)
+    SIMD_FORCE_INLINE int32_t allocSize(int32_t size)
+    {
+        return (size ? size * 2 : 1);
+    }
+    SIMD_FORCE_INLINE void copy(int32_t start, int32_t end, T* dest) const
+    {
+        int32_t i;
+        for (i = start; i < end; ++i)
 #ifdef BT_USE_PLACEMENT_NEW
-			new (&dest[i]) T(m_data[i]);
+            new (&dest[i]) T(m_data[i]);
 #else
-			dest[i] = m_data[i];
+            dest[i] = m_data[i];
 #endif //BT_USE_PLACEMENT_NEW
-	}
-
-	SIMD_FORCE_INLINE void init() {
-		//PCK: added this line
-		m_ownsMemory = true;
-		m_data = 0;
-		m_size = 0;
-		m_capacity = 0;
-	}
-	SIMD_FORCE_INLINE void destroy(int32_t first, int32_t last) {
-		int32_t i;
-		for (i = first; i < last; i++) {
-			m_data[i].~T();
-		}
-	}
-
-	SIMD_FORCE_INLINE void *allocate(int32_t size) {
-		if (size)
-			return m_allocator.allocate(size);
-		return 0;
-	}
-
-	SIMD_FORCE_INLINE void deallocate() {
-		if (m_data) {
-			//PCK: enclosed the deallocation in this block
-			if (m_ownsMemory) {
-				m_allocator.deallocate(m_data);
-			}
-			m_data = 0;
-		}
-	}
+    }
+
+    SIMD_FORCE_INLINE void init()
+    {
+        //PCK: added this line
+        m_ownsMemory = true;
+        m_data = 0;
+        m_size = 0;
+        m_capacity = 0;
+    }
+    SIMD_FORCE_INLINE void destroy(int32_t first, int32_t last)
+    {
+        int32_t i;
+        for (i = first; i < last; i++) {
+            m_data[i].~T();
+        }
+    }
+
+    SIMD_FORCE_INLINE void* allocate(int32_t size)
+    {
+        if (size)
+            return m_allocator.allocate(size);
+        return 0;
+    }
+
+    SIMD_FORCE_INLINE void deallocate()
+    {
+        if (m_data) {
+            //PCK: enclosed the deallocation in this block
+            if (m_ownsMemory) {
+                m_allocator.deallocate(m_data);
+            }
+            m_data = 0;
+        }
+    }
 
 public:
-	btAlignedObjectArray() {
-		init();
-	}
-
-	~btAlignedObjectArray() {
-		clear();
-	}
-
-	///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
-	btAlignedObjectArray(const btAlignedObjectArray &otherArray) {
-		init();
-
-		int32_t otherSize = otherArray.size();
-		resize(otherSize);
-		otherArray.copy(0, otherSize, m_data);
-	}
-
-	/// return the number of elements in the array
-	SIMD_FORCE_INLINE int32_t size() const {
-		return m_size;
-	}
-
-	SIMD_FORCE_INLINE const T &at(int32_t n) const {
-		btAssert(n >= 0);
-		btAssert(n < size());
-		return m_data[n];
-	}
-
-	SIMD_FORCE_INLINE T &at(int32_t n) {
-		btAssert(n >= 0);
-		btAssert(n < size());
-		return m_data[n];
-	}
-
-	SIMD_FORCE_INLINE const T &operator[](int32_t n) const {
-		btAssert(n >= 0);
-		btAssert(n < size());
-		return m_data[n];
-	}
-
-	SIMD_FORCE_INLINE T &operator[](int32_t n) {
-		btAssert(n >= 0);
-		btAssert(n < size());
-		return m_data[n];
-	}
-
-	///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
-	SIMD_FORCE_INLINE void clear() {
-		destroy(0, size());
-
-		deallocate();
-
-		init();
-	}
-
-	SIMD_FORCE_INLINE void pop_back() {
-		btAssert(m_size > 0);
-		m_size--;
-		m_data[m_size].~T();
-	}
-
-	///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
-	///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
-	SIMD_FORCE_INLINE void resize(int32_t newsize, const T &fillData = T()) {
-		int32_t curSize = size();
-
-		if (newsize < curSize) {
-			for (int32_t i = newsize; i < curSize; i++) {
-				m_data[i].~T();
-			}
-		} else {
-			if (newsize > size()) {
-				reserve(newsize);
-			}
+    btAlignedObjectArray()
+    {
+        init();
+    }
+
+    ~btAlignedObjectArray()
+    {
+        clear();
+    }
+
+    ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
+    btAlignedObjectArray(const btAlignedObjectArray& otherArray)
+    {
+        init();
+
+        int32_t otherSize = otherArray.size();
+        resize(otherSize);
+        otherArray.copy(0, otherSize, m_data);
+    }
+
+    /// return the number of elements in the array
+    SIMD_FORCE_INLINE int32_t size() const
+    {
+        return m_size;
+    }
+
+    SIMD_FORCE_INLINE const T& at(int32_t n) const
+    {
+        btAssert(n >= 0);
+        btAssert(n < size());
+        return m_data[n];
+    }
+
+    SIMD_FORCE_INLINE T& at(int32_t n)
+    {
+        btAssert(n >= 0);
+        btAssert(n < size());
+        return m_data[n];
+    }
+
+    SIMD_FORCE_INLINE const T& operator[](int32_t n) const
+    {
+        btAssert(n >= 0);
+        btAssert(n < size());
+        return m_data[n];
+    }
+
+    SIMD_FORCE_INLINE T& operator[](int32_t n)
+    {
+        btAssert(n >= 0);
+        btAssert(n < size());
+        return m_data[n];
+    }
+
+    ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+    SIMD_FORCE_INLINE void clear()
+    {
+        destroy(0, size());
+
+        deallocate();
+
+        init();
+    }
+
+    SIMD_FORCE_INLINE void pop_back()
+    {
+        btAssert(m_size > 0);
+        m_size--;
+        m_data[m_size].~T();
+    }
+
+    ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+    ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+    SIMD_FORCE_INLINE void resize(int32_t newsize, const T& fillData = T())
+    {
+        int32_t curSize = size();
+
+        if (newsize < curSize) {
+            for (int32_t i = newsize; i < curSize; i++) {
+                m_data[i].~T();
+            }
+        }
+        else {
+            if (newsize > size()) {
+                reserve(newsize);
+            }
 #ifdef BT_USE_PLACEMENT_NEW
-			for (int32_t i = curSize; i < newsize; i++) {
-				new (&m_data[i]) T(fillData);
-			}
+            for (int32_t i = curSize; i < newsize; i++) {
+                new (&m_data[i]) T(fillData);
+            }
 #endif //BT_USE_PLACEMENT_NEW
-		}
-
-		m_size = newsize;
-	}
-
-	SIMD_FORCE_INLINE T &expandNonInitializing() {
-		int32_t sz = size();
-		if (sz == capacity()) {
-			reserve(allocSize(size()));
-		}
-		m_size++;
-
-		return m_data[sz];
-	}
-
-	SIMD_FORCE_INLINE T &expand(const T &fillValue = T()) {
-		int32_t sz = size();
-		if (sz == capacity()) {
-			reserve(allocSize(size()));
-		}
-		m_size++;
+        }
+
+        m_size = newsize;
+    }
+
+    SIMD_FORCE_INLINE T& expandNonInitializing()
+    {
+        int32_t sz = size();
+        if (sz == capacity()) {
+            reserve(allocSize(size()));
+        }
+        m_size++;
+
+        return m_data[sz];
+    }
+
+    SIMD_FORCE_INLINE T& expand(const T& fillValue = T())
+    {
+        int32_t sz = size();
+        if (sz == capacity()) {
+            reserve(allocSize(size()));
+        }
+        m_size++;
 #ifdef BT_USE_PLACEMENT_NEW
-		new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+        new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
 #endif
 
-		return m_data[sz];
-	}
+        return m_data[sz];
+    }
 
-	SIMD_FORCE_INLINE void push_back(const T &_Val) {
-		int32_t sz = size();
-		if (sz == capacity()) {
-			reserve(allocSize(size()));
-		}
+    SIMD_FORCE_INLINE void push_back(const T& _Val)
+    {
+        int32_t sz = size();
+        if (sz == capacity()) {
+            reserve(allocSize(size()));
+        }
 
 #ifdef BT_USE_PLACEMENT_NEW
-		new (&m_data[m_size]) T(_Val);
+        new (&m_data[m_size]) T(_Val);
 #else
-		m_data[size()] = _Val;
+        m_data[size()] = _Val;
 #endif //BT_USE_PLACEMENT_NEW
 
-		m_size++;
-	}
-
-	/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
-	SIMD_FORCE_INLINE int32_t capacity() const {
-		return m_capacity;
-	}
-
-	SIMD_FORCE_INLINE void reserve(int32_t _Count) { // determine new minimum length of allocated storage
-		if (capacity() < _Count) { // not enough room, reallocate
-			T *s = (T *)allocate(_Count);
-
-			copy(0, size(), s);
-
-			destroy(0, size());
-
-			deallocate();
-
-			//PCK: added this line
-			m_ownsMemory = true;
-
-			m_data = s;
-
-			m_capacity = _Count;
-		}
-	}
-
-	class less {
-	public:
-		bool operator()(const T &a, const T &b) {
-			return (a < b);
-		}
-	};
-
-	template <typename L>
-	void quickSortInternal(const L &CompareFunc, int32_t lo, int32_t hi) {
-		//  lo is the lower index, hi is the upper index
-		//  of the region of array a that is to be sorted
-		int32_t i = lo, j = hi;
-		T x = m_data[(lo + hi) / 2];
-
-		//  partition
-		do {
-			while (CompareFunc(m_data[i], x))
-				i++;
-			while (CompareFunc(x, m_data[j]))
-				j--;
-			if (i <= j) {
-				swap(i, j);
-				i++;
-				j--;
-			}
-		} while (i <= j);
-
-		//  recursion
-		if (lo < j)
-			quickSortInternal(CompareFunc, lo, j);
-		if (i < hi)
-			quickSortInternal(CompareFunc, i, hi);
-	}
-
-	template <typename L>
-	void quickSort(const L &CompareFunc) {
-		//don't sort 0 or 1 elements
-		if (size() > 1) {
-			quickSortInternal(CompareFunc, 0, size() - 1);
-		}
-	}
-
-	///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
-	template <typename L>
-	void downHeap(T *pArr, int32_t k, int32_t n, const L &CompareFunc) {
-		/*  PRE: a[k+1..N] is a heap */
-		/* POST:  a[k..N]  is a heap */
-
-		T temp = pArr[k - 1];
-		/* k has child(s) */
-		while (k <= n / 2) {
-			int32_t child = 2 * k;
-
-			if ((child < n) && CompareFunc(pArr[child - 1], pArr[child])) {
-				child++;
-			}
-			/* pick larger child */
-			if (CompareFunc(temp, pArr[child - 1])) {
-				/* move child up */
-				pArr[k - 1] = pArr[child - 1];
-				k = child;
-			} else {
-				break;
-			}
-		}
-		pArr[k - 1] = temp;
-	} /*downHeap*/
-
-	void swap(int32_t index0, int32_t index1) {
+        m_size++;
+    }
+
+    /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+    SIMD_FORCE_INLINE int32_t capacity() const
+    {
+        return m_capacity;
+    }
+
+    SIMD_FORCE_INLINE void reserve(int32_t _Count)
+    { // determine new minimum length of allocated storage
+        if (capacity() < _Count) { // not enough room, reallocate
+            T* s = (T*)allocate(_Count);
+
+            copy(0, size(), s);
+
+            destroy(0, size());
+
+            deallocate();
+
+            //PCK: added this line
+            m_ownsMemory = true;
+
+            m_data = s;
+
+            m_capacity = _Count;
+        }
+    }
+
+    class less {
+    public:
+        bool operator()(const T& a, const T& b)
+        {
+            return (a < b);
+        }
+    };
+
+    template <typename L>
+    void quickSortInternal(const L& CompareFunc, int32_t lo, int32_t hi)
+    {
+        //  lo is the lower index, hi is the upper index
+        //  of the region of array a that is to be sorted
+        int32_t i = lo, j = hi;
+        T x = m_data[(lo + hi) / 2];
+
+        //  partition
+        do {
+            while (CompareFunc(m_data[i], x))
+                i++;
+            while (CompareFunc(x, m_data[j]))
+                j--;
+            if (i <= j) {
+                swap(i, j);
+                i++;
+                j--;
+            }
+        } while (i <= j);
+
+        //  recursion
+        if (lo < j)
+            quickSortInternal(CompareFunc, lo, j);
+        if (i < hi)
+            quickSortInternal(CompareFunc, i, hi);
+    }
+
+    template <typename L>
+    void quickSort(const L& CompareFunc)
+    {
+        //don't sort 0 or 1 elements
+        if (size() > 1) {
+            quickSortInternal(CompareFunc, 0, size() - 1);
+        }
+    }
+
+    ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+    template <typename L>
+    void downHeap(T* pArr, int32_t k, int32_t n, const L& CompareFunc)
+    {
+        /*  PRE: a[k+1..N] is a heap */
+        /* POST:  a[k..N]  is a heap */
+
+        T temp = pArr[k - 1];
+        /* k has child(s) */
+        while (k <= n / 2) {
+            int32_t child = 2 * k;
+
+            if ((child < n) && CompareFunc(pArr[child - 1], pArr[child])) {
+                child++;
+            }
+            /* pick larger child */
+            if (CompareFunc(temp, pArr[child - 1])) {
+                /* move child up */
+                pArr[k - 1] = pArr[child - 1];
+                k = child;
+            }
+            else {
+                break;
+            }
+        }
+        pArr[k - 1] = temp;
+    } /*downHeap*/
+
+    void swap(int32_t index0, int32_t index1)
+    {
 #ifdef BT_USE_MEMCPY
-		char temp[sizeof(T)];
-		memcpy(temp, &m_data[index0], sizeof(T));
-		memcpy(&m_data[index0], &m_data[index1], sizeof(T));
-		memcpy(&m_data[index1], temp, sizeof(T));
+        char temp[sizeof(T)];
+        memcpy(temp, &m_data[index0], sizeof(T));
+        memcpy(&m_data[index0], &m_data[index1], sizeof(T));
+        memcpy(&m_data[index1], temp, sizeof(T));
 #else
-		T temp = m_data[index0];
-		m_data[index0] = m_data[index1];
-		m_data[index1] = temp;
+        T temp = m_data[index0];
+        m_data[index0] = m_data[index1];
+        m_data[index1] = temp;
 #endif //BT_USE_PLACEMENT_NEW
-	}
-
-	template <typename L>
-	void heapSort(const L &CompareFunc) {
-		/* sort a[0..N-1],  N.B. 0 to N-1 */
-		int32_t k;
-		int32_t n = m_size;
-		for (k = n / 2; k > 0; k--) {
-			downHeap(m_data, k, n, CompareFunc);
-		}
-
-		/* a[1..N] is now a heap */
-		while (n >= 1) {
-			swap(0, n - 1); /* largest of a[0..n-1] */
-
-			n = n - 1;
-			/* restore a[1..i-1] heap */
-			downHeap(m_data, 1, n, CompareFunc);
-		}
-	}
-
-	///non-recursive binary search, assumes sorted array
-	int32_t findBinarySearch(const T &key) const {
-		int32_t first = 0;
-		int32_t last = size() - 1;
-
-		//assume sorted array
-		while (first <= last) {
-			int32_t mid = (first + last) / 2; // compute mid point.
-			if (key > m_data[mid])
-				first = mid + 1; // repeat search in top half.
-			else if (key < m_data[mid])
-				last = mid - 1; // repeat search in bottom half.
-			else
-				return mid; // found it. return position /////
-		}
-		return size(); // failed to find key
-	}
-
-	int32_t findLinearSearch(const T &key) const {
-		int32_t index = size();
-		int32_t i;
-
-		for (i = 0; i < size(); i++) {
-			if (m_data[i] == key) {
-				index = i;
-				break;
-			}
-		}
-		return index;
-	}
-
-	void remove(const T &key) {
-
-		int32_t findIndex = findLinearSearch(key);
-		if (findIndex < size()) {
-			swap(findIndex, size() - 1);
-			pop_back();
-		}
-	}
-
-	//PCK: whole function
-	void initializeFromBuffer(void *buffer, int32_t size, int32_t capacity) {
-		clear();
-		m_ownsMemory = false;
-		m_data = (T *)buffer;
-		m_size = size;
-		m_capacity = capacity;
-	}
-
-	void copyFromArray(const btAlignedObjectArray &otherArray) {
-		int32_t otherSize = otherArray.size();
-		resize(otherSize);
-		otherArray.copy(0, otherSize, m_data);
-	}
+    }
+
+    template <typename L>
+    void heapSort(const L& CompareFunc)
+    {
+        /* sort a[0..N-1],  N.B. 0 to N-1 */
+        int32_t k;
+        int32_t n = m_size;
+        for (k = n / 2; k > 0; k--) {
+            downHeap(m_data, k, n, CompareFunc);
+        }
+
+        /* a[1..N] is now a heap */
+        while (n >= 1) {
+            swap(0, n - 1); /* largest of a[0..n-1] */
+
+            n = n - 1;
+            /* restore a[1..i-1] heap */
+            downHeap(m_data, 1, n, CompareFunc);
+        }
+    }
+
+    ///non-recursive binary search, assumes sorted array
+    int32_t findBinarySearch(const T& key) const
+    {
+        int32_t first = 0;
+        int32_t last = size() - 1;
+
+        //assume sorted array
+        while (first <= last) {
+            int32_t mid = (first + last) / 2; // compute mid point.
+            if (key > m_data[mid])
+                first = mid + 1; // repeat search in top half.
+            else if (key < m_data[mid])
+                last = mid - 1; // repeat search in bottom half.
+            else
+                return mid; // found it. return position /////
+        }
+        return size(); // failed to find key
+    }
+
+    int32_t findLinearSearch(const T& key) const
+    {
+        int32_t index = size();
+        int32_t i;
+
+        for (i = 0; i < size(); i++) {
+            if (m_data[i] == key) {
+                index = i;
+                break;
+            }
+        }
+        return index;
+    }
+
+    void remove(const T& key)
+    {
+
+        int32_t findIndex = findLinearSearch(key);
+        if (findIndex < size()) {
+            swap(findIndex, size() - 1);
+            pop_back();
+        }
+    }
+
+    //PCK: whole function
+    void initializeFromBuffer(void* buffer, int32_t size, int32_t capacity)
+    {
+        clear();
+        m_ownsMemory = false;
+        m_data = (T*)buffer;
+        m_size = size;
+        m_capacity = capacity;
+    }
+
+    void copyFromArray(const btAlignedObjectArray& otherArray)
+    {
+        int32_t otherSize = otherArray.size();
+        resize(otherSize);
+        otherArray.copy(0, otherSize, m_data);
+    }
 };
 
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #endif //BT_OBJECT_ARRAY__
diff --git a/thirdparty/vhacd/inc/btConvexHullComputer.h b/thirdparty/vhacd/inc/btConvexHullComputer.h
index f10a3cf86f..3c5075c2cb 100644
--- a/thirdparty/vhacd/inc/btConvexHullComputer.h
+++ b/thirdparty/vhacd/inc/btConvexHullComputer.h
@@ -18,60 +18,59 @@ subject to the following restrictions:
 #include "btAlignedObjectArray.h"
 #include "btVector3.h"
 
-//GODOT ADDITION
-namespace VHACD {
-//
-
 /// Convex hull implementation based on Preparata and Hong
 /// See http://code.google.com/p/bullet/issues/detail?id=275
 /// Ole Kniemeyer, MAXON Computer GmbH
 class btConvexHullComputer {
 private:
-	btScalar compute(const void *coords, bool doubleCoords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp);
+    btScalar compute(const void* coords, bool doubleCoords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp);
 
 public:
-	class Edge {
-	private:
-		int32_t next;
-		int32_t reverse;
-		int32_t targetVertex;
-
-		friend class btConvexHullComputer;
-
-	public:
-		int32_t getSourceVertex() const {
-			return (this + reverse)->targetVertex;
-		}
-
-		int32_t getTargetVertex() const {
-			return targetVertex;
-		}
-
-		const Edge *getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
-		{
-			return this + next;
-		}
-
-		const Edge *getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
-		{
-			return (this + reverse)->getNextEdgeOfVertex();
-		}
-
-		const Edge *getReverseEdge() const {
-			return this + reverse;
-		}
-	};
-
-	// Vertices of the output hull
-	btAlignedObjectArray<btVector3> vertices;
-
-	// Edges of the output hull
-	btAlignedObjectArray<Edge> edges;
-
-	// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
-	btAlignedObjectArray<int32_t> faces;
-
-	/*
+    class Edge {
+    private:
+        int32_t next;
+        int32_t reverse;
+        int32_t targetVertex;
+
+        friend class btConvexHullComputer;
+
+    public:
+        int32_t getSourceVertex() const
+        {
+            return (this + reverse)->targetVertex;
+        }
+
+        int32_t getTargetVertex() const
+        {
+            return targetVertex;
+        }
+
+        const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
+        {
+            return this + next;
+        }
+
+        const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
+        {
+            return (this + reverse)->getNextEdgeOfVertex();
+        }
+
+        const Edge* getReverseEdge() const
+        {
+            return this + reverse;
+        }
+    };
+
+    // Vertices of the output hull
+    btAlignedObjectArray<btVector3> vertices;
+
+    // Edges of the output hull
+    btAlignedObjectArray<Edge> edges;
+
+    // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
+    btAlignedObjectArray<int32_t> faces;
+
+    /*
 		Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
 		between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
 		by that amount (each face is moved by "shrink" length units towards the center along its normal).
@@ -83,18 +82,16 @@ public:
 
 		The output convex hull can be found in the member variables "vertices", "edges", "faces".
 		*/
-	btScalar compute(const float *coords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp) {
-		return compute(coords, false, stride, count, shrink, shrinkClamp);
-	}
-
-	// same as above, but double precision
-	btScalar compute(const double *coords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp) {
-		return compute(coords, true, stride, count, shrink, shrinkClamp);
-	}
+    btScalar compute(const float* coords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp)
+    {
+        return compute(coords, false, stride, count, shrink, shrinkClamp);
+    }
+
+    // same as above, but double precision
+    btScalar compute(const double* coords, int32_t stride, int32_t count, btScalar shrink, btScalar shrinkClamp)
+    {
+        return compute(coords, true, stride, count, shrink, shrinkClamp);
+    }
 };
 
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #endif //BT_CONVEX_HULL_COMPUTER_H
diff --git a/thirdparty/vhacd/inc/btMinMax.h b/thirdparty/vhacd/inc/btMinMax.h
index a5be258667..40b0ceb6ed 100644
--- a/thirdparty/vhacd/inc/btMinMax.h
+++ b/thirdparty/vhacd/inc/btMinMax.h
@@ -17,50 +17,49 @@ subject to the following restrictions:
 
 #include "btScalar.h"
 
-//GODOT ADDITION
-namespace VHACD {
-//
-
 template <class T>
-SIMD_FORCE_INLINE const T &btMin(const T &a, const T &b) {
-	return a < b ? a : b;
+SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
+{
+    return a < b ? a : b;
 }
 
 template <class T>
-SIMD_FORCE_INLINE const T &btMax(const T &a, const T &b) {
-	return a > b ? a : b;
+SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
+{
+    return a > b ? a : b;
 }
 
 template <class T>
-SIMD_FORCE_INLINE const T &btClamped(const T &a, const T &lb, const T &ub) {
-	return a < lb ? lb : (ub < a ? ub : a);
+SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
+{
+    return a < lb ? lb : (ub < a ? ub : a);
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btSetMin(T &a, const T &b) {
-	if (b < a) {
-		a = b;
-	}
+SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
+{
+    if (b < a) {
+        a = b;
+    }
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btSetMax(T &a, const T &b) {
-	if (a < b) {
-		a = b;
-	}
+SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
+{
+    if (a < b) {
+        a = b;
+    }
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btClamp(T &a, const T &lb, const T &ub) {
-	if (a < lb) {
-		a = lb;
-	} else if (ub < a) {
-		a = ub;
-	}
+SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
+{
+    if (a < lb) {
+        a = lb;
+    }
+    else if (ub < a) {
+        a = ub;
+    }
 }
 
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #endif //BT_GEN_MINMAX_H
diff --git a/thirdparty/vhacd/inc/btScalar.h b/thirdparty/vhacd/inc/btScalar.h
index a9f836f9f2..b814474bdf 100644
--- a/thirdparty/vhacd/inc/btScalar.h
+++ b/thirdparty/vhacd/inc/btScalar.h
@@ -22,24 +22,17 @@ subject to the following restrictions:
 
 #include <float.h>
 #include <math.h>
-#include <stdint.h>
 #include <stdlib.h> //size_t for MSVC 6.0
+#include <stdint.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
 #define BT_BULLET_VERSION 279
 
-//GODOT ADDITION
-namespace VHACD {
-//
-
-inline int32_t btGetVersion() {
-	return BT_BULLET_VERSION;
+inline int32_t btGetVersion()
+{
+    return BT_BULLET_VERSION;
 }
 
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #if defined(DEBUG) || defined(_DEBUG)
 #define BT_DEBUG
 #endif
@@ -107,12 +100,12 @@ inline int32_t btGetVersion() {
 #include <spu_printf.h>
 #define printf spu_printf
 #define btAssert(x)                                                \
-	{                                                              \
-		if (!(x)) {                                                \
-			printf("Assert " __FILE__ ":%u (" #x ")\n", __LINE__); \
-			spu_hcmpeq(0, 0);                                      \
-		}                                                          \
-	}
+    {                                                              \
+        if (!(x)) {                                                \
+            printf("Assert " __FILE__ ":%u (" #x ")\n", __LINE__); \
+            spu_hcmpeq(0, 0);                                      \
+        }                                                          \
+    }
 #else
 #define btAssert assert
 #endif
@@ -206,10 +199,6 @@ inline int32_t btGetVersion() {
 #endif //__CELLOS_LV2__
 #endif
 
-//GODOT ADDITION
-namespace VHACD {
-//
-
 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
 #if defined(BT_USE_DOUBLE_PRECISION)
 typedef double btScalar;
@@ -222,130 +211,96 @@ typedef float btScalar;
 #endif
 
 #define BT_DECLARE_ALIGNED_ALLOCATOR()                                                                     \
-	SIMD_FORCE_INLINE void *operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); }   \
-	SIMD_FORCE_INLINE void operator delete(void *ptr) { btAlignedFree(ptr); }                              \
-	SIMD_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; }                                \
-	SIMD_FORCE_INLINE void operator delete(void *, void *) {}                                              \
-	SIMD_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \
-	SIMD_FORCE_INLINE void operator delete[](void *ptr) { btAlignedFree(ptr); }                            \
-	SIMD_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; }                              \
-	SIMD_FORCE_INLINE void operator delete[](void *, void *) {}
+    SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); }   \
+    SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); }                              \
+    SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; }                                \
+    SIMD_FORCE_INLINE void operator delete(void*, void*) {}                                                \
+    SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \
+    SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); }                            \
+    SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; }                              \
+    SIMD_FORCE_INLINE void operator delete[](void*, void*) {}
 
 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
 
-SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) {
-	return sqrt(x);
-}
-SIMD_FORCE_INLINE btScalar btFabs(btScalar x) {
-	return fabs(x);
-}
-SIMD_FORCE_INLINE btScalar btCos(btScalar x) {
-	return cos(x);
-}
-SIMD_FORCE_INLINE btScalar btSin(btScalar x) {
-	return sin(x);
-}
-SIMD_FORCE_INLINE btScalar btTan(btScalar x) {
-	return tan(x);
-}
-SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
-	if (x < btScalar(-1))
-		x = btScalar(-1);
-	if (x > btScalar(1))
-		x = btScalar(1);
-	return acos(x);
-}
-SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
-	if (x < btScalar(-1))
-		x = btScalar(-1);
-	if (x > btScalar(1))
-		x = btScalar(1);
-	return asin(x);
-}
-SIMD_FORCE_INLINE btScalar btAtan(btScalar x) {
-	return atan(x);
-}
-SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) {
-	return atan2(x, y);
-}
-SIMD_FORCE_INLINE btScalar btExp(btScalar x) {
-	return exp(x);
-}
-SIMD_FORCE_INLINE btScalar btLog(btScalar x) {
-	return log(x);
-}
-SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) {
-	return pow(x, y);
-}
-SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) {
-	return fmod(x, y);
-}
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar x)
+{
+    return sqrt(x);
+}
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
+{
+    if (x < btScalar(-1))
+        x = btScalar(-1);
+    if (x > btScalar(1))
+        x = btScalar(1);
+    return acos(x);
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
+{
+    if (x < btScalar(-1))
+        x = btScalar(-1);
+    if (x > btScalar(1))
+        x = btScalar(1);
+    return asin(x);
+}
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return pow(x, y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmod(x, y); }
 
 #else
 
-SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) {
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
+{
 #ifdef USE_APPROXIMATION
-	double x, z, tempf;
-	unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
-
-	tempf = y;
-	*tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
-	x = tempf;
-	z = y * btScalar(0.5);
-	x = (btScalar(1.5) * x) - (x * x) * (x * z); /* iteration formula     */
-	x = (btScalar(1.5) * x) - (x * x) * (x * z);
-	x = (btScalar(1.5) * x) - (x * x) * (x * z);
-	x = (btScalar(1.5) * x) - (x * x) * (x * z);
-	x = (btScalar(1.5) * x) - (x * x) * (x * z);
-	return x * y;
+    double x, z, tempf;
+    unsigned long* tfptr = ((unsigned long*)&tempf) + 1;
+
+    tempf = y;
+    *tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
+    x = tempf;
+    z = y * btScalar(0.5);
+    x = (btScalar(1.5) * x) - (x * x) * (x * z); /* iteration formula     */
+    x = (btScalar(1.5) * x) - (x * x) * (x * z);
+    x = (btScalar(1.5) * x) - (x * x) * (x * z);
+    x = (btScalar(1.5) * x) - (x * x) * (x * z);
+    x = (btScalar(1.5) * x) - (x * x) * (x * z);
+    return x * y;
 #else
-	return sqrtf(y);
+    return sqrtf(y);
 #endif
 }
-SIMD_FORCE_INLINE btScalar btFabs(btScalar x) {
-	return fabsf(x);
-}
-SIMD_FORCE_INLINE btScalar btCos(btScalar x) {
-	return cosf(x);
-}
-SIMD_FORCE_INLINE btScalar btSin(btScalar x) {
-	return sinf(x);
-}
-SIMD_FORCE_INLINE btScalar btTan(btScalar x) {
-	return tanf(x);
-}
-SIMD_FORCE_INLINE btScalar btAcos(btScalar x) {
-	if (x < btScalar(-1))
-		x = btScalar(-1);
-	if (x > btScalar(1))
-		x = btScalar(1);
-	return acosf(x);
-}
-SIMD_FORCE_INLINE btScalar btAsin(btScalar x) {
-	if (x < btScalar(-1))
-		x = btScalar(-1);
-	if (x > btScalar(1))
-		x = btScalar(1);
-	return asinf(x);
-}
-SIMD_FORCE_INLINE btScalar btAtan(btScalar x) {
-	return atanf(x);
-}
-SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) {
-	return atan2f(x, y);
-}
-SIMD_FORCE_INLINE btScalar btExp(btScalar x) {
-	return expf(x);
-}
-SIMD_FORCE_INLINE btScalar btLog(btScalar x) {
-	return logf(x);
-}
-SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) {
-	return powf(x, y);
-}
-SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) {
-	return fmodf(x, y);
-}
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
+{
+    if (x < btScalar(-1))
+        x = btScalar(-1);
+    if (x > btScalar(1))
+        x = btScalar(1);
+    return acosf(x);
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
+{
+    if (x < btScalar(-1))
+        x = btScalar(-1);
+    if (x > btScalar(1))
+        x = btScalar(1);
+    return asinf(x);
+}
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return powf(x, y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmodf(x, y); }
 
 #endif
 
@@ -366,110 +321,119 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) {
 #define SIMD_INFINITY FLT_MAX
 #endif
 
-SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) {
-	btScalar coeff_1 = SIMD_PI / 4.0f;
-	btScalar coeff_2 = 3.0f * coeff_1;
-	btScalar abs_y = btFabs(y);
-	btScalar angle;
-	if (x >= 0.0f) {
-		btScalar r = (x - abs_y) / (x + abs_y);
-		angle = coeff_1 - coeff_1 * r;
-	} else {
-		btScalar r = (x + abs_y) / (abs_y - x);
-		angle = coeff_2 - coeff_1 * r;
-	}
-	return (y < 0.0f) ? -angle : angle;
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
+{
+    btScalar coeff_1 = SIMD_PI / 4.0f;
+    btScalar coeff_2 = 3.0f * coeff_1;
+    btScalar abs_y = btFabs(y);
+    btScalar angle;
+    if (x >= 0.0f) {
+        btScalar r = (x - abs_y) / (x + abs_y);
+        angle = coeff_1 - coeff_1 * r;
+    }
+    else {
+        btScalar r = (x + abs_y) / (abs_y - x);
+        angle = coeff_2 - coeff_1 * r;
+    }
+    return (y < 0.0f) ? -angle : angle;
 }
 
-SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) {
-	return btFabs(x) < SIMD_EPSILON;
-}
+SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
 
-SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) {
-	return (((a) <= eps) && !((a) < -eps));
+SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps)
+{
+    return (((a) <= eps) && !((a) < -eps));
 }
-SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps) {
-	return (!((a) <= eps));
+SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps)
+{
+    return (!((a) <= eps));
 }
 
-SIMD_FORCE_INLINE int32_t btIsNegative(btScalar x) {
-	return x < btScalar(0.0) ? 1 : 0;
+SIMD_FORCE_INLINE int32_t btIsNegative(btScalar x)
+{
+    return x < btScalar(0.0) ? 1 : 0;
 }
 
-SIMD_FORCE_INLINE btScalar btRadians(btScalar x) {
-	return x * SIMD_RADS_PER_DEG;
-}
-SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) {
-	return x * SIMD_DEGS_PER_RAD;
-}
+SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
+SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
 
 #define BT_DECLARE_HANDLE(name) \
-	typedef struct name##__ {   \
-		int32_t unused;         \
-	} * name
+    typedef struct name##__ {   \
+        int32_t unused;             \
+    } * name
 
 #ifndef btFsel
-SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) {
-	return a >= 0 ? b : c;
+SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
+{
+    return a >= 0 ? b : c;
 }
 #endif
 #define btFsels(a, b, c) (btScalar) btFsel(a, b, c)
 
-SIMD_FORCE_INLINE bool btMachineIsLittleEndian() {
-	long int i = 1;
-	const char *p = (const char *)&i;
-	if (p[0] == 1) // Lowest address contains the least significant byte
-		return true;
-	else
-		return false;
+SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
+{
+    long int i = 1;
+    const char* p = (const char*)&i;
+    if (p[0] == 1) // Lowest address contains the least significant byte
+        return true;
+    else
+        return false;
 }
 
 ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
-SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) {
-	// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
-	// Rely on positive value or'ed with its negative having sign bit on
-	// and zero value or'ed with its negative (which is still zero) having sign bit off
-	// Use arithmetic shift right, shifting the sign bit through all 32 bits
-	unsigned testNz = (unsigned)(((int32_t)condition | -(int32_t)condition) >> 31);
-	unsigned testEqz = ~testNz;
-	return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
-}
-SIMD_FORCE_INLINE int32_t btSelect(unsigned condition, int32_t valueIfConditionNonZero, int32_t valueIfConditionZero) {
-	unsigned testNz = (unsigned)(((int32_t)condition | -(int32_t)condition) >> 31);
-	unsigned testEqz = ~testNz;
-	return static_cast<int32_t>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
-}
-SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) {
+SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
+{
+    // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+    // Rely on positive value or'ed with its negative having sign bit on
+    // and zero value or'ed with its negative (which is still zero) having sign bit off
+    // Use arithmetic shift right, shifting the sign bit through all 32 bits
+    unsigned testNz = (unsigned)(((int32_t)condition | -(int32_t)condition) >> 31);
+    unsigned testEqz = ~testNz;
+    return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE int32_t btSelect(unsigned condition, int32_t valueIfConditionNonZero, int32_t valueIfConditionZero)
+{
+    unsigned testNz = (unsigned)(((int32_t)condition | -(int32_t)condition) >> 31);
+    unsigned testEqz = ~testNz;
+    return static_cast<int32_t>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
 #ifdef BT_HAVE_NATIVE_FSEL
-	return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+    return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
 #else
-	return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
+    return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
 #endif
 }
 
 template <typename T>
-SIMD_FORCE_INLINE void btSwap(T &a, T &b) {
-	T tmp = a;
-	a = b;
-	b = tmp;
+SIMD_FORCE_INLINE void btSwap(T& a, T& b)
+{
+    T tmp = a;
+    a = b;
+    b = tmp;
 }
 
 //PCK: endian swapping functions
-SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) {
-	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
+SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
+{
+    return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
 }
 
-SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) {
-	return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
+SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
+{
+    return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
 }
 
-SIMD_FORCE_INLINE unsigned btSwapEndian(int32_t val) {
-	return btSwapEndian((unsigned)val);
+SIMD_FORCE_INLINE unsigned btSwapEndian(int32_t val)
+{
+    return btSwapEndian((unsigned)val);
 }
 
-SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) {
-	return btSwapEndian((unsigned short)val);
+SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
+{
+    return btSwapEndian((unsigned short)val);
 }
 
 ///btSwapFloat uses using char pointers to swap the endianness
@@ -478,88 +442,92 @@ SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) {
 ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
 ///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
 ///so instead of returning a float/double, we return integer/long long integer
-SIMD_FORCE_INLINE uint32_t btSwapEndianFloat(float d) {
-	uint32_t a = 0;
-	unsigned char *dst = (unsigned char *)&a;
-	unsigned char *src = (unsigned char *)&d;
-
-	dst[0] = src[3];
-	dst[1] = src[2];
-	dst[2] = src[1];
-	dst[3] = src[0];
-	return a;
+SIMD_FORCE_INLINE uint32_t btSwapEndianFloat(float d)
+{
+    uint32_t a = 0;
+    unsigned char* dst = (unsigned char*)&a;
+    unsigned char* src = (unsigned char*)&d;
+
+    dst[0] = src[3];
+    dst[1] = src[2];
+    dst[2] = src[1];
+    dst[3] = src[0];
+    return a;
 }
 
 // unswap using char pointers
-SIMD_FORCE_INLINE float btUnswapEndianFloat(uint32_t a) {
-	float d = 0.0f;
-	unsigned char *src = (unsigned char *)&a;
-	unsigned char *dst = (unsigned char *)&d;
+SIMD_FORCE_INLINE float btUnswapEndianFloat(uint32_t a)
+{
+    float d = 0.0f;
+    unsigned char* src = (unsigned char*)&a;
+    unsigned char* dst = (unsigned char*)&d;
 
-	dst[0] = src[3];
-	dst[1] = src[2];
-	dst[2] = src[1];
-	dst[3] = src[0];
+    dst[0] = src[3];
+    dst[1] = src[2];
+    dst[2] = src[1];
+    dst[3] = src[0];
 
-	return d;
+    return d;
 }
 
 // swap using char pointers
-SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char *dst) {
-	unsigned char *src = (unsigned char *)&d;
-
-	dst[0] = src[7];
-	dst[1] = src[6];
-	dst[2] = src[5];
-	dst[3] = src[4];
-	dst[4] = src[3];
-	dst[5] = src[2];
-	dst[6] = src[1];
-	dst[7] = src[0];
+SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
+{
+    unsigned char* src = (unsigned char*)&d;
+
+    dst[0] = src[7];
+    dst[1] = src[6];
+    dst[2] = src[5];
+    dst[3] = src[4];
+    dst[4] = src[3];
+    dst[5] = src[2];
+    dst[6] = src[1];
+    dst[7] = src[0];
 }
 
 // unswap using char pointers
-SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) {
-	double d = 0.0;
-	unsigned char *dst = (unsigned char *)&d;
-
-	dst[0] = src[7];
-	dst[1] = src[6];
-	dst[2] = src[5];
-	dst[3] = src[4];
-	dst[4] = src[3];
-	dst[5] = src[2];
-	dst[6] = src[1];
-	dst[7] = src[0];
-
-	return d;
+SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char* src)
+{
+    double d = 0.0;
+    unsigned char* dst = (unsigned char*)&d;
+
+    dst[0] = src[7];
+    dst[1] = src[6];
+    dst[2] = src[5];
+    dst[3] = src[4];
+    dst[4] = src[3];
+    dst[5] = src[2];
+    dst[6] = src[1];
+    dst[7] = src[0];
+
+    return d;
 }
 
 // returns normalized value in range [-SIMD_PI, SIMD_PI]
-SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) {
-	angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
-	if (angleInRadians < -SIMD_PI) {
-		return angleInRadians + SIMD_2_PI;
-	} else if (angleInRadians > SIMD_PI) {
-		return angleInRadians - SIMD_2_PI;
-	} else {
-		return angleInRadians;
-	}
+SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
+{
+    angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
+    if (angleInRadians < -SIMD_PI) {
+        return angleInRadians + SIMD_2_PI;
+    }
+    else if (angleInRadians > SIMD_PI) {
+        return angleInRadians - SIMD_2_PI;
+    }
+    else {
+        return angleInRadians;
+    }
 }
 
 ///rudimentary class to provide type info
 struct btTypedObject {
-	btTypedObject(int32_t objectType) :
-			m_objectType(objectType) {
-	}
-	int32_t m_objectType;
-	inline int32_t getObjectType() const {
-		return m_objectType;
-	}
+    btTypedObject(int32_t objectType)
+        : m_objectType(objectType)
+    {
+    }
+    int32_t m_objectType;
+    inline int32_t getObjectType() const
+    {
+        return m_objectType;
+    }
 };
-
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #endif //BT_SCALAR_H
diff --git a/thirdparty/vhacd/inc/btVector3.h b/thirdparty/vhacd/inc/btVector3.h
index 664728a419..0f2fefbbd5 100644
--- a/thirdparty/vhacd/inc/btVector3.h
+++ b/thirdparty/vhacd/inc/btVector3.h
@@ -30,387 +30,431 @@ subject to the following restrictions:
  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
  */
-//GODOT ADDITION
-namespace VHACD {
-//
-
 ATTRIBUTE_ALIGNED16(class)
-btVector3 {
+btVector3
+{
 public:
 #if defined(__SPU__) && defined(__CELLOS_LV2__)
-	btScalar m_floats[4];
+    btScalar m_floats[4];
 
 public:
-	SIMD_FORCE_INLINE const vec_float4 &get128() const {
-		return *((const vec_float4 *)&m_floats[0]);
-	}
+    SIMD_FORCE_INLINE const vec_float4& get128() const
+    {
+        return *((const vec_float4*)&m_floats[0]);
+    }
 
 public:
 #else //__CELLOS_LV2__ __SPU__
 #ifdef BT_USE_SSE // _WIN32
-	union {
-		__m128 mVec128;
-		btScalar m_floats[4];
-	};
-	SIMD_FORCE_INLINE __m128 get128() const {
-		return mVec128;
-	}
-	SIMD_FORCE_INLINE void set128(__m128 v128) {
-		mVec128 = v128;
-	}
+    union {
+        __m128 mVec128;
+        btScalar m_floats[4];
+    };
+    SIMD_FORCE_INLINE __m128 get128() const
+    {
+        return mVec128;
+    }
+    SIMD_FORCE_INLINE void set128(__m128 v128)
+    {
+        mVec128 = v128;
+    }
 #else
-	btScalar m_floats[4];
+    btScalar m_floats[4];
 #endif
 #endif //__CELLOS_LV2__ __SPU__
 
 public:
-	/**@brief No initialization constructor */
-	SIMD_FORCE_INLINE btVector3() {}
+    /**@brief No initialization constructor */
+    SIMD_FORCE_INLINE btVector3() {}
 
-	/**@brief Constructor from scalars
+    /**@brief Constructor from scalars 
    * @param x X value
    * @param y Y value 
    * @param z Z value 
    */
-	SIMD_FORCE_INLINE btVector3(const btScalar &x, const btScalar &y, const btScalar &z) {
-		m_floats[0] = x;
-		m_floats[1] = y;
-		m_floats[2] = z;
-		m_floats[3] = btScalar(0.);
-	}
-
-	/**@brief Add a vector to this one
+    SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
+    {
+        m_floats[0] = x;
+        m_floats[1] = y;
+        m_floats[2] = z;
+        m_floats[3] = btScalar(0.);
+    }
+
+    /**@brief Add a vector to this one 
  * @param The vector to add to this one */
-	SIMD_FORCE_INLINE btVector3 &operator+=(const btVector3 &v) {
+    SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
+    {
 
-		m_floats[0] += v.m_floats[0];
-		m_floats[1] += v.m_floats[1];
-		m_floats[2] += v.m_floats[2];
-		return *this;
-	}
+        m_floats[0] += v.m_floats[0];
+        m_floats[1] += v.m_floats[1];
+        m_floats[2] += v.m_floats[2];
+        return *this;
+    }
 
-	/**@brief Subtract a vector from this one
+    /**@brief Subtract a vector from this one
    * @param The vector to subtract */
-	SIMD_FORCE_INLINE btVector3 &operator-=(const btVector3 &v) {
-		m_floats[0] -= v.m_floats[0];
-		m_floats[1] -= v.m_floats[1];
-		m_floats[2] -= v.m_floats[2];
-		return *this;
-	}
-	/**@brief Scale the vector
+    SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
+    {
+        m_floats[0] -= v.m_floats[0];
+        m_floats[1] -= v.m_floats[1];
+        m_floats[2] -= v.m_floats[2];
+        return *this;
+    }
+    /**@brief Scale the vector
    * @param s Scale factor */
-	SIMD_FORCE_INLINE btVector3 &operator*=(const btScalar &s) {
-		m_floats[0] *= s;
-		m_floats[1] *= s;
-		m_floats[2] *= s;
-		return *this;
-	}
-
-	/**@brief Inversely scale the vector
+    SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
+    {
+        m_floats[0] *= s;
+        m_floats[1] *= s;
+        m_floats[2] *= s;
+        return *this;
+    }
+
+    /**@brief Inversely scale the vector 
    * @param s Scale factor to divide by */
-	SIMD_FORCE_INLINE btVector3 &operator/=(const btScalar &s) {
-		btFullAssert(s != btScalar(0.0));
-		return *this *= btScalar(1.0) / s;
-	}
+    SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
+    {
+        btFullAssert(s != btScalar(0.0));
+        return * this *= btScalar(1.0) / s;
+    }
 
-	/**@brief Return the dot product
+    /**@brief Return the dot product
    * @param v The other vector in the dot product */
-	SIMD_FORCE_INLINE btScalar dot(const btVector3 &v) const {
-		return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] + m_floats[2] * v.m_floats[2];
-	}
-
-	/**@brief Return the length of the vector squared */
-	SIMD_FORCE_INLINE btScalar length2() const {
-		return dot(*this);
-	}
-
-	/**@brief Return the length of the vector */
-	SIMD_FORCE_INLINE btScalar length() const {
-		return btSqrt(length2());
-	}
-
-	/**@brief Return the distance squared between the ends of this and another vector
+    SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
+    {
+        return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] + m_floats[2] * v.m_floats[2];
+    }
+
+    /**@brief Return the length of the vector squared */
+    SIMD_FORCE_INLINE btScalar length2() const
+    {
+        return dot(*this);
+    }
+
+    /**@brief Return the length of the vector */
+    SIMD_FORCE_INLINE btScalar length() const
+    {
+        return btSqrt(length2());
+    }
+
+    /**@brief Return the distance squared between the ends of this and another vector
    * This is symantically treating the vector like a point */
-	SIMD_FORCE_INLINE btScalar distance2(const btVector3 &v) const;
+    SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
 
-	/**@brief Return the distance between the ends of this and another vector
+    /**@brief Return the distance between the ends of this and another vector
    * This is symantically treating the vector like a point */
-	SIMD_FORCE_INLINE btScalar distance(const btVector3 &v) const;
-
-	SIMD_FORCE_INLINE btVector3 &safeNormalize() {
-		btVector3 absVec = this->absolute();
-		int32_t maxIndex = absVec.maxAxis();
-		if (absVec[maxIndex] > 0) {
-			*this /= absVec[maxIndex];
-			return *this /= length();
-		}
-		setValue(1, 0, 0);
-		return *this;
-	}
-
-	/**@brief Normalize this vector
+    SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
+
+    SIMD_FORCE_INLINE btVector3& safeNormalize()
+    {
+        btVector3 absVec = this->absolute();
+        int32_t maxIndex = absVec.maxAxis();
+        if (absVec[maxIndex] > 0) {
+            *this /= absVec[maxIndex];
+            return * this /= length();
+        }
+        setValue(1, 0, 0);
+        return *this;
+    }
+
+    /**@brief Normalize this vector 
    * x^2 + y^2 + z^2 = 1 */
-	SIMD_FORCE_INLINE btVector3 &normalize() {
-		return *this /= length();
-	}
+    SIMD_FORCE_INLINE btVector3& normalize()
+    {
+        return * this /= length();
+    }
 
-	/**@brief Return a normalized version of this vector */
-	SIMD_FORCE_INLINE btVector3 normalized() const;
+    /**@brief Return a normalized version of this vector */
+    SIMD_FORCE_INLINE btVector3 normalized() const;
 
-	/**@brief Return a rotated version of this vector
+    /**@brief Return a rotated version of this vector
    * @param wAxis The axis to rotate about 
    * @param angle The angle to rotate by */
-	SIMD_FORCE_INLINE btVector3 rotate(const btVector3 &wAxis, const btScalar angle) const;
+    SIMD_FORCE_INLINE btVector3 rotate(const btVector3& wAxis, const btScalar angle) const;
 
-	/**@brief Return the angle between this and another vector
+    /**@brief Return the angle between this and another vector
    * @param v The other vector */
-	SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const {
-		btScalar s = btSqrt(length2() * v.length2());
-		btFullAssert(s != btScalar(0.0));
-		return btAcos(dot(v) / s);
-	}
-	/**@brief Return a vector will the absolute values of each element */
-	SIMD_FORCE_INLINE btVector3 absolute() const {
-		return btVector3(
-				btFabs(m_floats[0]),
-				btFabs(m_floats[1]),
-				btFabs(m_floats[2]));
-	}
-	/**@brief Return the cross product between this and another vector
+    SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
+    {
+        btScalar s = btSqrt(length2() * v.length2());
+        btFullAssert(s != btScalar(0.0));
+        return btAcos(dot(v) / s);
+    }
+    /**@brief Return a vector will the absolute values of each element */
+    SIMD_FORCE_INLINE btVector3 absolute() const
+    {
+        return btVector3(
+            btFabs(m_floats[0]),
+            btFabs(m_floats[1]),
+            btFabs(m_floats[2]));
+    }
+    /**@brief Return the cross product between this and another vector 
    * @param v The other vector */
-	SIMD_FORCE_INLINE btVector3 cross(const btVector3 &v) const {
-		return btVector3(
-				m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
-				m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
-				m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
-	}
-
-	SIMD_FORCE_INLINE btScalar triple(const btVector3 &v1, const btVector3 &v2) const {
-		return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
-	}
-
-	/**@brief Return the axis with the smallest value
+    SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
+    {
+        return btVector3(
+            m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
+            m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+            m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
+    }
+
+    SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
+    {
+        return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+    }
+
+    /**@brief Return the axis with the smallest value 
    * Note return values are 0,1,2 for x, y, or z */
-	SIMD_FORCE_INLINE int32_t minAxis() const {
-		return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
-	}
+    SIMD_FORCE_INLINE int32_t minAxis() const
+    {
+        return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
+    }
 
-	/**@brief Return the axis with the largest value
+    /**@brief Return the axis with the largest value 
    * Note return values are 0,1,2 for x, y, or z */
-	SIMD_FORCE_INLINE int32_t maxAxis() const {
-		return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
-	}
-
-	SIMD_FORCE_INLINE int32_t furthestAxis() const {
-		return absolute().minAxis();
-	}
-
-	SIMD_FORCE_INLINE int32_t closestAxis() const {
-		return absolute().maxAxis();
-	}
-
-	SIMD_FORCE_INLINE void setInterpolate3(const btVector3 &v0, const btVector3 &v1, btScalar rt) {
-		btScalar s = btScalar(1.0) - rt;
-		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
-		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
-		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
-		//don't do the unused w component
-		//		m_co[3] = s * v0[3] + rt * v1[3];
-	}
-
-	/**@brief Return the linear interpolation between this and another vector
+    SIMD_FORCE_INLINE int32_t maxAxis() const
+    {
+        return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
+    }
+
+    SIMD_FORCE_INLINE int32_t furthestAxis() const
+    {
+        return absolute().minAxis();
+    }
+
+    SIMD_FORCE_INLINE int32_t closestAxis() const
+    {
+        return absolute().maxAxis();
+    }
+
+    SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
+    {
+        btScalar s = btScalar(1.0) - rt;
+        m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+        m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+        m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+        //don't do the unused w component
+        //		m_co[3] = s * v0[3] + rt * v1[3];
+    }
+
+    /**@brief Return the linear interpolation between this and another vector 
    * @param v The other vector 
    * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
-	SIMD_FORCE_INLINE btVector3 lerp(const btVector3 &v, const btScalar &t) const {
-		return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
-				m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
-				m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
-	}
-
-	/**@brief Elementwise multiply this vector by the other
+    SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
+    {
+        return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+            m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+            m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
+    }
+
+    /**@brief Elementwise multiply this vector by the other 
    * @param v The other vector */
-	SIMD_FORCE_INLINE btVector3 &operator*=(const btVector3 &v) {
-		m_floats[0] *= v.m_floats[0];
-		m_floats[1] *= v.m_floats[1];
-		m_floats[2] *= v.m_floats[2];
-		return *this;
-	}
-
-	/**@brief Return the x value */
-	SIMD_FORCE_INLINE const btScalar &getX() const { return m_floats[0]; }
-	/**@brief Return the y value */
-	SIMD_FORCE_INLINE const btScalar &getY() const { return m_floats[1]; }
-	/**@brief Return the z value */
-	SIMD_FORCE_INLINE const btScalar &getZ() const { return m_floats[2]; }
-	/**@brief Set the x value */
-	SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x; };
-	/**@brief Set the y value */
-	SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y; };
-	/**@brief Set the z value */
-	SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z; };
-	/**@brief Set the w value */
-	SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w; };
-	/**@brief Return the x value */
-	SIMD_FORCE_INLINE const btScalar &x() const { return m_floats[0]; }
-	/**@brief Return the y value */
-	SIMD_FORCE_INLINE const btScalar &y() const { return m_floats[1]; }
-	/**@brief Return the z value */
-	SIMD_FORCE_INLINE const btScalar &z() const { return m_floats[2]; }
-	/**@brief Return the w value */
-	SIMD_FORCE_INLINE const btScalar &w() const { return m_floats[3]; }
-
-	//SIMD_FORCE_INLINE btScalar&       operator[](int32_t i)       { return (&m_floats[0])[i];	}
-	//SIMD_FORCE_INLINE const btScalar& operator[](int32_t i) const { return (&m_floats[0])[i]; }
-	///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
-	SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
-	SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
-
-	SIMD_FORCE_INLINE bool operator==(const btVector3 &other) const {
-		return ((m_floats[3] == other.m_floats[3]) && (m_floats[2] == other.m_floats[2]) && (m_floats[1] == other.m_floats[1]) && (m_floats[0] == other.m_floats[0]));
-	}
-
-	SIMD_FORCE_INLINE bool operator!=(const btVector3 &other) const {
-		return !(*this == other);
-	}
-
-	/**@brief Set each element to the max of the current values and the values of another btVector3
+    SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
+    {
+        m_floats[0] *= v.m_floats[0];
+        m_floats[1] *= v.m_floats[1];
+        m_floats[2] *= v.m_floats[2];
+        return *this;
+    }
+
+    /**@brief Return the x value */
+    SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+    /**@brief Return the y value */
+    SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+    /**@brief Return the z value */
+    SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+    /**@brief Set the x value */
+    SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x; };
+    /**@brief Set the y value */
+    SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y; };
+    /**@brief Set the z value */
+    SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z; };
+    /**@brief Set the w value */
+    SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w; };
+    /**@brief Return the x value */
+    SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+    /**@brief Return the y value */
+    SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+    /**@brief Return the z value */
+    SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+    /**@brief Return the w value */
+    SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+    //SIMD_FORCE_INLINE btScalar&       operator[](int32_t i)       { return (&m_floats[0])[i];	}
+    //SIMD_FORCE_INLINE const btScalar& operator[](int32_t i) const { return (&m_floats[0])[i]; }
+    ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+    SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; }
+    SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; }
+
+    SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
+    {
+        return ((m_floats[3] == other.m_floats[3]) && (m_floats[2] == other.m_floats[2]) && (m_floats[1] == other.m_floats[1]) && (m_floats[0] == other.m_floats[0]));
+    }
+
+    SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
+    {
+        return !(*this == other);
+    }
+
+    /**@brief Set each element to the max of the current values and the values of another btVector3
    * @param other The other btVector3 to compare with 
    */
-	SIMD_FORCE_INLINE void setMax(const btVector3 &other) {
-		btSetMax(m_floats[0], other.m_floats[0]);
-		btSetMax(m_floats[1], other.m_floats[1]);
-		btSetMax(m_floats[2], other.m_floats[2]);
-		btSetMax(m_floats[3], other.w());
-	}
-	/**@brief Set each element to the min of the current values and the values of another btVector3
+    SIMD_FORCE_INLINE void setMax(const btVector3& other)
+    {
+        btSetMax(m_floats[0], other.m_floats[0]);
+        btSetMax(m_floats[1], other.m_floats[1]);
+        btSetMax(m_floats[2], other.m_floats[2]);
+        btSetMax(m_floats[3], other.w());
+    }
+    /**@brief Set each element to the min of the current values and the values of another btVector3
    * @param other The other btVector3 to compare with 
    */
-	SIMD_FORCE_INLINE void setMin(const btVector3 &other) {
-		btSetMin(m_floats[0], other.m_floats[0]);
-		btSetMin(m_floats[1], other.m_floats[1]);
-		btSetMin(m_floats[2], other.m_floats[2]);
-		btSetMin(m_floats[3], other.w());
-	}
-
-	SIMD_FORCE_INLINE void setValue(const btScalar &x, const btScalar &y, const btScalar &z) {
-		m_floats[0] = x;
-		m_floats[1] = y;
-		m_floats[2] = z;
-		m_floats[3] = btScalar(0.);
-	}
-
-	void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const {
-		v0->setValue(0., -z(), y());
-		v1->setValue(z(), 0., -x());
-		v2->setValue(-y(), x(), 0.);
-	}
-
-	void setZero() {
-		setValue(btScalar(0.), btScalar(0.), btScalar(0.));
-	}
-
-	SIMD_FORCE_INLINE bool isZero() const {
-		return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
-	}
-
-	SIMD_FORCE_INLINE bool fuzzyZero() const {
-		return length2() < SIMD_EPSILON;
-	}
-
-	SIMD_FORCE_INLINE void serialize(struct btVector3Data & dataOut) const;
-
-	SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data &dataIn);
-
-	SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData & dataOut) const;
-
-	SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData &dataIn);
-
-	SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData & dataOut) const;
-
-	SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData &dataIn);
+    SIMD_FORCE_INLINE void setMin(const btVector3& other)
+    {
+        btSetMin(m_floats[0], other.m_floats[0]);
+        btSetMin(m_floats[1], other.m_floats[1]);
+        btSetMin(m_floats[2], other.m_floats[2]);
+        btSetMin(m_floats[3], other.w());
+    }
+
+    SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+    {
+        m_floats[0] = x;
+        m_floats[1] = y;
+        m_floats[2] = z;
+        m_floats[3] = btScalar(0.);
+    }
+
+    void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const
+    {
+        v0->setValue(0., -z(), y());
+        v1->setValue(z(), 0., -x());
+        v2->setValue(-y(), x(), 0.);
+    }
+
+    void setZero()
+    {
+        setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+    }
+
+    SIMD_FORCE_INLINE bool isZero() const
+    {
+        return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
+    }
+
+    SIMD_FORCE_INLINE bool fuzzyZero() const
+    {
+        return length2() < SIMD_EPSILON;
+    }
+
+    SIMD_FORCE_INLINE void serialize(struct btVector3Data & dataOut) const;
+
+    SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
+
+    SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData & dataOut) const;
+
+    SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
+
+    SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData & dataOut) const;
+
+    SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
 };
 
 /**@brief Return the sum of two vectors (Point symantics)*/
 SIMD_FORCE_INLINE btVector3
-operator+(const btVector3 &v1, const btVector3 &v2) {
-	return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
+operator+(const btVector3& v1, const btVector3& v2)
+{
+    return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
 }
 
 /**@brief Return the elementwise product of two vectors */
 SIMD_FORCE_INLINE btVector3
-operator*(const btVector3 &v1, const btVector3 &v2) {
-	return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
+operator*(const btVector3& v1, const btVector3& v2)
+{
+    return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
 }
 
 /**@brief Return the difference between two vectors */
 SIMD_FORCE_INLINE btVector3
-operator-(const btVector3 &v1, const btVector3 &v2) {
-	return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
+operator-(const btVector3& v1, const btVector3& v2)
+{
+    return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
 }
 /**@brief Return the negative of the vector */
 SIMD_FORCE_INLINE btVector3
-operator-(const btVector3 &v) {
-	return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
+operator-(const btVector3& v)
+{
+    return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
 }
 
 /**@brief Return the vector scaled by s */
 SIMD_FORCE_INLINE btVector3
-operator*(const btVector3 &v, const btScalar &s) {
-	return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
+operator*(const btVector3& v, const btScalar& s)
+{
+    return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
 }
 
 /**@brief Return the vector scaled by s */
 SIMD_FORCE_INLINE btVector3
-operator*(const btScalar &s, const btVector3 &v) {
-	return v * s;
+operator*(const btScalar& s, const btVector3& v)
+{
+    return v * s;
 }
 
 /**@brief Return the vector inversely scaled by s */
 SIMD_FORCE_INLINE btVector3
-operator/(const btVector3 &v, const btScalar &s) {
-	btFullAssert(s != btScalar(0.0));
-	return v * (btScalar(1.0) / s);
+operator/(const btVector3& v, const btScalar& s)
+{
+    btFullAssert(s != btScalar(0.0));
+    return v * (btScalar(1.0) / s);
 }
 
 /**@brief Return the vector inversely scaled by s */
 SIMD_FORCE_INLINE btVector3
-operator/(const btVector3 &v1, const btVector3 &v2) {
-	return btVector3(v1.m_floats[0] / v2.m_floats[0], v1.m_floats[1] / v2.m_floats[1], v1.m_floats[2] / v2.m_floats[2]);
+operator/(const btVector3& v1, const btVector3& v2)
+{
+    return btVector3(v1.m_floats[0] / v2.m_floats[0], v1.m_floats[1] / v2.m_floats[1], v1.m_floats[2] / v2.m_floats[2]);
 }
 
 /**@brief Return the dot product between two vectors */
 SIMD_FORCE_INLINE btScalar
-btDot(const btVector3 &v1, const btVector3 &v2) {
-	return v1.dot(v2);
+btDot(const btVector3& v1, const btVector3& v2)
+{
+    return v1.dot(v2);
 }
 
 /**@brief Return the distance squared between two vectors */
 SIMD_FORCE_INLINE btScalar
-btDistance2(const btVector3 &v1, const btVector3 &v2) {
-	return v1.distance2(v2);
+btDistance2(const btVector3& v1, const btVector3& v2)
+{
+    return v1.distance2(v2);
 }
 
 /**@brief Return the distance between two vectors */
 SIMD_FORCE_INLINE btScalar
-btDistance(const btVector3 &v1, const btVector3 &v2) {
-	return v1.distance(v2);
+btDistance(const btVector3& v1, const btVector3& v2)
+{
+    return v1.distance(v2);
 }
 
 /**@brief Return the angle between two vectors */
 SIMD_FORCE_INLINE btScalar
-btAngle(const btVector3 &v1, const btVector3 &v2) {
-	return v1.angle(v2);
+btAngle(const btVector3& v1, const btVector3& v2)
+{
+    return v1.angle(v2);
 }
 
 /**@brief Return the cross product of two vectors */
 SIMD_FORCE_INLINE btVector3
-btCross(const btVector3 &v1, const btVector3 &v2) {
-	return v1.cross(v2);
+btCross(const btVector3& v1, const btVector3& v2)
+{
+    return v1.cross(v2);
 }
 
 SIMD_FORCE_INLINE btScalar
-btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3) {
-	return v1.triple(v2, v3);
+btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+{
+    return v1.triple(v2, v3);
 }
 
 /**@brief Return the linear interpolation between two vectors
@@ -418,236 +462,254 @@ btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3) {
  * @param v2 The other vector 
  * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
 SIMD_FORCE_INLINE btVector3
-lerp(const btVector3 &v1, const btVector3 &v2, const btScalar &t) {
-	return v1.lerp(v2, t);
+lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
+{
+    return v1.lerp(v2, t);
 }
 
-SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3 &v) const {
-	return (v - *this).length2();
+SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
+{
+    return (v - *this).length2();
 }
 
-SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3 &v) const {
-	return (v - *this).length();
+SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
+{
+    return (v - *this).length();
 }
 
-SIMD_FORCE_INLINE btVector3 btVector3::normalized() const {
-	return *this / length();
+SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
+{
+    return *this / length();
 }
 
-SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3 &wAxis, const btScalar angle) const {
-	// wAxis must be a unit lenght vector
+SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3& wAxis, const btScalar angle) const
+{
+    // wAxis must be a unit lenght vector
 
-	btVector3 o = wAxis * wAxis.dot(*this);
-	btVector3 x = *this - o;
-	btVector3 y;
+    btVector3 o = wAxis * wAxis.dot(*this);
+    btVector3 x = *this - o;
+    btVector3 y;
 
-	y = wAxis.cross(*this);
+    y = wAxis.cross(*this);
 
-	return (o + x * btCos(angle) + y * btSin(angle));
+    return (o + x * btCos(angle) + y * btSin(angle));
 }
 
 class btVector4 : public btVector3 {
 public:
-	SIMD_FORCE_INLINE btVector4() {}
-
-	SIMD_FORCE_INLINE btVector4(const btScalar &x, const btScalar &y, const btScalar &z, const btScalar &w) :
-			btVector3(x, y, z) {
-		m_floats[3] = w;
-	}
-
-	SIMD_FORCE_INLINE btVector4 absolute4() const {
-		return btVector4(
-				btFabs(m_floats[0]),
-				btFabs(m_floats[1]),
-				btFabs(m_floats[2]),
-				btFabs(m_floats[3]));
-	}
-
-	btScalar getW() const { return m_floats[3]; }
-
-	SIMD_FORCE_INLINE int32_t maxAxis4() const {
-		int32_t maxIndex = -1;
-		btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
-		if (m_floats[0] > maxVal) {
-			maxIndex = 0;
-			maxVal = m_floats[0];
-		}
-		if (m_floats[1] > maxVal) {
-			maxIndex = 1;
-			maxVal = m_floats[1];
-		}
-		if (m_floats[2] > maxVal) {
-			maxIndex = 2;
-			maxVal = m_floats[2];
-		}
-		if (m_floats[3] > maxVal) {
-			maxIndex = 3;
-		}
-		return maxIndex;
-	}
-
-	SIMD_FORCE_INLINE int32_t minAxis4() const {
-		int32_t minIndex = -1;
-		btScalar minVal = btScalar(BT_LARGE_FLOAT);
-		if (m_floats[0] < minVal) {
-			minIndex = 0;
-			minVal = m_floats[0];
-		}
-		if (m_floats[1] < minVal) {
-			minIndex = 1;
-			minVal = m_floats[1];
-		}
-		if (m_floats[2] < minVal) {
-			minIndex = 2;
-			minVal = m_floats[2];
-		}
-		if (m_floats[3] < minVal) {
-			minIndex = 3;
-		}
-
-		return minIndex;
-	}
-
-	SIMD_FORCE_INLINE int32_t closestAxis4() const {
-		return absolute4().maxAxis4();
-	}
-
-	/**@brief Set x,y,z and zero w
+    SIMD_FORCE_INLINE btVector4() {}
+
+    SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
+        : btVector3(x, y, z)
+    {
+        m_floats[3] = w;
+    }
+
+    SIMD_FORCE_INLINE btVector4 absolute4() const
+    {
+        return btVector4(
+            btFabs(m_floats[0]),
+            btFabs(m_floats[1]),
+            btFabs(m_floats[2]),
+            btFabs(m_floats[3]));
+    }
+
+    btScalar getW() const { return m_floats[3]; }
+
+    SIMD_FORCE_INLINE int32_t maxAxis4() const
+    {
+        int32_t maxIndex = -1;
+        btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
+        if (m_floats[0] > maxVal) {
+            maxIndex = 0;
+            maxVal = m_floats[0];
+        }
+        if (m_floats[1] > maxVal) {
+            maxIndex = 1;
+            maxVal = m_floats[1];
+        }
+        if (m_floats[2] > maxVal) {
+            maxIndex = 2;
+            maxVal = m_floats[2];
+        }
+        if (m_floats[3] > maxVal) {
+            maxIndex = 3;
+        }
+        return maxIndex;
+    }
+
+    SIMD_FORCE_INLINE int32_t minAxis4() const
+    {
+        int32_t minIndex = -1;
+        btScalar minVal = btScalar(BT_LARGE_FLOAT);
+        if (m_floats[0] < minVal) {
+            minIndex = 0;
+            minVal = m_floats[0];
+        }
+        if (m_floats[1] < minVal) {
+            minIndex = 1;
+            minVal = m_floats[1];
+        }
+        if (m_floats[2] < minVal) {
+            minIndex = 2;
+            minVal = m_floats[2];
+        }
+        if (m_floats[3] < minVal) {
+            minIndex = 3;
+        }
+
+        return minIndex;
+    }
+
+    SIMD_FORCE_INLINE int32_t closestAxis4() const
+    {
+        return absolute4().maxAxis4();
+    }
+
+    /**@brief Set x,y,z and zero w 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    */
 
-	/*		void getValue(btScalar *m) const
+    /*		void getValue(btScalar *m) const 
 		{
 			m[0] = m_floats[0];
 			m[1] = m_floats[1];
 			m[2] =m_floats[2];
 		}
 */
-	/**@brief Set the values
+    /**@brief Set the values 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    * @param w Value of w
    */
-	SIMD_FORCE_INLINE void setValue(const btScalar &x, const btScalar &y, const btScalar &z, const btScalar &w) {
-		m_floats[0] = x;
-		m_floats[1] = y;
-		m_floats[2] = z;
-		m_floats[3] = w;
-	}
+    SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
+    {
+        m_floats[0] = x;
+        m_floats[1] = y;
+        m_floats[2] = z;
+        m_floats[3] = w;
+    }
 };
 
 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
-SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar &sourceVal, btScalar &destVal) {
+SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
+{
 #ifdef BT_USE_DOUBLE_PRECISION
-	unsigned char *dest = (unsigned char *)&destVal;
-	unsigned char *src = (unsigned char *)&sourceVal;
-	dest[0] = src[7];
-	dest[1] = src[6];
-	dest[2] = src[5];
-	dest[3] = src[4];
-	dest[4] = src[3];
-	dest[5] = src[2];
-	dest[6] = src[1];
-	dest[7] = src[0];
+    unsigned char* dest = (unsigned char*)&destVal;
+    unsigned char* src = (unsigned char*)&sourceVal;
+    dest[0] = src[7];
+    dest[1] = src[6];
+    dest[2] = src[5];
+    dest[3] = src[4];
+    dest[4] = src[3];
+    dest[5] = src[2];
+    dest[6] = src[1];
+    dest[7] = src[0];
 #else
-	unsigned char *dest = (unsigned char *)&destVal;
-	unsigned char *src = (unsigned char *)&sourceVal;
-	dest[0] = src[3];
-	dest[1] = src[2];
-	dest[2] = src[1];
-	dest[3] = src[0];
+    unsigned char* dest = (unsigned char*)&destVal;
+    unsigned char* src = (unsigned char*)&sourceVal;
+    dest[0] = src[3];
+    dest[1] = src[2];
+    dest[2] = src[1];
+    dest[3] = src[0];
 #endif //BT_USE_DOUBLE_PRECISION
 }
 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
-SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3 &sourceVec, btVector3 &destVec) {
-	for (int32_t i = 0; i < 4; i++) {
-		btSwapScalarEndian(sourceVec[i], destVec[i]);
-	}
+SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
+{
+    for (int32_t i = 0; i < 4; i++) {
+        btSwapScalarEndian(sourceVec[i], destVec[i]);
+    }
 }
 
 ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
-SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3 &vector) {
-
-	btVector3 swappedVec;
-	for (int32_t i = 0; i < 4; i++) {
-		btSwapScalarEndian(vector[i], swappedVec[i]);
-	}
-	vector = swappedVec;
+SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
+{
+
+    btVector3 swappedVec;
+    for (int32_t i = 0; i < 4; i++) {
+        btSwapScalarEndian(vector[i], swappedVec[i]);
+    }
+    vector = swappedVec;
 }
 
 template <class T>
-SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q) {
-	if (btFabs(n[2]) > SIMDSQRT12) {
-		// choose p in y-z plane
-		btScalar a = n[1] * n[1] + n[2] * n[2];
-		btScalar k = btRecipSqrt(a);
-		p[0] = 0;
-		p[1] = -n[2] * k;
-		p[2] = n[1] * k;
-		// set q = n x p
-		q[0] = a * k;
-		q[1] = -n[0] * p[2];
-		q[2] = n[0] * p[1];
-	} else {
-		// choose p in x-y plane
-		btScalar a = n[0] * n[0] + n[1] * n[1];
-		btScalar k = btRecipSqrt(a);
-		p[0] = -n[1] * k;
-		p[1] = n[0] * k;
-		p[2] = 0;
-		// set q = n x p
-		q[0] = -n[2] * p[1];
-		q[1] = n[2] * p[0];
-		q[2] = a * k;
-	}
+SIMD_FORCE_INLINE void btPlaneSpace1(const T& n, T& p, T& q)
+{
+    if (btFabs(n[2]) > SIMDSQRT12) {
+        // choose p in y-z plane
+        btScalar a = n[1] * n[1] + n[2] * n[2];
+        btScalar k = btRecipSqrt(a);
+        p[0] = 0;
+        p[1] = -n[2] * k;
+        p[2] = n[1] * k;
+        // set q = n x p
+        q[0] = a * k;
+        q[1] = -n[0] * p[2];
+        q[2] = n[0] * p[1];
+    }
+    else {
+        // choose p in x-y plane
+        btScalar a = n[0] * n[0] + n[1] * n[1];
+        btScalar k = btRecipSqrt(a);
+        p[0] = -n[1] * k;
+        p[1] = n[0] * k;
+        p[2] = 0;
+        // set q = n x p
+        q[0] = -n[2] * p[1];
+        q[1] = n[2] * p[0];
+        q[2] = a * k;
+    }
 }
 
 struct btVector3FloatData {
-	float m_floats[4];
+    float m_floats[4];
 };
 
 struct btVector3DoubleData {
-	double m_floats[4];
+    double m_floats[4];
 };
 
-SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData &dataOut) const {
-	///could also do a memcpy, check if it is worth it
-	for (int32_t i = 0; i < 4; i++)
-		dataOut.m_floats[i] = float(m_floats[i]);
+SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
+{
+    ///could also do a memcpy, check if it is worth it
+    for (int32_t i = 0; i < 4; i++)
+        dataOut.m_floats[i] = float(m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData &dataIn) {
-	for (int32_t i = 0; i < 4; i++)
-		m_floats[i] = btScalar(dataIn.m_floats[i]);
+SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
+{
+    for (int32_t i = 0; i < 4; i++)
+        m_floats[i] = btScalar(dataIn.m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData &dataOut) const {
-	///could also do a memcpy, check if it is worth it
-	for (int32_t i = 0; i < 4; i++)
-		dataOut.m_floats[i] = double(m_floats[i]);
+SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
+{
+    ///could also do a memcpy, check if it is worth it
+    for (int32_t i = 0; i < 4; i++)
+        dataOut.m_floats[i] = double(m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData &dataIn) {
-	for (int32_t i = 0; i < 4; i++)
-		m_floats[i] = btScalar(dataIn.m_floats[i]);
+SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
+{
+    for (int32_t i = 0; i < 4; i++)
+        m_floats[i] = btScalar(dataIn.m_floats[i]);
 }
 
-SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data &dataOut) const {
-	///could also do a memcpy, check if it is worth it
-	for (int32_t i = 0; i < 4; i++)
-		dataOut.m_floats[i] = m_floats[i];
+SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
+{
+    ///could also do a memcpy, check if it is worth it
+    for (int32_t i = 0; i < 4; i++)
+        dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data &dataIn) {
-	for (int32_t i = 0; i < 4; i++)
-		m_floats[i] = dataIn.m_floats[i];
+SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
+{
+    for (int32_t i = 0; i < 4; i++)
+        m_floats[i] = dataIn.m_floats[i];
 }
 
-//GODOT ADDITION
-}; // namespace VHACD
-//
-
 #endif //BT_VECTOR3_H