Adding some C++ external dependency code

ballisticacore-cmake/.idea/inspectionProfiles/Project_Default.xml

@@ -6,6 +6,7 @@
     <inspection_tool class="ClangTidy" enabled="true" level="WARNING" enabled_by_default="true">
       <option name="clangTidyChecks" value="*,-android-*,-bugprone-bool-pointer-implicit-conversion,-bugprone-exception-escape,-bugprone-signed-char-misuse,-cert-dcl50-cpp,-cert-env33-c,-cert-mem57-cpp,-cert-oop57-cpp,-cert-oop58-cpp,-clang-analyzer-*,-clang-diagnostic-*,-cppcoreguidelines-avoid-goto,-cppcoreguidelines-init-variables,-cppcoreguidelines-macro-usage,-cppcoreguidelines-no-malloc,-cppcoreguidelines-owning-memory,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-pro-bounds-constant-array-index,-cppcoreguidelines-pro-bounds-pointer-arithmetic,-cppcoreguidelines-pro-type-const-cast,-cppcoreguidelines-pro-type-cstyle-cast,-cppcoreguidelines-pro-type-reinterpret-cast,-cppcoreguidelines-pro-type-union-access,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-special-member-functions,-darwin-*,-fuchsia-*,-google-*,-hicpp-avoid-goto,-hicpp-no-assembler,-linuxkernel-*,-llvm-*,-llvmlibc-*,-misc-bool-pointer-implicit-conversion,-misc-definitions-in-headers,-misc-non-private-member-variables-in-classes,-misc-unused-alias-decls,-misc-unused-parameters,-misc-unused-using-decls,-modernize-avoid-c-arrays,-modernize-use-default-member-init,-modernize-use-trailing-return-type,-modernize-use-using,-objc-*,-openmp-exception-escape,-readability-braces-around-statements,-readability-else-after-return,-readability-function-size,-readability-identifier-naming,-readability-implicit-bool-conversion,-readability-isolate-declaration,-readability-magic-numbers,-readability-named-parameter,-readability-qualified-auto,-readability-redundant-access-specifiers,-readability-redundant-member-init,-readability-redundant-preprocessor,-readability-simplify-boolean-expr,-readability-uppercase-literal-suffix,-zircon-*,-cert-dcl03-c,-cert-dcl16-c,-cert-dcl37-c,-cert-dcl51-cpp,-cert-dcl54-cpp,-cert-dcl59-cpp,-cert-err09-cpp,-cert-err61-cpp,-cert-fio38-c,-cert-msc30-c,-cert-msc32-c,-cert-oop11-cpp,-cert-oop54-cpp,-cert-pos44-c,-cppcoreguidelines-avoid-c-arrays,-cppcoreguidelines-avoid-magic-numbers,-cppcoreguidelines-c-copy-assignment-signature,-cppcoreguidelines-explicit-virtual-functions,-cppcoreguidelines-non-private-member-variables-in-classes,-fuchsia-header-anon-namespaces,-google-readability-braces-around-statements,-google-readability-function-size,-google-readability-namespace-comments,-hicpp-avoid-c-arrays,-hicpp-braces-around-statements,-hicpp-deprecated-headers,-hicpp-explicit-conversions,-hicpp-function-size,-hicpp-invalid-access-moved,-hicpp-member-init,-hicpp-move-const-arg,-hicpp-named-parameter,-hicpp-new-delete-operators,-hicpp-no-array-decay,-hicpp-no-malloc,-hicpp-noexcept-move,-hicpp-special-member-functions,-hicpp-static-assert,-hicpp-undelegated-constructor,-hicpp-uppercase-literal-suffix,-hicpp-use-auto,-hicpp-use-emplace,-hicpp-use-equals-default,-hicpp-use-equals-delete,-hicpp-use-noexcept,-hicpp-use-nullptr,-hicpp-use-override,-hicpp-vararg,google-default-arguments,google-explicit-constructor,google-runtime-operator,-readability-convert-member-functions-to-static,-bugprone-reserved-identifier,-cppcoreguidelines-pro-type-static-cast-downcast,-readability-make-member-function-const,-cert-err58-cpp,-bugprone-lambda-function-name,-modernize-use-nodiscard,-llvmlibc-callee-namespace,-readability-use-anyofallof,-cppcoreguidelines-avoid-non-const-global-variables " />
     </inspection_tool>
+    <inspection_tool class="ConstructionIsNotAllowed" enabled="false" level="WARNING" enabled_by_default="false" />
     <inspection_tool class="DanglingPointers" enabled="false" level="WARNING" enabled_by_default="false" />
     <inspection_tool class="DuplicateSwitchCase" enabled="false" level="WARNING" enabled_by_default="false" />
     <inspection_tool class="ImplicitPointerAndIntegerConversion" enabled="false" level="WARNING" enabled_by_default="false" />
@@ -51,6 +52,7 @@
       <option name="enableSimplifyTernaryWithConstantBranch" value="false" />
     </inspection_tool>
     <inspection_tool class="StaticnessMismatch" enabled="false" level="ERROR" enabled_by_default="false" />
+    <inspection_tool class="UnusedExpressionResult" enabled="false" level="WARNING" enabled_by_default="false" />
     <inspection_tool class="UnusedGlobalDeclaration" enabled="false" level="WARNING" enabled_by_default="false" />
     <inspection_tool class="UnusedLocalVariable" enabled="false" level="WARNING" enabled_by_default="false" />
     <inspection_tool class="UnusedMacro" enabled="false" level="WARNING" enabled_by_default="false" />

src/external/httprequest/httprequest.hpp

@@ -0,0 +1,689 @@
+//
+//  https://github.com/elnormous/HTTPRequest
+//
+
+#ifndef HTTPREQUEST_HPP
+#define HTTPREQUEST_HPP
+
+#include <cctype>
+#include <cstddef>
+#include <cstdint>
+#include <algorithm>
+#include <functional>
+#include <map>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <system_error>
+#include <type_traits>
+#include <vector>
+
+#ifdef _WIN32
+#  pragma push_macro("WIN32_LEAN_AND_MEAN")
+#  pragma push_macro("NOMINMAX")
+#  ifndef WIN32_LEAN_AND_MEAN
+#    define WIN32_LEAN_AND_MEAN
+#  endif
+#  ifndef NOMINMAX
+#    define NOMINMAX
+#  endif
+#  include <winsock2.h>
+#  if _WIN32_WINNT < _WIN32_WINNT_WINXP
+char* strdup(const char* src)
+{
+    std::size_t length = 0;
+    while (src[length]) ++length;
+    char* result = static_cast<char*>(malloc(length + 1));
+    char* p = result;
+    while (*src) *p++ = *src++;
+    *p = '\0';
+    return result;
+}
+#    include <wspiapi.h>
+#  endif
+#  include <ws2tcpip.h>
+#  pragma pop_macro("WIN32_LEAN_AND_MEAN")
+#  pragma pop_macro("NOMINMAX")
+#else
+#  include <sys/socket.h>
+#  include <netinet/in.h>
+#  include <netdb.h>
+#  include <unistd.h>
+#  include <errno.h>
+#endif
+
+namespace http
+{
+    class RequestError final: public std::logic_error
+    {
+    public:
+        explicit RequestError(const char* str): std::logic_error(str) {}
+        explicit RequestError(const std::string& str): std::logic_error(str) {}
+    };
+
+    class ResponseError final: public std::runtime_error
+    {
+    public:
+        explicit ResponseError(const char* str): std::runtime_error(str) {}
+        explicit ResponseError(const std::string& str): std::runtime_error(str) {}
+    };
+
+    enum class InternetProtocol: std::uint8_t
+    {
+        V4,
+        V6
+    };
+
+    inline namespace detail
+    {
+#ifdef _WIN32
+        class WinSock final
+        {
+        public:
+            WinSock()
+            {
+                WSADATA wsaData;
+                const auto error = WSAStartup(MAKEWORD(2, 2), &wsaData);
+                if (error != 0)
+                    throw std::system_error(error, std::system_category(), "WSAStartup failed");
+
+                if (LOBYTE(wsaData.wVersion) != 2 || HIBYTE(wsaData.wVersion) != 2)
+                {
+                    WSACleanup();
+                    throw std::runtime_error("Invalid WinSock version");
+                }
+
+                started = true;
+            }
+
+            ~WinSock()
+            {
+                if (started) WSACleanup();
+            }
+
+            WinSock(WinSock&& other) noexcept:
+                started(other.started)
+            {
+                other.started = false;
+            }
+
+            WinSock& operator=(WinSock&& other) noexcept
+            {
+                if (&other == this) return *this;
+                if (started) WSACleanup();
+                started = other.started;
+                other.started = false;
+                return *this;
+            }
+
+        private:
+            bool started = false;
+        };
+#endif
+
+        inline int getLastError() noexcept
+        {
+#ifdef _WIN32
+            return WSAGetLastError();
+#else
+            return errno;
+#endif
+        }
+
+        constexpr int getAddressFamily(InternetProtocol internetProtocol)
+        {
+            return (internetProtocol == InternetProtocol::V4) ? AF_INET :
+                (internetProtocol == InternetProtocol::V6) ? AF_INET6 :
+                throw RequestError("Unsupported protocol");
+        }
+
+#ifdef _WIN32
+        constexpr auto closeSocket = closesocket;
+#else
+        constexpr auto closeSocket = close;
+#endif
+
+#if defined(__APPLE__) || defined(_WIN32)
+        constexpr int noSignal = 0;
+#else
+        constexpr int noSignal = MSG_NOSIGNAL;
+#endif
+
+        class Socket final
+        {
+        public:
+#ifdef _WIN32
+            using Type = SOCKET;
+            static constexpr Type invalid = INVALID_SOCKET;
+#else
+            using Type = int;
+            static constexpr Type invalid = -1;
+#endif
+
+            explicit Socket(InternetProtocol internetProtocol):
+                endpoint(socket(getAddressFamily(internetProtocol), SOCK_STREAM, IPPROTO_TCP))
+            {
+                if (endpoint == invalid)
+                    throw std::system_error(getLastError(), std::system_category(), "Failed to create socket");
+
+#if defined(__APPLE__)
+                const int value = 1;
+                if (setsockopt(endpoint, SOL_SOCKET, SO_NOSIGPIPE, &value, sizeof(value)) == -1)
+                    throw std::system_error(getLastError(), std::system_category(), "Failed to set socket option");
+#endif
+            }
+
+            ~Socket()
+            {
+                if (endpoint != invalid) closeSocket(endpoint);
+            }
+
+            Socket(Socket&& other) noexcept:
+                endpoint(other.endpoint)
+            {
+                other.endpoint = invalid;
+            }
+
+            Socket& operator=(Socket&& other) noexcept
+            {
+                if (&other == this) return *this;
+                if (endpoint != invalid) closeSocket(endpoint);
+                endpoint = other.endpoint;
+                other.endpoint = invalid;
+                return *this;
+            }
+
+            void connect(const struct sockaddr* address, socklen_t addressSize)
+            {
+                auto result = ::connect(endpoint, address, addressSize);
+
+#ifdef _WIN32
+                while (result == -1 && WSAGetLastError() == WSAEINTR)
+                    result = ::connect(endpoint, address, addressSize);
+#else
+                while (result == -1 && errno == EINTR)
+                    result = ::connect(endpoint, address, addressSize);
+#endif
+
+                if (result == -1)
+                    throw std::system_error(getLastError(), std::system_category(), "Failed to connect");
+            }
+
+            size_t send(const void* buffer, size_t length, int flags)
+            {
+#ifdef _WIN32
+                auto result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
+                                     static_cast<int>(length), flags);
+
+                while (result == -1 && WSAGetLastError() == WSAEINTR)
+                    result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
+                                    static_cast<int>(length), flags);
+
+#else
+                auto result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
+                                     length, flags);
+
+                while (result == -1 && errno == EINTR)
+                    result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
+                                    length, flags);
+#endif
+                if (result == -1)
+                    throw std::system_error(getLastError(), std::system_category(), "Failed to send data");
+
+                return static_cast<size_t>(result);
+            }
+
+            size_t recv(void* buffer, size_t length, int flags)
+            {
+#ifdef _WIN32
+                auto result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
+                                     static_cast<int>(length), flags);
+
+                while (result == -1 && WSAGetLastError() == WSAEINTR)
+                    result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
+                                    static_cast<int>(length), flags);
+#else
+                auto result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
+                                     length, flags);
+
+                while (result == -1 && errno == EINTR)
+                    result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
+                                    length, flags);
+#endif
+                if (result == -1)
+                    throw std::system_error(getLastError(), std::system_category(), "Failed to read data");
+
+                return static_cast<size_t>(result);
+            }
+
+            operator Type() const noexcept { return endpoint; }
+
+        private:
+            Type endpoint = invalid;
+        };
+    }
+
+    inline std::string urlEncode(const std::string& str)
+    {
+        constexpr char hexChars[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
+
+        std::string result;
+
+        for (auto i = str.begin(); i != str.end(); ++i)
+        {
+            const std::uint8_t cp = *i & 0xFF;
+
+            if ((cp >= 0x30 && cp <= 0x39) || // 0-9
+                (cp >= 0x41 && cp <= 0x5A) || // A-Z
+                (cp >= 0x61 && cp <= 0x7A) || // a-z
+                cp == 0x2D || cp == 0x2E || cp == 0x5F) // - . _
+                result += static_cast<char>(cp);
+            else if (cp <= 0x7F) // length = 1
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+            else if ((cp >> 5) == 0x06) // length = 2
+            {
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+                if (++i == str.end()) break;
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+            }
+            else if ((cp >> 4) == 0x0E) // length = 3
+            {
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+                if (++i == str.end()) break;
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+                if (++i == str.end()) break;
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+            }
+            else if ((cp >> 3) == 0x1E) // length = 4
+            {
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+                if (++i == str.end()) break;
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+                if (++i == str.end()) break;
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+                if (++i == str.end()) break;
+                result += std::string("%") + hexChars[(*i & 0xF0) >> 4] + hexChars[*i & 0x0F];
+            }
+        }
+
+        return result;
+    }
+
+    struct Response final
+    {
+        enum Status
+        {
+            Continue = 100,
+            SwitchingProtocol = 101,
+            Processing = 102,
+            EarlyHints = 103,
+
+            Ok = 200,
+            Created = 201,
+            Accepted = 202,
+            NonAuthoritativeInformation = 203,
+            NoContent = 204,
+            ResetContent = 205,
+            PartialContent = 206,
+            MultiStatus = 207,
+            AlreadyReported = 208,
+            ImUsed = 226,
+
+            MultipleChoice = 300,
+            MovedPermanently = 301,
+            Found = 302,
+            SeeOther = 303,
+            NotModified = 304,
+            UseProxy = 305,
+            TemporaryRedirect = 307,
+            PermanentRedirect = 308,
+
+            BadRequest = 400,
+            Unauthorized = 401,
+            PaymentRequired = 402,
+            Forbidden = 403,
+            NotFound = 404,
+            MethodNotAllowed = 405,
+            NotAcceptable = 406,
+            ProxyAuthenticationRequired = 407,
+            RequestTimeout = 408,
+            Conflict = 409,
+            Gone = 410,
+            LengthRequired = 411,
+            PreconditionFailed = 412,
+            PayloadTooLarge = 413,
+            UriTooLong = 414,
+            UnsupportedMediaType = 415,
+            RangeNotSatisfiable = 416,
+            ExpectationFailed = 417,
+            ImaTeapot = 418,
+            MisdirectedRequest = 421,
+            UnprocessableEntity = 422,
+            Locked = 423,
+            FailedDependency = 424,
+            TooEarly = 425,
+            UpgradeRequired = 426,
+            PreconditionRequired = 428,
+            TooManyRequests = 429,
+            RequestHeaderFieldsTooLarge = 431,
+            UnavailableForLegalReasons = 451,
+
+            InternalServerError = 500,
+            NotImplemented = 501,
+            BadGateway = 502,
+            ServiceUnavailable = 503,
+            GatewayTimeout = 504,
+            HttpVersionNotSupported = 505,
+            VariantAlsoNegotiates = 506,
+            InsufficientStorage = 507,
+            LoopDetected = 508,
+            NotExtended = 510,
+            NetworkAuthenticationRequired = 511
+        };
+
+        int status = 0;
+        std::vector<std::string> headers;
+        std::vector<std::uint8_t> body;
+    };
+
+    class Request final
+    {
+    public:
+        explicit Request(const std::string& url,
+                         InternetProtocol protocol = InternetProtocol::V4):
+            internetProtocol(protocol)
+        {
+            const auto schemeEndPosition = url.find("://");
+
+            if (schemeEndPosition != std::string::npos)
+            {
+                scheme = url.substr(0, schemeEndPosition);
+                path = url.substr(schemeEndPosition + 3);
+            }
+            else
+            {
+                scheme = "http";
+                path = url;
+            }
+
+            const auto fragmentPosition = path.find('#');
+
+            // remove the fragment part
+            if (fragmentPosition != std::string::npos)
+                path.resize(fragmentPosition);
+
+            const auto pathPosition = path.find('/');
+
+            if (pathPosition == std::string::npos)
+            {
+                domain = path;
+                path = "/";
+            }
+            else
+            {
+                domain = path.substr(0, pathPosition);
+                path = path.substr(pathPosition);
+            }
+
+            const auto portPosition = domain.find(':');
+
+            if (portPosition != std::string::npos)
+            {
+                port = domain.substr(portPosition + 1);
+                domain.resize(portPosition);
+            }
+            else
+                port = "80";
+        }
+
+        Response send(const std::string& method,
+                      const std::map<std::string, std::string>& parameters,
+                      const std::vector<std::string>& headers = {})
+        {
+            std::string body;
+            bool first = true;
+
+            for (const auto& parameter : parameters)
+            {
+                if (!first) body += "&";
+                first = false;
+
+                body += urlEncode(parameter.first) + "=" + urlEncode(parameter.second);
+            }
+
+            return send(method, body, headers);
+        }
+
+        Response send(const std::string& method = "GET",
+                      const std::string& body = "",
+                      const std::vector<std::string>& headers = {})
+        {
+            return send(method,
+                        std::vector<uint8_t>(body.begin(), body.end()),
+                        headers);
+        }
+
+        Response send(const std::string& method,
+                      const std::vector<uint8_t>& body,
+                      const std::vector<std::string>& headers)
+        {
+            if (scheme != "http")
+                throw RequestError("Only HTTP scheme is supported");
+
+            addrinfo hints = {};
+            hints.ai_family = getAddressFamily(internetProtocol);
+            hints.ai_socktype = SOCK_STREAM;
+
+            addrinfo* info;
+            if (getaddrinfo(domain.c_str(), port.c_str(), &hints, &info) != 0)
+                throw std::system_error(getLastError(), std::system_category(), "Failed to get address info of " + domain);
+
+            std::unique_ptr<addrinfo, decltype(&freeaddrinfo)> addressInfo(info, freeaddrinfo);
+
+            std::string headerData = method + " " + path + " HTTP/1.1\r\n";
+
+            for (const std::string& header : headers)
+                headerData += header + "\r\n";
+
+            headerData += "Host: " + domain + "\r\n"
+                "Content-Length: " + std::to_string(body.size()) + "\r\n"
+                "\r\n";
+
+            std::vector<uint8_t> requestData(headerData.begin(), headerData.end());
+            requestData.insert(requestData.end(), body.begin(), body.end());
+
+            Socket socket(internetProtocol);
+
+            // take the first address from the list
+            socket.connect(addressInfo->ai_addr, static_cast<socklen_t>(addressInfo->ai_addrlen));
+
+            auto remaining = requestData.size();
+            auto sendData = requestData.data();
+
+            // send the request
+            while (remaining > 0)
+            {
+                const auto size = socket.send(sendData, remaining, noSignal);
+                remaining -= size;
+                sendData += size;
+            }
+
+            std::uint8_t tempBuffer[4096];
+            constexpr std::uint8_t crlf[] = {'\r', '\n'};
+            Response response;
+            std::vector<std::uint8_t> responseData;
+            bool firstLine = true;
+            bool parsedHeaders = false;
+            bool contentLengthReceived = false;
+            unsigned long contentLength = 0;
+            bool chunkedResponse = false;
+            std::size_t expectedChunkSize = 0;
+            bool removeCrlfAfterChunk = false;
+
+            // read the response
+            for (;;)
+            {
+                const auto size = socket.recv(tempBuffer, sizeof(tempBuffer), noSignal);
+
+                if (size == 0)
+                    break; // disconnected
+
+                responseData.insert(responseData.end(), tempBuffer, tempBuffer + size);
+
+                if (!parsedHeaders)
+                    for (;;)
+                    {
+                        const auto i = std::search(responseData.begin(), responseData.end(), std::begin(crlf), std::end(crlf));
+
+                        // didn't find a newline
+                        if (i == responseData.end()) break;
+
+                        const std::string line(responseData.begin(), i);
+                        responseData.erase(responseData.begin(), i + 2);
+
+                        // empty line indicates the end of the header section
+                        if (line.empty())
+                        {
+                            parsedHeaders = true;
+                            break;
+                        }
+                        else if (firstLine) // first line
+                        {
+                            firstLine = false;
+
+                            std::string::size_type lastPos = 0;
+                            const auto length = line.length();
+                            std::vector<std::string> parts;
+
+                            // tokenize first line
+                            while (lastPos < length + 1)
+                            {
+                                auto pos = line.find(' ', lastPos);
+                                if (pos == std::string::npos) pos = length;
+
+                                if (pos != lastPos)
+                                    parts.emplace_back(line.data() + lastPos,
+                                                       static_cast<std::vector<std::string>::size_type>(pos) - lastPos);
+
+                                lastPos = pos + 1;
+                            }
+
+                            if (parts.size() >= 2)
+                                response.status = std::stoi(parts[1]);
+                        }
+                        else // headers
+                        {
+                            response.headers.push_back(line);
+
+                            const auto pos = line.find(':');
+
+                            if (pos != std::string::npos)
+                            {
+                                std::string headerName = line.substr(0, pos);
+                                std::string headerValue = line.substr(pos + 1);
+
+                                // ltrim
+                                headerValue.erase(headerValue.begin(),
+                                                  std::find_if(headerValue.begin(), headerValue.end(),
+                                                               [](int c) {return !std::isspace(c);}));
+
+                                // rtrim
+                                headerValue.erase(std::find_if(headerValue.rbegin(), headerValue.rend(),
+                                                               [](int c) {return !std::isspace(c);}).base(),
+                                                  headerValue.end());
+
+                                if (headerName == "Content-Length")
+                                {
+                                    contentLength = std::stoul(headerValue);
+                                    contentLengthReceived = true;
+                                    response.body.reserve(contentLength);
+                                }
+                                else if (headerName == "Transfer-Encoding")
+                                {
+                                    if (headerValue == "chunked")
+                                        chunkedResponse = true;
+                                    else
+                                        throw ResponseError("Unsupported transfer encoding: " + headerValue);
+                                }
+                            }
+                        }
+                    }
+
+                if (parsedHeaders)
+                {
+                    // Content-Length must be ignored if Transfer-Encoding is received
+                    if (chunkedResponse)
+                    {
+                        bool dataReceived = false;
+                        for (;;)
+                        {
+                            if (expectedChunkSize > 0)
+                            {
+                                const auto toWrite = std::min(expectedChunkSize, responseData.size());
+                                response.body.insert(response.body.end(), responseData.begin(), responseData.begin() + static_cast<ptrdiff_t>(toWrite));
+                                responseData.erase(responseData.begin(), responseData.begin() + static_cast<ptrdiff_t>(toWrite));
+                                expectedChunkSize -= toWrite;
+
+                                if (expectedChunkSize == 0) removeCrlfAfterChunk = true;
+                                if (responseData.empty()) break;
+                            }
+                            else
+                            {
+                                if (removeCrlfAfterChunk)
+                                {
+                                    if (responseData.size() >= 2)
+                                    {
+                                        removeCrlfAfterChunk = false;
+                                        responseData.erase(responseData.begin(), responseData.begin() + 2);
+                                    }
+                                    else break;
+                                }
+
+                                const auto i = std::search(responseData.begin(), responseData.end(), std::begin(crlf), std::end(crlf));
+
+                                if (i == responseData.end()) break;
+
+                                const std::string line(responseData.begin(), i);
+                                responseData.erase(responseData.begin(), i + 2);
+
+                                expectedChunkSize = std::stoul(line, nullptr, 16);
+
+                                if (expectedChunkSize == 0)
+                                {
+                                    dataReceived = true;
+                                    break;
+                                }
+                            }
+                        }
+
+                        if (dataReceived)
+                            break;
+                    }
+                    else
+                    {
+                        response.body.insert(response.body.end(), responseData.begin(), responseData.end());
+                        responseData.clear();
+
+                        // got the whole content
+                        if (contentLengthReceived && response.body.size() >= contentLength)
+                            break;
+                    }
+                }
+            }
+
+            return response;
+        }
+
+    private:
+#ifdef _WIN32
+        WinSock winSock;
+#endif
+        InternetProtocol internetProtocol;
+        std::string scheme;
+        std::string domain;
+        std::string port;
+        std::string path;
+    };
+}
+
+#endif

src/external/open_dynamics_engine-ef/ode/IceAABB.cpp

@@ -0,0 +1,413 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains AABB-related code.
+ *	\file		IceAABB.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	AABB class.
+ *	\class		AABB
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the sum of two AABBs.
+ *	\param		aabb	[in] the other AABB
+ *	\return		Self-Reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABB& AABB::Add(const AABB& aabb)
+{
+	// Compute new min & max values
+	Point Min;	GetMin(Min);
+	Point Tmp;	aabb.GetMin(Tmp);
+	Min.Min(Tmp);
+
+	Point Max;	GetMax(Max);
+	aabb.GetMax(Tmp);
+	Max.Max(Tmp);
+
+	// Update this
+	SetMinMax(Min, Max);
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Makes a cube from the AABB.
+ *	\param		cube	[out] the cube AABB
+ *	\return		cube edge length
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float AABB::MakeCube(AABB& cube) const
+{
+	Point Ext;	GetExtents(Ext);
+	float Max = Ext.Max();
+
+	Point Cnt;	GetCenter(Cnt);
+	cube.SetCenterExtents(Cnt, Point(Max, Max, Max));
+	return Max;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Makes a sphere from the AABB.
+ *	\param		sphere	[out] sphere containing the AABB
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABB::MakeSphere(Sphere& sphere) const
+{
+	GetExtents(sphere.mCenter);
+	sphere.mRadius = sphere.mCenter.Magnitude() * 1.00001f;	// To make sure sphere::Contains(*this)	succeeds
+	GetCenter(sphere.mCenter);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks a box is inside another box.
+ *	\param		box		[in] the other AABB
+ *	\return		true if current box is inside input box
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABB::IsInside(const AABB& box) const
+{
+	if(box.GetMin(0)>GetMin(0))	return false;
+	if(box.GetMin(1)>GetMin(1))	return false;
+	if(box.GetMin(2)>GetMin(2))	return false;
+	if(box.GetMax(0)<GetMax(0))	return false;
+	if(box.GetMax(1)<GetMax(1))	return false;
+	if(box.GetMax(2)<GetMax(2))	return false;
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the AABB planes.
+ *	\param		planes	[out] 6 planes surrounding the box
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABB::ComputePlanes(Plane* planes)	const
+{
+	// Checkings
+	if(!planes)	return false;
+
+	Point Center, Extents;
+	GetCenter(Center);
+	GetExtents(Extents);
+
+	// Writes normals
+	planes[0].n = Point(1.0f, 0.0f, 0.0f);
+	planes[1].n = Point(-1.0f, 0.0f, 0.0f);
+	planes[2].n = Point(0.0f, 1.0f, 0.0f);
+	planes[3].n = Point(0.0f, -1.0f, 0.0f);
+	planes[4].n = Point(0.0f, 0.0f, 1.0f);
+	planes[5].n = Point(0.0f, 0.0f, -1.0f);
+
+	// Compute a point on each plane
+	Point p0 = Point(Center.x+Extents.x, Center.y, Center.z);
+	Point p1 = Point(Center.x-Extents.x, Center.y, Center.z);
+	Point p2 = Point(Center.x, Center.y+Extents.y, Center.z);
+	Point p3 = Point(Center.x, Center.y-Extents.y, Center.z);
+	Point p4 = Point(Center.x, Center.y, Center.z+Extents.z);
+	Point p5 = Point(Center.x, Center.y, Center.z-Extents.z);
+
+	// Compute d
+	planes[0].d = -(planes[0].n|p0);
+	planes[1].d = -(planes[1].n|p1);
+	planes[2].d = -(planes[2].n|p2);
+	planes[3].d = -(planes[3].n|p3);
+	planes[4].d = -(planes[4].n|p4);
+	planes[5].d = -(planes[5].n|p5);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the aabb points.
+ *	\param		pts	[out] 8 box points
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABB::ComputePoints(Point* pts)	const
+{
+	// Checkings
+	if(!pts)	return false;
+
+	// Get box corners
+	Point min;	GetMin(min);
+	Point max;	GetMax(max);
+
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+	// Generate 8 corners of the bbox
+	pts[0] = Point(min.x, min.y, min.z);
+	pts[1] = Point(max.x, min.y, min.z);
+	pts[2] = Point(max.x, max.y, min.z);
+	pts[3] = Point(min.x, max.y, min.z);
+	pts[4] = Point(min.x, min.y, max.z);
+	pts[5] = Point(max.x, min.y, max.z);
+	pts[6] = Point(max.x, max.y, max.z);
+	pts[7] = Point(min.x, max.y, max.z);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets vertex normals.
+ *	param		pts	[out] 8 box points
+ *	return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const Point* AABB::GetVertexNormals()	const
+{
+	static float VertexNormals[] =
+	{
+		-INVSQRT3,	-INVSQRT3,	-INVSQRT3,
+		INVSQRT3,	-INVSQRT3,	-INVSQRT3,
+		INVSQRT3,	INVSQRT3,	-INVSQRT3,
+		-INVSQRT3,	INVSQRT3,	-INVSQRT3,
+		-INVSQRT3,	-INVSQRT3,	INVSQRT3,
+		INVSQRT3,	-INVSQRT3,	INVSQRT3,
+		INVSQRT3,	INVSQRT3,	INVSQRT3,
+		-INVSQRT3,	INVSQRT3,	INVSQRT3
+	};
+	return (const Point*)VertexNormals;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Returns edges.
+ *	\return		24 indices (12 edges) indexing the list returned by ComputePoints()
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const udword* AABB::GetEdges() const
+{
+	static udword Indices[] = {
+	0, 1,	1, 2,	2, 3,	3, 0,
+	7, 6,	6, 5,	5, 4,	4, 7,
+	1, 5,	6, 2,
+	3, 7,	4, 0
+	};
+	return Indices;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Returns edge normals.
+ *	\return		edge normals in local space
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const Point* AABB::GetEdgeNormals() const
+{
+	static float EdgeNormals[] =
+	{
+		0,			-INVSQRT2,	-INVSQRT2,	// 0-1
+		INVSQRT2,	0,			-INVSQRT2,	// 1-2
+		0,			INVSQRT2,	-INVSQRT2,	// 2-3
+		-INVSQRT2,	0,			-INVSQRT2,	// 3-0
+
+		0,			INVSQRT2,	INVSQRT2,	// 7-6
+		INVSQRT2,	0,			INVSQRT2,	// 6-5
+		0,			-INVSQRT2,	INVSQRT2,	// 5-4
+		-INVSQRT2,	0,			INVSQRT2,	// 4-7
+
+		INVSQRT2,	-INVSQRT2,	0,			// 1-5
+		INVSQRT2,	INVSQRT2,	0,			// 6-2
+		-INVSQRT2,	INVSQRT2,	0,			// 3-7
+		-INVSQRT2,	-INVSQRT2,	0			// 4-0
+	};
+	return (const Point*)EdgeNormals;
+}
+
+// ===========================================================================
+//  (C) 1996-98 Vienna University of Technology
+// ===========================================================================
+//  NAME:       bboxarea
+//  TYPE:       c++ code
+//  PROJECT:    Bounding Box Area
+//  CONTENT:    Computes area of 2D projection of 3D oriented bounding box
+//  VERSION:    1.0
+// ===========================================================================
+//  AUTHORS:    ds      Dieter Schmalstieg
+//              ep      Erik Pojar
+// ===========================================================================
+//  HISTORY:
+//
+//  19-sep-99 15:23:03  ds      last modification
+//  01-dec-98 15:23:03  ep      created
+// ===========================================================================
+
+//----------------------------------------------------------------------------
+// SAMPLE CODE STARTS HERE
+//----------------------------------------------------------------------------
+
+// NOTE: This sample program requires OPEN INVENTOR!
+
+//indexlist: this table stores the 64 possible cases of classification of
+//the eyepoint with respect to the 6 defining planes of the bbox (2^6=64)
+//only 26 (3^3-1, where 1 is "inside" cube) of these cases are valid.
+//the first 6 numbers in each row are the indices of the bbox vertices that
+//form the outline of which we want to compute the area (counterclockwise
+//ordering), the 7th entry means the number of vertices in the outline.
+//there are 6 cases with a single face and and a 4-vertex outline, and
+//20 cases with 2 or 3 faces and a 6-vertex outline. a value of 0 indicates
+//an invalid case.
+
+
+// Original list was made of 7 items, I added an 8th element:
+// - to padd on a cache line
+// - to repeat the first entry to avoid modulos
+//
+// I also replaced original ints with sbytes.
+
+static const sbyte gIndexList[64][8] =
+{
+    {-1,-1,-1,-1,-1,-1,-1,   0}, // 0 inside
+    { 0, 4, 7, 3, 0,-1,-1,   4}, // 1 left
+    { 1, 2, 6, 5, 1,-1,-1,   4}, // 2 right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, // 3 -
+    { 0, 1, 5, 4, 0,-1,-1,   4}, // 4 bottom
+    { 0, 1, 5, 4, 7, 3, 0,   6}, // 5 bottom, left
+    { 0, 1, 2, 6, 5, 4, 0,   6}, // 6 bottom, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, // 7 -
+    { 2, 3, 7, 6, 2,-1,-1,   4}, // 8 top
+    { 0, 4, 7, 6, 2, 3, 0,   6}, // 9 top, left
+    { 1, 2, 3, 7, 6, 5, 1,   6}, //10 top, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //11 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //12 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //13 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //14 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //15 -
+    { 0, 3, 2, 1, 0,-1,-1,   4}, //16 front
+    { 0, 4, 7, 3, 2, 1, 0,   6}, //17 front, left
+    { 0, 3, 2, 6, 5, 1, 0,   6}, //18 front, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //19 -
+    { 0, 3, 2, 1, 5, 4, 0,   6}, //20 front, bottom
+    { 1, 5, 4, 7, 3, 2, 1,   6}, //21 front, bottom, left
+    { 0, 3, 2, 6, 5, 4, 0,   6}, //22 front, bottom, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //23 -
+    { 0, 3, 7, 6, 2, 1, 0,   6}, //24 front, top
+    { 0, 4, 7, 6, 2, 1, 0,   6}, //25 front, top, left
+    { 0, 3, 7, 6, 5, 1, 0,   6}, //26 front, top, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //27 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //28 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //29 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //30 -
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //31 -
+    { 4, 5, 6, 7, 4,-1,-1,   4}, //32 back
+    { 0, 4, 5, 6, 7, 3, 0,   6}, //33 back, left
+    { 1, 2, 6, 7, 4, 5, 1,   6}, //34 back, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //35 -
+    { 0, 1, 5, 6, 7, 4, 0,   6}, //36 back, bottom
+    { 0, 1, 5, 6, 7, 3, 0,   6}, //37 back, bottom, left
+    { 0, 1, 2, 6, 7, 4, 0,   6}, //38 back, bottom, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //39 -
+    { 2, 3, 7, 4, 5, 6, 2,   6}, //40 back, top
+    { 0, 4, 5, 6, 2, 3, 0,   6}, //41 back, top, left
+    { 1, 2, 3, 7, 4, 5, 1,   6}, //42 back, top, right
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //43 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //44 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //45 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //46 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //47 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //48 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //49 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //50 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //51 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //52 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //53 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //54 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //55 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //56 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //57 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //58 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //59 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //60 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //61 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}, //62 invalid
+    {-1,-1,-1,-1,-1,-1,-1,   0}  //63 invalid
+};
+
+const sbyte* AABB::ComputeOutline(const Point& local_eye, sdword& num)	const
+{
+	// Get box corners
+	Point min;	GetMin(min);
+	Point max;	GetMax(max);
+
+	// Compute 6-bit code to classify eye with respect to the 6 defining planes of the bbox
+	int pos = ((local_eye.x < min.x) ?  1 : 0)	// 1 = left
+			+ ((local_eye.x > max.x) ?  2 : 0)	// 2 = right
+			+ ((local_eye.y < min.y) ?  4 : 0)	// 4 = bottom
+			+ ((local_eye.y > max.y) ?  8 : 0)	// 8 = top
+			+ ((local_eye.z < min.z) ? 16 : 0)	// 16 = front
+			+ ((local_eye.z > max.z) ? 32 : 0);	// 32 = back
+
+	// Look up number of vertices in outline
+	num = (sdword)gIndexList[pos][7];
+	// Zero indicates invalid case
+	if(!num) return null;
+
+	return &gIndexList[pos][0];
+}
+
+// calculateBoxArea: computes the screen-projected 2D area of an oriented 3D bounding box
+
+//const Point&		eye,		//eye point (in bbox object coordinates)
+//const AABB&			box,		//3d bbox
+//const Matrix4x4&	mat,		//free transformation for bbox
+//float width, float height, int& num)
+float AABB::ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num)	const
+{
+	const sbyte* Outline = ComputeOutline(eye, num);
+	if(!Outline)	return -1.0f;
+
+	// Compute box vertices
+	Point vertexBox[8], dst[8];
+	ComputePoints(vertexBox);
+
+	// Transform all outline corners into 2D screen space
+	for(sdword i=0;i<num;i++)
+	{
+		HPoint Projected;
+		vertexBox[Outline[i]].ProjectToScreen(width, height, mat, Projected);
+		dst[i] = Projected;
+	}
+
+	float Sum = (dst[num-1][0] - dst[0][0]) * (dst[num-1][1] + dst[0][1]);
+
+	for(int i=0; i<num-1; i++)
+		Sum += (dst[i][0] - dst[i+1][0]) * (dst[i][1] + dst[i+1][1]);
+
+	return Sum * 0.5f;	//return computed value corrected by 0.5
+}

src/external/open_dynamics_engine-ef/ode/IceAABB.h

@@ -0,0 +1,505 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains AABB-related code. (axis-aligned bounding box)
+ *	\file		IceAABB.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 13, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEAABB_H__
+#define __ICEAABB_H__
+
+	// Forward declarations
+	class Sphere;
+
+//! Declarations of type-independent methods (most of them implemented in the .cpp)
+#define AABB_COMMON_METHODS																											\
+			AABB&			Add(const AABB& aabb);																					\
+			float			MakeCube(AABB& cube)																			const;	\
+			void			MakeSphere(Sphere& sphere)																		const;	\
+			const sbyte*	ComputeOutline(const Point& local_eye, sdword& num)												const;	\
+			float			ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num)	const;	\
+			bool			IsInside(const AABB& box)																		const;	\
+			bool			ComputePlanes(Plane* planes)																	const;	\
+			bool			ComputePoints(Point* pts)																		const;	\
+			const Point*	GetVertexNormals()																				const;	\
+			const udword*	GetEdges()																						const;	\
+			const Point*	GetEdgeNormals()																				const;	\
+	inline_	BOOL			ContainsPoint(const Point& p)																	const	\
+							{																										\
+								if(p.x > GetMax(0) || p.x < GetMin(0)) return FALSE;												\
+								if(p.y > GetMax(1) || p.y < GetMin(1)) return FALSE;												\
+								if(p.z > GetMax(2) || p.z < GetMin(2)) return FALSE;												\
+								return TRUE;																						\
+							}
+
+	enum AABBType
+	{
+		AABB_RENDER			= 0,	//!< AABB used for rendering. Not visible == not rendered.
+		AABB_UPDATE			= 1,	//!< AABB used for dynamic updates. Not visible == not updated.
+
+		AABB_FORCE_DWORD	= 0x7fffffff,
+	};
+
+#ifdef USE_MINMAX
+
+	struct ICEMATHS_API ShadowAABB
+	{
+		Point	mMin;
+		Point	mMax;
+	};
+
+	class ICEMATHS_API AABB
+	{
+		public:
+		//! Constructor
+		inline_						AABB()	{}
+		//! Destructor
+		inline_						~AABB()	{}
+
+		//! Type-independent methods
+									AABB_COMMON_METHODS;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an AABB from min & max vectors.
+		 *	\param		min			[in] the min point
+		 *	\param		max			[in] the max point
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetMinMax(const Point& min, const Point& max)		{ mMin = min;		mMax = max;									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an AABB from center & extents vectors.
+		 *	\param		c			[in] the center point
+		 *	\param		e			[in] the extents vector
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetCenterExtents(const Point& c, const Point& e)	{ mMin = c - e;		mMax = c + e;								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an empty AABB.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetEmpty()											{ Point p(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);	mMin = -p; mMax = p;}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups a point AABB.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetPoint(const Point& pt)							{ mMin = mMax = pt;												}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the size of the AABB. The size is defined as the longest extent.
+		 *	\return		the size of the AABB
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						float		GetSize()								const		{ Point e; GetExtents(e);	return e.Max();	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Extends the AABB.
+		 *	\param		p	[in] the next point
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		Extend(const Point& p)
+									{
+										if(p.x > mMax.x)	mMax.x = p.x;
+										if(p.x < mMin.x)	mMin.x = p.x;
+
+										if(p.y > mMax.y)	mMax.y = p.y;
+										if(p.y < mMin.y)	mMin.y = p.y;
+
+										if(p.z > mMax.z)	mMax.z = p.z;
+										if(p.z < mMin.z)	mMin.z = p.z;
+									}
+		// Data access
+
+		//! Get min point of the box
+		inline_			void		GetMin(Point& min)						const		{ min = mMin;								}
+		//! Get max point of the box
+		inline_			void		GetMax(Point& max)						const		{ max = mMax;								}
+
+		//! Get component of the box's min point along a given axis
+		inline_			float		GetMin(udword axis)						const		{ return mMin[axis];						}
+		//! Get component of the box's max point along a given axis
+		inline_			float		GetMax(udword axis)						const		{ return mMax[axis];						}
+
+		//! Get box center
+		inline_			void		GetCenter(Point& center)				const		{ center = (mMax + mMin)*0.5f;				}
+		//! Get box extents
+		inline_			void		GetExtents(Point& extents)				const		{ extents = (mMax - mMin)*0.5f;				}
+
+		//! Get component of the box's center along a given axis
+		inline_			float		GetCenter(udword axis)					const		{ return (mMax[axis] + mMin[axis])*0.5f;	}
+		//! Get component of the box's extents along a given axis
+		inline_			float		GetExtents(udword axis)					const		{ return (mMax[axis] - mMin[axis])*0.5f;	}
+
+		//! Get box diagonal
+		inline_			void		GetDiagonal(Point& diagonal)			const		{ diagonal = mMax - mMin;					}
+		inline_			float		GetWidth()								const		{ return mMax.x - mMin.x;					}
+		inline_			float		GetHeight()								const		{ return mMax.y - mMin.y;					}
+		inline_			float		GetDepth()								const		{ return mMax.z - mMin.z;					}
+
+		//! Volume
+		inline_			float		GetVolume()								const		{ return GetWidth() * GetHeight() * GetDepth();		}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the intersection between two AABBs.
+		 *	\param		a		[in] the other AABB
+		 *	\return		true on intersection
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL		Intersect(const AABB& a)				const
+									{
+										if(mMax.x < a.mMin.x
+										|| a.mMax.x < mMin.x
+										|| mMax.y < a.mMin.y
+										|| a.mMax.y < mMin.y
+										|| mMax.z < a.mMin.z
+										|| a.mMax.z < mMin.z)	return FALSE;
+
+										return TRUE;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the 1D-intersection between two AABBs, on a given axis.
+		 *	\param		a		[in] the other AABB
+		 *	\param		axis	[in] the axis (0, 1, 2)
+		 *	\return		true on intersection
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL		Intersect(const AABB& a, udword axis)	const
+									{
+										if(mMax[axis] < a.mMin[axis] || a.mMax[axis] < mMin[axis])	return FALSE;
+										return TRUE;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Recomputes the AABB after an arbitrary transform by a 4x4 matrix.
+		 *	Original code by Charles Bloom on the GD-Algorithm list. (I slightly modified it)
+		 *	\param		mtx			[in] the transform matrix
+		 *	\param		aabb		[out] the transformed AABB [can be *this]
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			void		Rotate(const Matrix4x4& mtx, AABB& aabb)	const
+									{
+										// The three edges transformed: you can efficiently transform an X-only vector
+										// by just getting the "X" column of the matrix
+										Point vx,vy,vz;
+										mtx.GetRow(0, vx);	vx *= (mMax.x - mMin.x);
+										mtx.GetRow(1, vy);	vy *= (mMax.y - mMin.y);
+										mtx.GetRow(2, vz);	vz *= (mMax.z - mMin.z);
+
+										// Transform the min point
+										aabb.mMin = aabb.mMax = mMin * mtx;
+
+										// Take the transformed min & axes and find new extents
+										// Using CPU code in the right place is faster...
+										if(IS_NEGATIVE_FLOAT(vx.x))	aabb.mMin.x += vx.x; else aabb.mMax.x += vx.x;
+										if(IS_NEGATIVE_FLOAT(vx.y))	aabb.mMin.y += vx.y; else aabb.mMax.y += vx.y;
+										if(IS_NEGATIVE_FLOAT(vx.z))	aabb.mMin.z += vx.z; else aabb.mMax.z += vx.z;
+										if(IS_NEGATIVE_FLOAT(vy.x))	aabb.mMin.x += vy.x; else aabb.mMax.x += vy.x;
+										if(IS_NEGATIVE_FLOAT(vy.y))	aabb.mMin.y += vy.y; else aabb.mMax.y += vy.y;
+										if(IS_NEGATIVE_FLOAT(vy.z))	aabb.mMin.z += vy.z; else aabb.mMax.z += vy.z;
+										if(IS_NEGATIVE_FLOAT(vz.x))	aabb.mMin.x += vz.x; else aabb.mMax.x += vz.x;
+										if(IS_NEGATIVE_FLOAT(vz.y))	aabb.mMin.y += vz.y; else aabb.mMax.y += vz.y;
+										if(IS_NEGATIVE_FLOAT(vz.z))	aabb.mMin.z += vz.z; else aabb.mMax.z += vz.z;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the AABB is valid.
+		 *	\return		true if the box is valid
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL		IsValid()	const
+									{
+										// Consistency condition for (Min, Max) boxes: min < max
+										if(mMin.x > mMax.x)	return FALSE;
+										if(mMin.y > mMax.y)	return FALSE;
+										if(mMin.z > mMax.z)	return FALSE;
+										return TRUE;
+									}
+
+		//! Operator for AABB *= float. Scales the extents, keeps same center.
+		inline_			AABB&		operator*=(float s)
+									{
+										Point Center;	GetCenter(Center);
+										Point Extents;	GetExtents(Extents);
+										SetCenterExtents(Center, Extents * s);
+										return *this;
+									}
+
+		//! Operator for AABB /= float. Scales the extents, keeps same center.
+		inline_			AABB&		operator/=(float s)
+									{
+										Point Center;	GetCenter(Center);
+										Point Extents;	GetExtents(Extents);
+										SetCenterExtents(Center, Extents / s);
+										return *this;
+									}
+
+		//! Operator for AABB += Point. Translates the box.
+		inline_			AABB&		operator+=(const Point& trans)
+									{
+										mMin+=trans;
+										mMax+=trans;
+										return *this;
+									}
+		private:
+						Point		mMin;			//!< Min point
+						Point		mMax;			//!< Max point
+	};
+
+#else
+
+	class ICEMATHS_API AABB
+	{
+		public:
+		//! Constructor
+		inline_						AABB()	{}
+		//! Destructor
+		inline_						~AABB()	{}
+
+		//! Type-independent methods
+									AABB_COMMON_METHODS;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an AABB from min & max vectors.
+		 *	\param		min			[in] the min point
+		 *	\param		max			[in] the max point
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetMinMax(const Point& min, const Point& max)		{ mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f;		}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an AABB from center & extents vectors.
+		 *	\param		c			[in] the center point
+		 *	\param		e			[in] the extents vector
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetCenterExtents(const Point& c, const Point& e)	{ mCenter = c;	 mExtents = e;									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an empty AABB.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetEmpty()											{ mCenter.Zero(); mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups a point AABB.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		SetPoint(const Point& pt)							{ mCenter = pt; mExtents.Zero();								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the size of the AABB. The size is defined as the longest extent.
+		 *	\return		the size of the AABB
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						float		GetSize()								const		{ return mExtents.Max();					}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Extends the AABB.
+		 *	\param		p	[in] the next point
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						void		Extend(const Point& p)
+									{
+										Point Max = mCenter + mExtents;
+										Point Min = mCenter - mExtents;
+
+										if(p.x > Max.x)	Max.x = p.x;
+										if(p.x < Min.x)	Min.x = p.x;
+
+										if(p.y > Max.y)	Max.y = p.y;
+										if(p.y < Min.y)	Min.y = p.y;
+
+										if(p.z > Max.z)	Max.z = p.z;
+										if(p.z < Min.z)	Min.z = p.z;
+
+										SetMinMax(Min, Max);
+									}
+		// Data access
+
+		//! Get min point of the box
+		inline_			void		GetMin(Point& min)						const		{ min = mCenter - mExtents;					}
+		//! Get max point of the box
+		inline_			void		GetMax(Point& max)						const		{ max = mCenter + mExtents;					}
+
+		//! Get component of the box's min point along a given axis
+		inline_			float		GetMin(udword axis)						const		{ return mCenter[axis] - mExtents[axis];	}
+		//! Get component of the box's max point along a given axis
+		inline_			float		GetMax(udword axis)						const		{ return mCenter[axis] + mExtents[axis];	}
+
+		//! Get box center
+		inline_			void		GetCenter(Point& center)				const		{ center = mCenter;							}
+		//! Get box extents
+		inline_			void		GetExtents(Point& extents)				const		{ extents = mExtents;						}
+
+		//! Get component of the box's center along a given axis
+		inline_			float		GetCenter(udword axis)					const		{ return mCenter[axis];						}
+		//! Get component of the box's extents along a given axis
+		inline_			float		GetExtents(udword axis)					const		{ return mExtents[axis];					}
+
+		//! Get box diagonal
+		inline_			void		GetDiagonal(Point& diagonal)			const		{ diagonal = mExtents * 2.0f;				}
+		inline_			float		GetWidth()								const		{ return mExtents.x * 2.0f;					}
+		inline_			float		GetHeight()								const		{ return mExtents.y * 2.0f;					}
+		inline_			float		GetDepth()								const		{ return mExtents.z * 2.0f;					}
+
+		//! Volume
+		inline_			float		GetVolume()								const		{ return mExtents.x * mExtents.y * mExtents.z * 8.0f;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the intersection between two AABBs.
+		 *	\param		a		[in] the other AABB
+		 *	\return		true on intersection
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL		Intersect(const AABB& a)				const
+									{
+										float tx = mCenter.x - a.mCenter.x;	float ex = a.mExtents.x + mExtents.x;	if(AIR(tx) > IR(ex))	return FALSE;
+										float ty = mCenter.y - a.mCenter.y;	float ey = a.mExtents.y + mExtents.y;	if(AIR(ty) > IR(ey))	return FALSE;
+										float tz = mCenter.z - a.mCenter.z;	float ez = a.mExtents.z + mExtents.z;	if(AIR(tz) > IR(ez))	return FALSE;
+										return TRUE;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	The standard intersection method from Gamasutra. Just here to check its speed against the one above.
+		 *	\param		a		[in] the other AABB
+		 *	\return		true on intersection
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			bool		GomezIntersect(const AABB& a)
+									{
+										Point	T = mCenter - a.mCenter;	// Vector from A to B
+										return	((fabsf(T.x) <= (a.mExtents.x + mExtents.x))
+												&& (fabsf(T.y) <= (a.mExtents.y + mExtents.y))
+												&& (fabsf(T.z) <= (a.mExtents.z + mExtents.z)));
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the 1D-intersection between two AABBs, on a given axis.
+		 *	\param		a		[in] the other AABB
+		 *	\param		axis	[in] the axis (0, 1, 2)
+		 *	\return		true on intersection
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL		Intersect(const AABB& a, udword axis)	const
+									{
+										float t = mCenter[axis] - a.mCenter[axis];
+										float e = a.mExtents[axis] + mExtents[axis];
+										if(AIR(t) > IR(e))	return FALSE;
+										return TRUE;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Recomputes the AABB after an arbitrary transform by a 4x4 matrix.
+		 *	\param		mtx			[in] the transform matrix
+		 *	\param		aabb		[out] the transformed AABB [can be *this]
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			void		Rotate(const Matrix4x4& mtx, AABB& aabb)	const
+									{
+										// Compute new center
+										aabb.mCenter = mCenter * mtx;
+
+										// Compute new extents. FPU code & CPU code have been interleaved for improved performance.
+										Point Ex(mtx.m[0][0] * mExtents.x, mtx.m[0][1] * mExtents.x, mtx.m[0][2] * mExtents.x);
+										IR(Ex.x)&=0x7fffffff;	IR(Ex.y)&=0x7fffffff;	IR(Ex.z)&=0x7fffffff;
+
+										Point Ey(mtx.m[1][0] * mExtents.y, mtx.m[1][1] * mExtents.y, mtx.m[1][2] * mExtents.y);
+										IR(Ey.x)&=0x7fffffff;	IR(Ey.y)&=0x7fffffff;	IR(Ey.z)&=0x7fffffff;
+
+										Point Ez(mtx.m[2][0] * mExtents.z, mtx.m[2][1] * mExtents.z, mtx.m[2][2] * mExtents.z);
+										IR(Ez.x)&=0x7fffffff;	IR(Ez.y)&=0x7fffffff;	IR(Ez.z)&=0x7fffffff;
+
+										aabb.mExtents.x = Ex.x + Ey.x + Ez.x;
+										aabb.mExtents.y = Ex.y + Ey.y + Ez.y;
+										aabb.mExtents.z = Ex.z + Ey.z + Ez.z;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the AABB is valid.
+		 *	\return		true if the box is valid
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL		IsValid()	const
+									{
+										// Consistency condition for (Center, Extents) boxes: Extents >= 0
+										if(IS_NEGATIVE_FLOAT(mExtents.x))	return FALSE;
+										if(IS_NEGATIVE_FLOAT(mExtents.y))	return FALSE;
+										if(IS_NEGATIVE_FLOAT(mExtents.z))	return FALSE;
+										return TRUE;
+									}
+
+		//! Operator for AABB *= float. Scales the extents, keeps same center.
+		inline_			AABB&		operator*=(float s)		{ mExtents*=s;	return *this;	}
+
+		//! Operator for AABB /= float. Scales the extents, keeps same center.
+		inline_			AABB&		operator/=(float s)		{ mExtents/=s;	return *this;	}
+
+		//! Operator for AABB += Point. Translates the box.
+		inline_			AABB&		operator+=(const Point& trans)
+									{
+										mCenter+=trans;
+										return *this;
+									}
+		private:
+						Point		mCenter;			//!< AABB Center
+						Point		mExtents;			//!< x, y and z extents
+	};
+
+#endif
+
+	inline_ void ComputeMinMax(const Point& p, Point& min, Point& max)
+	{
+		if(p.x > max.x)	max.x = p.x;
+		if(p.x < min.x)	min.x = p.x;
+
+		if(p.y > max.y)	max.y = p.y;
+		if(p.y < min.y)	min.y = p.y;
+
+		if(p.z > max.z)	max.z = p.z;
+		if(p.z < min.z)	min.z = p.z;
+	}
+
+	inline_ void ComputeAABB(AABB& aabb, const Point* list, udword nb_pts)
+	{
+		if(list)
+		{
+			Point Maxi(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
+			Point Mini(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT);
+			while(nb_pts--)
+			{
+//				_prefetch(list+1);	// off by one ?
+				ComputeMinMax(*list++, Mini, Maxi);
+			}
+			aabb.SetMinMax(Mini, Maxi);
+		}
+	}
+
+#endif	// __ICEAABB_H__

src/external/open_dynamics_engine-ef/ode/IceAxes.h

@@ -0,0 +1,60 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains axes definition.
+ *	\file		IceAxes.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEAXES_H__
+#define __ICEAXES_H__
+
+
+// ericf added
+#ifdef _X
+#undef _X
+#endif
+
+	enum PointComponent
+	{
+		_X					= 0,
+		_Y					= 1,
+		_Z					= 2,
+		_W					= 3,
+
+		_FORCE_DWORD		= 0x7fffffff
+	};
+
+	enum AxisOrder
+	{
+		AXES_XYZ			= (_X)|(_Y<<2)|(_Z<<4),
+		AXES_XZY			= (_X)|(_Z<<2)|(_Y<<4),
+		AXES_YXZ			= (_Y)|(_X<<2)|(_Z<<4),
+		AXES_YZX			= (_Y)|(_Z<<2)|(_X<<4),
+		AXES_ZXY			= (_Z)|(_X<<2)|(_Y<<4),
+		AXES_ZYX			= (_Z)|(_Y<<2)|(_X<<4),
+
+		AXES_FORCE_DWORD	= 0x7fffffff
+	};
+
+	class ICEMATHS_API Axes
+	{
+		public:
+
+		inline_			Axes(AxisOrder order)
+						{
+							mAxis0 = (order   ) & 3;
+							mAxis1 = (order>>2) & 3;
+							mAxis2 = (order>>4) & 3;
+						}
+		inline_			~Axes()		{}
+
+				udword	mAxis0;
+				udword	mAxis1;
+				udword	mAxis2;
+	};
+
+#endif // __ICEAXES_H__

src/external/open_dynamics_engine-ef/ode/IceBoundingSphere.h

@@ -0,0 +1,142 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code to compute the minimal bounding sphere.
+ *	\file		IceBoundingSphere.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEBOUNDINGSPHERE_H__
+#define __ICEBOUNDINGSPHERE_H__
+
+	enum BSphereMethod
+	{
+		BS_NONE,
+		BS_GEMS,
+		BS_MINIBALL,
+
+		BS_FORCE_DWORD	= 0x7fffffff
+	};
+
+	class ICEMATHS_API Sphere
+	{
+		public:
+		//! Constructor
+		inline_					Sphere()																		{}
+		//! Constructor
+		inline_					Sphere(const Point& center, float radius) : mCenter(center), mRadius(radius)	{}
+		//! Constructor
+								Sphere(udword nb_verts, const Point* verts);
+		//! Copy constructor
+		inline_					Sphere(const Sphere& sphere) : mCenter(sphere.mCenter), mRadius(sphere.mRadius)	{}
+		//! Destructor
+		inline_					~Sphere()																		{}
+
+				BSphereMethod	Compute(udword nb_verts, const Point* verts);
+				bool			FastCompute(udword nb_verts, const Point* verts);
+
+		// Access methods
+		inline_	const Point&	GetCenter()						const		{ return mCenter; }
+		inline_	float			GetRadius()						const		{ return mRadius; }
+
+		inline_	const Point&	Center()						const		{ return mCenter; }
+		inline_	float			Radius()						const		{ return mRadius; }
+
+		inline_	Sphere&			Set(const Point& center, float radius)		{ mCenter = center; mRadius = radius; return *this; }
+		inline_	Sphere&			SetCenter(const Point& center)				{ mCenter = center; return *this; }
+		inline_	Sphere&			SetRadius(float radius)						{ mRadius = radius; return *this; }
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if a point is contained within the sphere.
+		 *	\param		p	[in] the point to test
+		 *	\return		true if inside the sphere
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	bool			Contains(const Point& p)		const
+								{
+									return mCenter.SquareDistance(p) <= mRadius*mRadius;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if a sphere is contained within the sphere.
+		 *	\param		sphere	[in] the sphere to test
+		 *	\return		true if inside the sphere
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	bool			Contains(const Sphere& sphere)	const
+								{
+									// If our radius is the smallest, we can't possibly contain the other sphere
+									if(mRadius < sphere.mRadius)	return false;
+									// So r is always positive or null now
+									float r = mRadius - sphere.mRadius;
+									return mCenter.SquareDistance(sphere.mCenter) <= r*r;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if a box is contained within the sphere.
+		 *	\param		aabb	[in] the box to test
+		 *	\return		true if inside the sphere
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_ BOOL			Contains(const AABB& aabb)	const
+								{
+									// I assume if all 8 box vertices are inside the sphere, so does the whole box.
+									// Sounds ok but maybe there's a better way?
+									float R2 = mRadius * mRadius;
+#ifdef USE_MIN_MAX
+									const Point& Max = ((ShadowAABB&)&aabb).mMax;
+									const Point& Min = ((ShadowAABB&)&aabb).mMin;
+#else
+									Point Max; aabb.GetMax(Max);
+									Point Min; aabb.GetMin(Min);
+#endif
+									Point p;
+									p.x=Max.x; p.y=Max.y; p.z=Max.z;	if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Min.x;							if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Max.x; p.y=Min.y;				if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Min.x;							if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Max.x; p.y=Max.y; p.z=Min.z;	if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Min.x;							if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Max.x; p.y=Min.y;				if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+									p.x=Min.x;							if(mCenter.SquareDistance(p)>=R2)	return FALSE;
+
+									return TRUE;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if the sphere intersects another sphere
+		 *	\param		sphere	[in] the other sphere
+		 *	\return		true if spheres overlap
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	bool			Intersect(const Sphere& sphere)	const
+								{
+									float r = mRadius + sphere.mRadius;
+									return mCenter.SquareDistance(sphere.mCenter) <= r*r;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the sphere is valid.
+		 *	\return		true if the box is valid
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	BOOL			IsValid()	const
+								{
+									// Consistency condition for spheres: Radius >= 0.0f
+									if(mRadius < 0.0f)	return FALSE;
+									return TRUE;
+								}
+		public:
+				Point			mCenter;		//!< Sphere center
+				float			mRadius;		//!< Sphere radius
+	};
+
+#endif // __ICEBOUNDINGSPHERE_H__

src/external/open_dynamics_engine-ef/ode/IceContainer.cpp

@@ -0,0 +1,352 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a simple container class.
+ *	\file		IceContainer.cpp
+ *	\author		Pierre Terdiman
+ *	\date		February, 5, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a list of 32-bits values.
+ *	Use this class when you need to store an unknown number of values. The list is automatically
+ *	resized and can contains 32-bits entities (dwords or floats)
+ *
+ *	\class		Container
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ *	\date		08.15.98
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceCore;
+
+// Static members
+#ifdef CONTAINER_STATS
+udword Container::mNbContainers = 0;
+udword Container::mUsedRam = 0;
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor. No entries allocated there.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container::Container() : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f)
+{
+#ifdef CONTAINER_STATS
+	mNbContainers++;
+	mUsedRam+=sizeof(Container);
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor. Also allocates a given number of entries.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container::Container(udword size, float growth_factor) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(growth_factor)
+{
+#ifdef CONTAINER_STATS
+	mNbContainers++;
+	mUsedRam+=sizeof(Container);
+#endif
+	SetSize(size);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Copy constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container::Container(const Container& object) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f)
+{
+#ifdef CONTAINER_STATS
+	mNbContainers++;
+	mUsedRam+=sizeof(Container);
+#endif
+	*this = object;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.	Frees everything and leaves.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container::~Container()
+{
+	Empty();
+#ifdef CONTAINER_STATS
+	mNbContainers--;
+	mUsedRam-=GetUsedRam();
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Clears the container. All stored values are deleted, and it frees used ram.
+ *	\see		Reset()
+ *	\return		Self-Reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container& Container::Empty()
+{
+#ifdef CONTAINER_STATS
+	mUsedRam-=mMaxNbEntries*sizeof(udword);
+#endif
+	DELETEARRAY(mEntries);
+	mCurNbEntries = mMaxNbEntries = 0;
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Resizes the container.
+ *	\param		needed	[in] assume the container can be added at least "needed" values
+ *	\return		true if success.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Container::Resize(udword needed)
+{
+#ifdef CONTAINER_STATS
+	// Subtract previous amount of bytes
+	mUsedRam-=mMaxNbEntries*sizeof(udword);
+#endif
+
+	// Get more entries
+	mMaxNbEntries = mMaxNbEntries ? udword(float(mMaxNbEntries)*mGrowthFactor) : 2;	// Default nb Entries = 2
+	if(mMaxNbEntries<mCurNbEntries + needed)	mMaxNbEntries = mCurNbEntries + needed;
+
+	// Get some bytes for new entries
+	udword*	NewEntries = new udword[mMaxNbEntries];
+	CHECKALLOC(NewEntries);
+
+#ifdef CONTAINER_STATS
+	// Add current amount of bytes
+	mUsedRam+=mMaxNbEntries*sizeof(udword);
+#endif
+
+	// Copy old data if needed
+	if(mCurNbEntries)	CopyMemory(NewEntries, mEntries, mCurNbEntries*sizeof(udword));
+
+	// Delete old data
+	DELETEARRAY(mEntries);
+
+	// Assign new pointer
+	mEntries = NewEntries;
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Sets the initial size of the container. If it already contains something, it's discarded.
+ *	\param		nb		[in] Number of entries
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Container::SetSize(udword nb)
+{
+	// Make sure it's empty
+	Empty();
+
+	// Checkings
+	if(!nb)	return false;
+
+	// Initialize for nb entries
+	mMaxNbEntries = nb;
+
+	// Get some bytes for new entries
+	mEntries = new udword[mMaxNbEntries];
+	CHECKALLOC(mEntries);
+
+#ifdef CONTAINER_STATS
+	// Add current amount of bytes
+	mUsedRam+=mMaxNbEntries*sizeof(udword);
+#endif
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the container and get rid of unused bytes.
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Container::Refit()
+{
+#ifdef CONTAINER_STATS
+	// Subtract previous amount of bytes
+	mUsedRam-=mMaxNbEntries*sizeof(udword);
+#endif
+
+	// Get just enough entries
+	mMaxNbEntries = mCurNbEntries;
+	if(!mMaxNbEntries)	return false;
+
+	// Get just enough bytes
+	udword*	NewEntries = new udword[mMaxNbEntries];
+	CHECKALLOC(NewEntries);
+
+#ifdef CONTAINER_STATS
+	// Add current amount of bytes
+	mUsedRam+=mMaxNbEntries*sizeof(udword);
+#endif
+
+	// Copy old data
+	CopyMemory(NewEntries, mEntries, mCurNbEntries*sizeof(udword));
+
+	// Delete old data
+	DELETEARRAY(mEntries);
+
+	// Assign new pointer
+	mEntries = NewEntries;
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks whether the container already contains a given value.
+ *	\param		entry			[in] the value to look for in the container
+ *	\param		location		[out] a possible pointer to store the entry location
+ *	\see		Add(udword entry)
+ *	\see		Add(float entry)
+ *	\see		Empty()
+ *	\return		true if the value has been found in the container, else false.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Container::Contains(udword entry, udword* location) const
+{
+	// Look for the entry
+	for(udword i=0;i<mCurNbEntries;i++)
+	{
+		if(mEntries[i]==entry)
+		{
+			if(location)	*location = i;
+			return true;
+		}
+	}
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Deletes an entry. If the container contains such an entry, it's removed.
+ *	\param		entry		[in] the value to delete.
+ *	\return		true if the value has been found in the container, else false.
+ *	\warning	This method is arbitrary slow (O(n)) and should be used carefully. Insertion order is not preserved.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Container::Delete(udword entry)
+{
+	// Look for the entry
+	for(udword i=0;i<mCurNbEntries;i++)
+	{
+		if(mEntries[i]==entry)
+		{
+			// Entry has been found at index i. The strategy is to copy the last current entry at index i, and decrement the current number of entries.
+			DeleteIndex(i);
+			return true;
+		}
+	}
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Deletes an entry, preserving the insertion order. If the container contains such an entry, it's removed.
+ *	\param		entry		[in] the value to delete.
+ *	\return		true if the value has been found in the container, else false.
+ *	\warning	This method is arbitrary slow (O(n)) and should be used carefully.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Container::DeleteKeepingOrder(udword entry)
+{
+	// Look for the entry
+	for(udword i=0;i<mCurNbEntries;i++)
+	{
+		if(mEntries[i]==entry)
+		{
+			// Entry has been found at index i.
+			// Shift entries to preserve order. You really should use a linked list instead.
+			mCurNbEntries--;
+			for(udword j=i;j<mCurNbEntries;j++)
+			{
+				mEntries[j] = mEntries[j+1];
+			}
+			return true;
+		}
+	}
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the next entry, starting from input one.
+ *	\param		entry		[in/out] On input, the entry to look for. On output, the next entry
+ *	\param		find_mode	[in] wrap/clamp
+ *	\return		Self-Reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container& Container::FindNext(udword& entry, FindMode find_mode)
+{
+	udword Location;
+	if(Contains(entry, &Location))
+	{
+		Location++;
+		if(Location==mCurNbEntries)	Location = find_mode==FIND_WRAP ? 0 : mCurNbEntries-1;
+		entry = mEntries[Location];
+	}
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the previous entry, starting from input one.
+ *	\param		entry		[in/out] On input, the entry to look for. On output, the previous entry
+ *	\param		find_mode	[in] wrap/clamp
+ *	\return		Self-Reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Container& Container::FindPrev(udword& entry, FindMode find_mode)
+{
+	udword Location;
+	if(Contains(entry, &Location))
+	{
+		Location--;
+		if(Location==0xffffffff)	Location = find_mode==FIND_WRAP ? mCurNbEntries-1 : 0;
+		entry = mEntries[Location];
+	}
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the ram used by the container.
+ *	\return		the ram used in bytes.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword Container::GetUsedRam() const
+{
+	return sizeof(Container) + mMaxNbEntries * sizeof(udword);
+}
+
+/*void Container::operator=(const Container& object)
+{
+	SetSize(object.GetNbEntries());
+	CopyMemory(mEntries, object.GetEntries(), mMaxNbEntries*sizeof(udword));
+	mCurNbEntries = mMaxNbEntries;
+}*/

src/external/open_dynamics_engine-ef/ode/IceContainer.h

@@ -0,0 +1,212 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a simple container class.
+ *	\file		IceContainer.h
+ *	\author		Pierre Terdiman
+ *	\date		February, 5, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICECONTAINER_H__
+#define __ICECONTAINER_H__
+
+	#define CONTAINER_STATS
+
+	enum FindMode
+	{
+		FIND_CLAMP,
+		FIND_WRAP,
+
+		FIND_FORCE_DWORD = 0x7fffffff
+	};
+
+	class ICECORE_API Container
+	{
+		public:
+		// Constructor / Destructor
+								Container();
+								Container(const Container& object);
+								Container(udword size, float growth_factor);
+								~Container();
+		// Management
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	A O(1) method to add a value in the container. The container is automatically resized if needed.
+		 *	The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation
+		 *	costs a lot more than the call overhead...
+		 *
+		 *	\param		entry		[in] a udword to store in the container
+		 *	\see		Add(float entry)
+		 *	\see		Empty()
+		 *	\see		Contains(udword entry)
+		 *	\return		Self-Reference
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	Container&		Add(udword entry)
+								{
+									// Resize if needed
+									if(mCurNbEntries==mMaxNbEntries)	Resize();
+
+									// Add new entry
+									mEntries[mCurNbEntries++]	= entry;
+									return *this;
+								}
+
+		inline_	Container&		Add(const udword* entries, udword nb)
+								{
+									// Resize if needed
+									if(mCurNbEntries+nb>mMaxNbEntries)	Resize(nb);
+
+									// Add new entry
+									CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(udword));
+									mCurNbEntries+=nb;
+									return *this;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	A O(1) method to add a value in the container. The container is automatically resized if needed.
+		 *	The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation
+		 *	costs a lot more than the call overhead...
+		 *
+		 *	\param		entry		[in] a float to store in the container
+		 *	\see		Add(udword entry)
+		 *	\see		Empty()
+		 *	\see		Contains(udword entry)
+		 *	\return		Self-Reference
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	Container&		Add(float entry)
+								{
+									// Resize if needed
+									if(mCurNbEntries==mMaxNbEntries)	Resize();
+
+									// Add new entry
+									mEntries[mCurNbEntries++]	= IR(entry);
+									return *this;
+								}
+
+		inline_	Container&		Add(const float* entries, udword nb)
+								{
+									// Resize if needed
+									if(mCurNbEntries+nb>mMaxNbEntries)	Resize(nb);
+
+									// Add new entry
+									CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(float));
+									mCurNbEntries+=nb;
+									return *this;
+								}
+
+		//! Add unique [slow]
+		inline_	Container&		AddUnique(udword entry)
+								{
+									if(!Contains(entry))	Add(entry);
+									return *this;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Clears the container. All stored values are deleted, and it frees used ram.
+		 *	\see		Reset()
+		 *	\return		Self-Reference
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				Container&		Empty();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Resets the container. Stored values are discarded but the buffer is kept so that further calls don't need resizing again.
+		 *	That's a kind of temporal coherence.
+		 *	\see		Empty()
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	void			Reset()
+								{
+									// Avoid the write if possible
+									// ### CMOV
+									if(mCurNbEntries)	mCurNbEntries = 0;
+								}
+
+		// HANDLE WITH CARE
+		inline_	void			ForceSize(udword size)
+								{
+									mCurNbEntries = size;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Sets the initial size of the container. If it already contains something, it's discarded.
+		 *	\param		nb		[in] Number of entries
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				bool			SetSize(udword nb);
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Refits the container and get rid of unused bytes.
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				bool			Refit();
+
+		// Checks whether the container already contains a given value.
+				bool			Contains(udword entry, udword* location=null) const;
+		// Deletes an entry - doesn't preserve insertion order.
+				bool			Delete(udword entry);
+		// Deletes an entry - does preserve insertion order.
+				bool			DeleteKeepingOrder(udword entry);
+		//! Deletes the very last entry.
+		inline_	void			DeleteLastEntry()						{ if(mCurNbEntries)	mCurNbEntries--;			}
+		//! Deletes the entry whose index is given
+		inline_	void			DeleteIndex(udword index)				{ mEntries[index] = mEntries[--mCurNbEntries];	}
+
+		// Helpers
+				Container&		FindNext(udword& entry, FindMode find_mode=FIND_CLAMP);
+				Container&		FindPrev(udword& entry, FindMode find_mode=FIND_CLAMP);
+		// Data access.
+		inline_	udword			GetNbEntries()					const	{ return mCurNbEntries;					}	//!< Returns the current number of entries.
+		inline_	udword			GetEntry(udword i)				const	{ return mEntries[i];					}	//!< Returns ith entry
+		inline_	udword*			GetEntries()					const	{ return mEntries;						}	//!< Returns the list of entries.
+
+		inline_	udword			GetFirst()						const	{ return mEntries[0];					}
+		inline_	udword			GetLast()						const	{ return mEntries[mCurNbEntries-1];		}
+
+		// Growth control
+		inline_	float			GetGrowthFactor()				const	{ return mGrowthFactor;					}	//!< Returns the growth factor
+		inline_	void			SetGrowthFactor(float growth)			{ mGrowthFactor = growth;				}	//!< Sets the growth factor
+		inline_	bool			IsFull()						const	{ return mCurNbEntries==mMaxNbEntries;	}	//!< Checks the container is full
+		inline_	BOOL			IsNotEmpty()					const	{ return mCurNbEntries;					}	//!< Checks the container is empty
+
+		//! Read-access as an array
+		inline_	udword			operator[](udword i)			const	{ ASSERT(i>=0 && i<mCurNbEntries); return mEntries[i];	}
+		//! Write-access as an array
+		inline_	udword&			operator[](udword i)					{ ASSERT(i>=0 && i<mCurNbEntries); return mEntries[i];	}
+
+		// Stats
+				udword			GetUsedRam()					const;
+
+		//! Operator for "Container A = Container B"
+				//void			operator = (const Container& object);
+
+#ifdef CONTAINER_STATS
+		inline_	udword			GetNbContainers()				const	{ return mNbContainers;		}
+		inline_	udword			GetTotalBytes()					const	{ return mUsedRam;			}
+		private:
+
+		static	udword			mNbContainers;		//!< Number of containers around
+		static	udword			mUsedRam;			//!< Amount of bytes used by containers in the system
+#endif
+		private:
+		// Resizing
+				bool			Resize(udword needed=1);
+		// Data
+				udword			mMaxNbEntries;		//!< Maximum possible number of entries
+				udword			mCurNbEntries;		//!< Current number of entries
+				udword*			mEntries;			//!< List of entries
+				float			mGrowthFactor;		//!< Resize: new number of entries = old number * mGrowthFactor
+	};
+
+#endif // __ICECONTAINER_H__

src/external/open_dynamics_engine-ef/ode/IceFPU.h

@@ -0,0 +1,337 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains FPU related code.
+ *	\file		IceFPU.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEFPU_H__
+#define __ICEFPU_H__
+
+	#define	SIGN_BITMASK			0x80000000
+
+	//! Integer representation of a floating-point value.
+	#define IR(x)					((udword&)(x))
+
+	//! Signed integer representation of a floating-point value.
+	#define SIR(x)					((sdword&)(x))
+
+	//! Absolute integer representation of a floating-point value
+	#define AIR(x)					(IR(x)&0x7fffffff)
+
+	//! Floating-point representation of an integer value.
+	#define FR(x)					((float&)(x))
+
+	//! Integer-based comparison of a floating point value.
+	//! Don't use it blindly, it can be faster or slower than the FPU comparison, depends on the context.
+	#define IS_NEGATIVE_FLOAT(x)	(IR(x)&0x80000000)
+
+	//! Fast fabs for floating-point values. It just clears the sign bit.
+	//! Don't use it blindy, it can be faster or slower than the FPU comparison, depends on the context.
+	inline_ float FastFabs(float x)
+	{
+		udword FloatBits = IR(x)&0x7fffffff;
+		return FR(FloatBits);
+	}
+
+	//! Fast square root for floating-point values.
+	inline_ float FastSqrt(float square)
+	{
+#if defined(_MSC_VER) && 0 // ericf disabling
+			float retval;
+
+			__asm {
+					mov             eax, square
+					sub             eax, 0x3F800000
+					sar             eax, 1
+					add             eax, 0x3F800000
+					mov             [retval], eax
+			}
+			return retval;
+#else
+			return sqrtf(square);
+#endif
+	}
+
+	//! Saturates positive to zero.
+	inline_ float fsat(float f)
+	{
+		udword y = (udword&)f & ~((sdword&)f >>31);
+		return (float&)y;
+	}
+
+	//! Computes 1.0f / sqrtf(x).
+	inline_ float frsqrt(float f)
+	{
+		float x = f * 0.5f;
+		udword y = 0x5f3759df - ((udword&)f >> 1);
+		// Iteration...
+		(float&)y  = (float&)y * ( 1.5f - ( x * (float&)y * (float&)y ) );
+		// Result
+		return (float&)y;
+	}
+
+	//! Computes 1.0f / sqrtf(x). Comes from NVIDIA.
+	inline_ float InvSqrt(const float& x)
+	{
+		udword tmp = (udword(IEEE_1_0 << 1) + IEEE_1_0 - *(udword*)&x) >> 1;   
+		float y = *(float*)&tmp;                                             
+		return y * (1.47f - 0.47f * x * y * y);
+	}
+
+	//! Computes 1.0f / sqrtf(x). Comes from Quake3. Looks like the first one I had above.
+	//! See http://www.magic-software.com/3DGEDInvSqrt.html
+	inline_ float RSqrt(float number)
+	{
+		long i;
+		float x2, y;
+		const float threehalfs = 1.5f;
+
+		x2 = number * 0.5f;
+		y  = number;
+		i  = * (long *) &y;
+		i  = 0x5f3759df - (i >> 1);
+		y  = * (float *) &i;
+		y  = y * (threehalfs - (x2 * y * y));
+
+		return y;
+	}
+
+	//! TO BE DOCUMENTED
+	inline_ float fsqrt(float f)
+	{
+		udword y = ( ( (sdword&)f - 0x3f800000 ) >> 1 ) + 0x3f800000;
+		// Iteration...?
+		// (float&)y = (3.0f - ((float&)y * (float&)y) / f) * (float&)y * 0.5f;
+		// Result
+		return (float&)y;
+	}
+
+	//! Returns the float ranged espilon value.
+	inline_ float fepsilon(float f)
+	{
+		udword b = (udword&)f & 0xff800000;
+		udword a = b | 0x00000001;
+		(float&)a -= (float&)b;
+		// Result
+		return (float&)a;
+	}
+
+	//! Is the float valid ?
+	inline_ bool IsNAN(float value)				{ return (IR(value)&0x7f800000) == 0x7f800000;	}
+	inline_ bool IsIndeterminate(float value)	{ return IR(value) == 0xffc00000;				}
+	inline_ bool IsPlusInf(float value)			{ return IR(value) == 0x7f800000;				}
+	inline_ bool IsMinusInf(float value)		{ return IR(value) == 0xff800000;				}
+
+	inline_	bool IsValidFloat(float value)
+	{
+		if(IsNAN(value))			return false;
+		if(IsIndeterminate(value))	return false;
+		if(IsPlusInf(value))		return false;
+		if(IsMinusInf(value))		return false;
+		return true;
+	}
+
+	#define CHECK_VALID_FLOAT(x)	ASSERT(IsValidFloat(x));
+
+/*
+	//! FPU precision setting function.
+	inline_ void SetFPU()
+	{
+		// This function evaluates whether the floating-point
+		// control word is set to single precision/round to nearest/
+		// exceptions disabled. If these conditions don't hold, the
+		// function changes the control word to set them and returns
+		// TRUE, putting the old control word value in the passback
+		// location pointed to by pwOldCW.
+		{
+			uword wTemp, wSave;
+ 
+			__asm fstcw wSave
+			if (wSave & 0x300 ||            // Not single mode
+				0x3f != (wSave & 0x3f) ||   // Exceptions enabled
+				wSave & 0xC00)              // Not round to nearest mode
+			{
+				__asm
+				{
+					mov ax, wSave
+					and ax, not 300h    ;; single mode
+					or  ax, 3fh         ;; disable all exceptions
+					and ax, not 0xC00   ;; round to nearest mode
+					mov wTemp, ax
+					fldcw   wTemp
+				}
+			}
+		}
+	}
+*/
+	//! This function computes the slowest possible floating-point value (you can also directly use FLT_EPSILON)
+	inline_ float ComputeFloatEpsilon()
+	{
+		float f = 1.0f;
+		((udword&)f)^=1;
+		return f - 1.0f;	// You can check it's the same as FLT_EPSILON
+	}
+
+	inline_ bool IsFloatZero(float x, float epsilon=1e-6f)
+	{
+		return x*x < epsilon;
+	}
+
+	#define FCOMI_ST0	_asm	_emit	0xdb	_asm	_emit	0xf0
+	#define FCOMIP_ST0	_asm	_emit	0xdf	_asm	_emit	0xf0
+	#define FCMOVB_ST0	_asm	_emit	0xda	_asm	_emit	0xc0
+	#define FCMOVNB_ST0	_asm	_emit	0xdb	_asm	_emit	0xc0
+
+	#define FCOMI_ST1	_asm	_emit	0xdb	_asm	_emit	0xf1
+	#define FCOMIP_ST1	_asm	_emit	0xdf	_asm	_emit	0xf1
+	#define FCMOVB_ST1	_asm	_emit	0xda	_asm	_emit	0xc1
+	#define FCMOVNB_ST1	_asm	_emit	0xdb	_asm	_emit	0xc1
+
+	#define FCOMI_ST2	_asm	_emit	0xdb	_asm	_emit	0xf2
+	#define FCOMIP_ST2	_asm	_emit	0xdf	_asm	_emit	0xf2
+	#define FCMOVB_ST2	_asm	_emit	0xda	_asm	_emit	0xc2
+	#define FCMOVNB_ST2	_asm	_emit	0xdb	_asm	_emit	0xc2
+
+	#define FCOMI_ST3	_asm	_emit	0xdb	_asm	_emit	0xf3
+	#define FCOMIP_ST3	_asm	_emit	0xdf	_asm	_emit	0xf3
+	#define FCMOVB_ST3	_asm	_emit	0xda	_asm	_emit	0xc3
+	#define FCMOVNB_ST3	_asm	_emit	0xdb	_asm	_emit	0xc3
+
+	#define FCOMI_ST4	_asm	_emit	0xdb	_asm	_emit	0xf4
+	#define FCOMIP_ST4	_asm	_emit	0xdf	_asm	_emit	0xf4
+	#define FCMOVB_ST4	_asm	_emit	0xda	_asm	_emit	0xc4
+	#define FCMOVNB_ST4	_asm	_emit	0xdb	_asm	_emit	0xc4
+
+	#define FCOMI_ST5	_asm	_emit	0xdb	_asm	_emit	0xf5
+	#define FCOMIP_ST5	_asm	_emit	0xdf	_asm	_emit	0xf5
+	#define FCMOVB_ST5	_asm	_emit	0xda	_asm	_emit	0xc5
+	#define FCMOVNB_ST5	_asm	_emit	0xdb	_asm	_emit	0xc5
+
+	#define FCOMI_ST6	_asm	_emit	0xdb	_asm	_emit	0xf6
+	#define FCOMIP_ST6	_asm	_emit	0xdf	_asm	_emit	0xf6
+	#define FCMOVB_ST6	_asm	_emit	0xda	_asm	_emit	0xc6
+	#define FCMOVNB_ST6	_asm	_emit	0xdb	_asm	_emit	0xc6
+
+	#define FCOMI_ST7	_asm	_emit	0xdb	_asm	_emit	0xf7
+	#define FCOMIP_ST7	_asm	_emit	0xdf	_asm	_emit	0xf7
+	#define FCMOVB_ST7	_asm	_emit	0xda	_asm	_emit	0xc7
+	#define FCMOVNB_ST7	_asm	_emit	0xdb	_asm	_emit	0xc7
+
+	//! A global function to find MAX(a,b) using FCOMI/FCMOV
+	inline_ float FCMax2(float a, float b)
+	{
+#if defined(_MSC_VER) && 0  // ericf disabling
+		float Res;
+		_asm	fld		[a]
+		_asm	fld		[b]
+		FCOMI_ST1
+		FCMOVB_ST1
+		_asm	fstp	[Res]
+		_asm	fcomp
+		return Res;
+#else
+		return (a > b) ? a : b;
+#endif
+	}
+
+	//! A global function to find MIN(a,b) using FCOMI/FCMOV
+	inline_ float FCMin2(float a, float b)
+	{
+#if defined(_MSC_VER) && 0  // ericf disabling
+		float Res;
+		_asm	fld		[a]
+		_asm	fld		[b]
+		FCOMI_ST1
+		FCMOVNB_ST1
+		_asm	fstp	[Res]
+		_asm	fcomp
+		return Res;
+#else
+		return (a < b) ? a : b;
+#endif
+	}
+
+	//! A global function to find MAX(a,b,c) using FCOMI/FCMOV
+	inline_ float FCMax3(float a, float b, float c)
+	{
+#if defined(_MSC_VER) && 0  // ericf disabling
+		float Res;
+		_asm	fld		[a]
+		_asm	fld		[b]
+		_asm	fld		[c]
+		FCOMI_ST1
+		FCMOVB_ST1
+		FCOMI_ST2
+		FCMOVB_ST2
+		_asm	fstp	[Res]
+		_asm	fcompp
+		return Res;
+#else
+		return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c);
+#endif
+	}
+
+	//! A global function to find MIN(a,b,c) using FCOMI/FCMOV
+	inline_ float FCMin3(float a, float b, float c)
+	{
+#if defined(_MSC_VER) && 0  // ericf disabling
+		float Res;
+		_asm	fld		[a]
+		_asm	fld		[b]
+		_asm	fld		[c]
+		FCOMI_ST1
+		FCMOVNB_ST1
+		FCOMI_ST2
+		FCMOVNB_ST2
+		_asm	fstp	[Res]
+		_asm	fcompp
+		return Res;
+#else
+		return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c);
+#endif
+	}
+
+	inline_ int ConvertToSortable(float f)
+	{
+		int& Fi = (int&)f;
+		int Fmask = (Fi>>31);
+		Fi ^= Fmask;
+		Fmask &= ~(1<<31);
+		Fi -= Fmask;
+		return Fi;
+	}
+
+	enum FPUMode
+	{
+		FPU_FLOOR		= 0,
+		FPU_CEIL		= 1,
+		FPU_BEST		= 2,
+
+		FPU_FORCE_DWORD	= 0x7fffffff
+	};
+
+	FUNCTION ICECORE_API FPUMode	GetFPUMode();
+	FUNCTION ICECORE_API void		SaveFPU();
+	FUNCTION ICECORE_API void		RestoreFPU();
+	FUNCTION ICECORE_API void		SetFPUFloorMode();
+	FUNCTION ICECORE_API void		SetFPUCeilMode();
+	FUNCTION ICECORE_API void		SetFPUBestMode();
+
+	FUNCTION ICECORE_API void		SetFPUPrecision24();
+	FUNCTION ICECORE_API void		SetFPUPrecision53();
+	FUNCTION ICECORE_API void		SetFPUPrecision64();
+	FUNCTION ICECORE_API void		SetFPURoundingChop();
+	FUNCTION ICECORE_API void		SetFPURoundingUp();
+	FUNCTION ICECORE_API void		SetFPURoundingDown();
+	FUNCTION ICECORE_API void		SetFPURoundingNear();
+
+	FUNCTION ICECORE_API int		intChop(const float& f);
+	FUNCTION ICECORE_API int		intFloor(const float& f);
+	FUNCTION ICECORE_API int		intCeil(const float& f);
+
+#endif // __ICEFPU_H__

src/external/open_dynamics_engine-ef/ode/IceHPoint.cpp

@@ -0,0 +1,77 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for homogeneous points.
+ *	\file		IceHPoint.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Homogeneous point.
+ *
+ *	Use it:
+ *	- for clipping in homogeneous space (standard way)
+ *	- to differentiate between points (w=1) and vectors (w=0).
+ *	- in some cases you can also use it instead of Point for padding reasons.
+ *
+ *	\class		HPoint
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ *	\warning	No cross-product in 4D.
+ *	\warning	HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Point Mul = HPoint * Matrix3x3;
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Point HPoint::operator*(const Matrix3x3& mat) const
+{
+	return Point(
+	x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0],
+	x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1],
+	x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// HPoint Mul = HPoint * Matrix4x4;
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HPoint HPoint::operator*(const Matrix4x4& mat) const
+{
+	return HPoint(
+	x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0],
+	x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1],
+	x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2],
+	x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3]);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// HPoint *= Matrix4x4
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HPoint& HPoint::operator*=(const Matrix4x4& mat)
+{
+	float xp = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0];
+	float yp = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1];
+	float zp = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2];
+	float wp = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3];
+
+	x = xp; y = yp; z = zp; w = wp;
+
+	return	*this;
+}

src/external/open_dynamics_engine-ef/ode/IceHPoint.h

@@ -0,0 +1,161 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for homogeneous points.
+ *	\file		IceHPoint.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEHPOINT_H__
+#define __ICEHPOINT_H__
+
+	class ICEMATHS_API HPoint : public Point
+	{
+		public:
+
+		//! Empty constructor
+		inline_				HPoint()																		{}
+		//! Constructor from floats
+		inline_				HPoint(float _x, float _y, float _z, float _w=0.0f) : Point(_x, _y, _z), w(_w)	{}
+		//! Constructor from array
+		inline_				HPoint(const float f[4]) : Point(f), w(f[3])									{}
+		//! Constructor from a Point
+		inline_				HPoint(const Point& p, float _w=0.0f) : Point(p), w(_w)							{}
+		//! Destructor
+		inline_				~HPoint()																		{}
+
+		//! Clear the point
+		inline_	HPoint&		Zero()											{ x =			y =			z =			w = 0.0f;		return *this;	}
+
+		//! Assignment from values
+		inline_	HPoint&		Set(float _x, float _y, float _z, float _w )	{ x  = _x;		y  = _y;	z  = _z;	w  = _w;		return *this;	}
+		//! Assignment from array
+		inline_	HPoint&		Set(const float f[4])							{ x  = f[_X];	y  = f[_Y];	z  = f[_Z];	w  = f[_W];		return *this;	}
+		//! Assignment from another h-point
+		inline_	HPoint&		Set(const HPoint& src)							{ x  = src.x;	y  = src.y;	z  = src.z;	w = src.w;		return *this;	}
+
+		//! Add a vector
+		inline_	HPoint&		Add(float _x, float _y, float _z, float _w )	{ x += _x;		y += _y;	z += _z;	w += _w;		return *this;	}
+		//! Add a vector
+		inline_	HPoint&		Add(const float f[4])							{ x += f[_X];	y += f[_Y];	z += f[_Z];	w += f[_W];		return *this;	}
+
+		//! Subtract a vector
+		inline_	HPoint&		Sub(float _x, float _y, float _z, float _w )	{ x -= _x;		y -= _y;	z -= _z;	w -= _w;		return *this;	}
+		//! Subtract a vector
+		inline_	HPoint&		Sub(const float f[4])							{ x -= f[_X];	y -= f[_Y];	z -= f[_Z];	w -= f[_W];		return *this;	}
+		
+		//! Multiplies by a scalar
+		inline_	HPoint&		Mul(float s)									{ x *= s;		y *= s;		z *= s;		w *= s;			return *this;	}
+
+		//! Returns MIN(x, y, z, w);
+				float		Min()								const		{ return MIN(x, MIN(y, MIN(z, w)));										}
+		//! Returns MAX(x, y, z, w);
+				float		Max()								const		{ return MAX(x, MAX(y, MAX(z, w)));										}
+		//! Sets each element to be componentwise minimum
+				HPoint&		Min(const HPoint& p)							{ x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); w = MIN(w, p.w);	return *this;	}
+		//! Sets each element to be componentwise maximum
+				HPoint&		Max(const HPoint& p)							{ x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); w = MAX(w, p.w);	return *this;	}
+
+		//! Computes square magnitude
+		inline_	float		SquareMagnitude()					const		{ return x*x + y*y + z*z + w*w;											}
+		//! Computes magnitude
+		inline_	float		Magnitude()							const		{ return sqrtf(x*x + y*y + z*z + w*w);									}
+
+		//! Normalize the vector
+		inline_	HPoint&		Normalize()
+							{
+								float M = Magnitude();
+								if(M)
+								{
+									M = 1.0f / M;
+									x *= M;
+									y *= M;
+									z *= M;
+									w *= M;
+								}
+								return *this;
+							}
+
+		// Arithmetic operators
+		//! Operator for HPoint Negate = - HPoint;
+		inline_	HPoint		operator-()							const		{ return HPoint(-x, -y, -z, -w);							}
+
+		//! Operator for HPoint Plus  = HPoint + HPoint;
+		inline_	HPoint		operator+(const HPoint& p)			const		{ return HPoint(x + p.x, y + p.y, z + p.z, w + p.w);		}
+		//! Operator for HPoint Minus = HPoint - HPoint;
+		inline_	HPoint		operator-(const HPoint& p)			const		{ return HPoint(x - p.x, y - p.y, z - p.z, w - p.w);		}
+
+		//! Operator for HPoint Mul   = HPoint * HPoint;
+		inline_	HPoint		operator*(const HPoint& p)			const		{ return HPoint(x * p.x, y * p.y, z * p.z, w * p.w);		}
+		//! Operator for HPoint Scale = HPoint * float;
+		inline_	HPoint		operator*(float s)					const		{ return HPoint(x * s, y * s, z * s, w * s);				}
+		//! Operator for HPoint Scale = float * HPoint;
+		inline_	friend	HPoint	operator*(float s, const HPoint& p)			{ return HPoint(s * p.x, s * p.y, s * p.z, s * p.w);		}
+
+		//! Operator for HPoint Div   = HPoint / HPoint;
+		inline_	HPoint		operator/(const HPoint& p)			const		{ return HPoint(x / p.x, y / p.y, z / p.z, w / p.w);		}
+		//! Operator for HPoint Scale = HPoint / float;
+		inline_	HPoint		operator/(float s)					const		{ s = 1.0f / s; return HPoint(x * s, y * s, z * s, w * s);	}
+		//! Operator for HPoint Scale = float / HPoint;
+		inline_	friend	HPoint	operator/(float s, const HPoint& p)			{ return HPoint(s / p.x, s / p.y, s / p.z, s / p.w);		}
+
+		//! Operator for float DotProd = HPoint | HPoint;
+		inline_	float		operator|(const HPoint& p)			const		{ return x*p.x + y*p.y + z*p.z + w*p.w;						}
+		// No cross-product in 4D
+
+		//! Operator for HPoint += HPoint;
+		inline_	HPoint&		operator+=(const HPoint& p)						{ x += p.x; y += p.y; z += p.z; w += p.w;	return *this;	}
+		//! Operator for HPoint += float;
+		inline_	HPoint&		operator+=(float s)								{ x += s;   y += s;   z += s;   w += s;		return *this;	}
+
+		//! Operator for HPoint -= HPoint;
+		inline_	HPoint&		operator-=(const HPoint& p)						{ x -= p.x; y -= p.y; z -= p.z; w -= p.w;	return *this;	}
+		//! Operator for HPoint -= float;
+		inline_	HPoint&		operator-=(float s)								{ x -= s;   y -= s;   z -= s;   w -= s;		return *this;	}
+
+		//! Operator for HPoint *= HPoint;
+		inline_	HPoint&		operator*=(const HPoint& p)						{ x *= p.x; y *= p.y; z *= p.z; w *= p.w;	return *this;	}
+		//! Operator for HPoint *= float;
+		inline_	HPoint&		operator*=(float s)								{ x*=s; y*=s; z*=s; w*=s;					return *this;	}
+
+		//! Operator for HPoint /= HPoint;
+		inline_	HPoint&		operator/=(const HPoint& p)						{ x /= p.x; y /= p.y; z /= p.z; w /= p.w;	return *this;	}
+		//! Operator for HPoint /= float;
+		inline_	HPoint&		operator/=(float s)								{ s = 1.0f / s; x*=s; y*=s; z*=s; w*=s;		return *this;	}
+
+		// Arithmetic operators
+
+		//! Operator for Point Mul = HPoint * Matrix3x3;
+				Point		operator*(const Matrix3x3& mat)		const;
+		//! Operator for HPoint Mul = HPoint * Matrix4x4;
+				HPoint		operator*(const Matrix4x4& mat)		const;
+
+		// HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4
+		//! Operator for HPoint *= Matrix4x4
+				HPoint&		operator*=(const Matrix4x4& mat);
+
+		// Logical operators
+
+		//! Operator for "if(HPoint==HPoint)"
+		inline_	bool		operator==(const HPoint& p)			const		{ return ( (x==p.x)&&(y==p.y)&&(z==p.z)&&(w==p.w));			}
+		//! Operator for "if(HPoint!=HPoint)"
+		inline_	bool		operator!=(const HPoint& p)			const		{ return ( (x!=p.x)||(y!=p.y)||(z!=p.z)||(w!=p.w));			}
+
+		// Cast operators
+
+		//! Cast a HPoint to a Point. w is discarded.
+//#ifdef _MSC_VER
+        // ericf disabled
+		//inline_				operator	Point()					const		{ return Point(x, y, z);									}
+		// gcc complains that conversion to a base class will never use a type conversion operator
+//#endif
+
+		public:
+				float		w;
+	};
+
+#endif // __ICEHPOINT_H__
+

src/external/open_dynamics_engine-ef/ode/IceIndexedTriangle.cpp

@@ -0,0 +1,556 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a handy indexed triangle class.
+ *	\file		IceIndexedTriangle.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 17, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains an indexed triangle class.
+ *
+ *	\class		Triangle
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ *	\date		08.15.98
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Flips the winding order.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::Flip()
+{
+	Swap(mVRef[1], mVRef[2]);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle area.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\return		the area
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::Area(const Point* verts)	const
+{
+	if(!verts)	return 0.0f;
+	const Point& p0 = verts[0];
+	const Point& p1 = verts[1];
+	const Point& p2 = verts[2];
+	return ((p0-p1)^(p0-p2)).Magnitude() * 0.5f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle perimeter.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\return		the perimeter
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::Perimeter(const Point* verts)	const
+{
+	if(!verts)	return 0.0f;
+	const Point& p0 = verts[0];
+	const Point& p1 = verts[1];
+	const Point& p2 = verts[2];
+	return		p0.Distance(p1)
+			+	p0.Distance(p2)
+			+	p1.Distance(p2);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle compacity.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\return		the compacity
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::Compacity(const Point* verts) const
+{
+	if(!verts)	return 0.0f;
+	float P = Perimeter(verts);
+	if(P==0.0f)	return 0.0f;
+	return (4.0f*PI*Area(verts)/(P*P));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle normal.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\param		normal	[out] the computed normal
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::Normal(const Point* verts, Point& normal)	const
+{
+	if(!verts)	return;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+	normal = ((p2-p1)^(p0-p1)).Normalize();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle denormalized normal.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\param		normal	[out] the computed normal
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::DenormalizedNormal(const Point* verts, Point& normal)	const
+{
+	if(!verts)	return;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+	normal = ((p2-p1)^(p0-p1));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle center.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\param		center	[out] the computed center
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::Center(const Point* verts, Point& center)	const
+{
+	if(!verts)	return;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+	center = (p0+p1+p2)*INV3;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the centered normal
+ *	\param		verts	[in] the list of indexed vertices
+ *	\param		normal	[out] the computed centered normal
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::CenteredNormal(const Point* verts, Point& normal)	const
+{
+	if(!verts)	return;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+	Point Center = (p0+p1+p2)*INV3;
+	normal = Center + ((p2-p1)^(p0-p1)).Normalize();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes a random point within the triangle.
+ *	\param		verts	[in] the list of indexed vertices
+ *	param		normal	[out] the computed centered normal
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::RandomPoint(const Point* verts, Point& random)	const
+{
+	if(!verts)	return;
+
+	// Random barycentric coords
+	float Alpha	= UnitRandomFloat();
+	float Beta	= UnitRandomFloat();
+	float Gamma	= UnitRandomFloat();
+	float OneOverTotal = 1.0f / (Alpha + Beta + Gamma);
+	Alpha	*= OneOverTotal;
+	Beta	*= OneOverTotal;
+	Gamma	*= OneOverTotal;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+	random = Alpha*p0 + Beta*p1 + Gamma*p2;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes backface culling.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\param		source	[in] source point (in local space) from which culling must be computed
+ *	\return		true if the triangle is visible from the source point
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::IsVisible(const Point* verts, const Point& source)	const
+{
+	// Checkings
+	if(!verts)	return false;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+
+	// Compute denormalized normal
+	Point Normal = (p2 - p1)^(p0 - p1);
+
+	// Backface culling
+	return (Normal | source) >= 0.0f;
+
+// Same as:
+//	Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]);
+//	return PL.Distance(source) > PL.d;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes backface culling.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\param		source	[in] source point (in local space) from which culling must be computed
+ *	\return		true if the triangle is visible from the source point
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::BackfaceCulling(const Point* verts, const Point& source)	const
+{
+	// Checkings
+	if(!verts)	return false;
+
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+
+	// Compute base
+//	Point Base = (p0 + p1 + p2)*INV3;
+
+	// Compute denormalized normal
+	Point Normal = (p2 - p1)^(p0 - p1);
+
+	// Backface culling
+//	return (Normal | (source - Base)) >= 0.0f;
+	return (Normal | (source - p0)) >= 0.0f;
+
+// Same as: (but a bit faster)
+//	Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]);
+//	return PL.Distance(source)>0.0f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the occlusion potential of the triangle.
+ *	\param		verts	[in] the list of indexed vertices
+ *	param		source	[in] source point (in local space) from which occlusion potential must be computed
+ *	\return		the occlusion potential
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::ComputeOcclusionPotential(const Point* verts, const Point& view)	const
+{
+	if(!verts)	return 0.0f;
+	// Occlusion potential: -(A * (N|V) / d^2)
+	// A = polygon area
+	// N = polygon normal
+	// V = view vector
+	// d = distance viewpoint-center of polygon
+
+	float A = Area(verts);
+	Point N;	Normal(verts, N);
+	Point C;	Center(verts, C);
+	float d = view.Distance(C);
+	return -(A*(N|view))/(d*d);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Replaces a vertex reference with another one.
+ *	\param		oldref	[in] the vertex reference to replace
+ *	\param		newref	[in] the new vertex reference
+ *	\return		true if success, else false if the input vertex reference doesn't belong to the triangle
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::ReplaceVertex(udword oldref, udword newref)
+{
+			if(mVRef[0]==oldref)	{ mVRef[0] = newref; return true; }
+	else	if(mVRef[1]==oldref)	{ mVRef[1] = newref; return true; }
+	else	if(mVRef[2]==oldref)	{ mVRef[2] = newref; return true; }
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks whether the triangle is degenerate or not. A degenerate triangle has two common vertex references. This is a zero-area triangle.
+ *	\return		true if the triangle is degenerate
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::IsDegenerate()	const
+{
+	if(mVRef[0]==mVRef[1])	return true;
+	if(mVRef[1]==mVRef[2])	return true;
+	if(mVRef[2]==mVRef[0])	return true;
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks whether the input vertex reference belongs to the triangle or not.
+ *	\param		ref		[in] the vertex reference to look for
+ *	\return		true if the triangle contains the vertex reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::HasVertex(udword ref)	const
+{
+	if(mVRef[0]==ref)	return true;
+	if(mVRef[1]==ref)	return true;
+	if(mVRef[2]==ref)	return true;
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks whether the input vertex reference belongs to the triangle or not.
+ *	\param		ref		[in] the vertex reference to look for
+ *	\param		index	[out] the corresponding index in the triangle
+ *	\return		true if the triangle contains the vertex reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::HasVertex(udword ref, udword* index)	const
+{
+	if(mVRef[0]==ref)	{ *index = 0;	return true; }
+	if(mVRef[1]==ref)	{ *index = 1;	return true; }
+	if(mVRef[2]==ref)	{ *index = 2;	return true; }
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Finds an edge in a tri, given two vertex references.
+ *	\param		vref0	[in] the edge's first vertex reference
+ *	\param		vref1	[in] the edge's second vertex reference
+ *	\return		the edge number between 0 and 2, or 0xff if input refs are wrong.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ubyte IndexedTriangle::FindEdge(udword vref0, udword vref1)	const
+{
+			if(mVRef[0]==vref0 && mVRef[1]==vref1)	return 0;
+	else	if(mVRef[0]==vref1 && mVRef[1]==vref0)	return 0;
+	else	if(mVRef[0]==vref0 && mVRef[2]==vref1)	return 1;
+	else	if(mVRef[0]==vref1 && mVRef[2]==vref0)	return 1;
+	else	if(mVRef[1]==vref0 && mVRef[2]==vref1)	return 2;
+	else	if(mVRef[1]==vref1 && mVRef[2]==vref0)	return 2;
+	return 0xff;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the last reference given the first two.
+ *	\param		vref0	[in] the first vertex reference
+ *	\param		vref1	[in] the second vertex reference
+ *	\return		the last reference, or INVALID_ID if input refs are wrong.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword IndexedTriangle::OppositeVertex(udword vref0, udword vref1)	const
+{
+			if(mVRef[0]==vref0 && mVRef[1]==vref1)	return mVRef[2];
+	else	if(mVRef[0]==vref1 && mVRef[1]==vref0)	return mVRef[2];
+	else	if(mVRef[0]==vref0 && mVRef[2]==vref1)	return mVRef[1];
+	else	if(mVRef[0]==vref1 && mVRef[2]==vref0)	return mVRef[1];
+	else	if(mVRef[1]==vref0 && mVRef[2]==vref1)	return mVRef[0];
+	else	if(mVRef[1]==vref1 && mVRef[2]==vref0)	return mVRef[0];
+	return INVALID_ID;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the three sorted vertex references according to an edge number.
+ *	edgenb = 0	=> edge 0-1, returns references 0, 1, 2
+ *	edgenb = 1	=> edge 0-2, returns references 0, 2, 1
+ *	edgenb = 2	=> edge 1-2, returns references 1, 2, 0
+ *
+ *	\param		edgenb	[in] the edge number, 0, 1 or 2
+ *	\param		vref0	[out] the returned first vertex reference
+ *	\param		vref1	[out] the returned second vertex reference
+ *	\param		vref2	[out] the returned third vertex reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const
+{
+	if(edgenb==0)
+	{
+		vref0 = mVRef[0];
+		vref1 = mVRef[1];
+		vref2 = mVRef[2];
+	}
+	else if(edgenb==1)
+	{
+		vref0 = mVRef[0];
+		vref1 = mVRef[2];
+		vref2 = mVRef[1];
+	}
+	else if(edgenb==2)
+	{
+		vref0 = mVRef[1];
+		vref1 = mVRef[2];
+		vref2 = mVRef[0];
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle's smallest edge length.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\return		the smallest edge length
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::MinEdgeLength(const Point* verts)	const
+{
+	if(!verts)	return 0.0f;
+
+	float Min = MAX_FLOAT;
+	float Length01 = verts[0].Distance(verts[1]);
+	float Length02 = verts[0].Distance(verts[2]);
+	float Length12 = verts[1].Distance(verts[2]);
+	if(Length01 < Min)	Min = Length01;
+	if(Length02 < Min)	Min = Length02;
+	if(Length12 < Min)	Min = Length12;
+	return Min;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle's largest edge length.
+ *	\param		verts	[in] the list of indexed vertices
+ *	\return		the largest edge length
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::MaxEdgeLength(const Point* verts)	const
+{
+	if(!verts)	return 0.0f;
+
+	float Max = MIN_FLOAT;
+	float Length01 = verts[0].Distance(verts[1]);
+	float Length02 = verts[0].Distance(verts[2]);
+	float Length12 = verts[1].Distance(verts[2]);
+	if(Length01 > Max)	Max = Length01;
+	if(Length02 > Max)	Max = Length02;
+	if(Length12 > Max)	Max = Length12;
+	return Max;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes a point on the triangle according to the stabbing information.
+ *	\param		verts		[in] the list of indexed vertices
+ *	param		u,v			[in] point's barycentric coordinates
+ *	\param		pt			[out] point on triangle
+ *	\param		nearvtx		[out] index of nearest vertex
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void IndexedTriangle::ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx)	const
+{
+	// Checkings
+	if(!verts)	return;
+
+	// Get face in local or global space
+	const Point& p0 = verts[mVRef[0]];
+	const Point& p1 = verts[mVRef[1]];
+	const Point& p2 = verts[mVRef[2]];
+
+	// Compute point coordinates
+	pt = (1.0f - u - v)*p0 + u*p1 + v*p2;
+
+	// Compute nearest vertex if needed
+	if(nearvtx)
+	{
+		// Compute distance vector
+		Point d(p0.SquareDistance(pt),	// Distance^2 from vertex 0 to point on the face
+				p1.SquareDistance(pt),	// Distance^2 from vertex 1 to point on the face
+				p2.SquareDistance(pt));	// Distance^2 from vertex 2 to point on the face
+
+		// Get smallest distance
+		*nearvtx = mVRef[d.SmallestAxis()];
+	}
+}
+
+	//**************************************
+	// Angle between two vectors (in radians)
+	// we use this formula
+	// uv = |u||v| cos(u,v)
+	// u  ^ v  = w
+	// |w| = |u||v| |sin(u,v)|
+	//**************************************
+	float Angle(const Point& u, const Point& v)
+	{
+		float NormU = u.Magnitude();	// |u|
+		float NormV = v.Magnitude();	// |v|
+		float Product = NormU*NormV;	// |u||v|
+		if(Product==0.0f)	return 0.0f;
+		float OneOverProduct = 1.0f / Product;
+
+		// Cosinus
+		float Cosinus = (u|v) * OneOverProduct;
+
+		// Sinus
+		Point w = u^v;
+		float NormW = w.Magnitude();
+
+		float AbsSinus = NormW * OneOverProduct;
+
+		// Remove degeneracy
+		if(AbsSinus > 1.0f) AbsSinus = 1.0f;
+		if(AbsSinus < -1.0f) AbsSinus = -1.0f;
+
+		if(Cosinus>=0.0f)	return asinf(AbsSinus);
+		else				return (PI-asinf(AbsSinus));
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the angle between two triangles.
+ *	\param		tri		[in] the other triangle
+ *	\param		verts	[in] the list of indexed vertices
+ *	\return		the angle in radians
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float IndexedTriangle::Angle(const IndexedTriangle& tri, const Point* verts)	const
+{
+	// Checkings
+	if(!verts)	return 0.0f;
+
+	// Compute face normals
+	Point n0, n1;
+	Normal(verts, n0);
+	tri.Normal(verts, n1);
+
+	// Compute angle
+	float dp = n0|n1;
+	if(dp>1.0f)		return 0.0f;
+	if(dp<-1.0f)	return PI;
+	return acosf(dp);
+
+//	return ::Angle(n0,n1);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks a triangle is the same as another one.
+ *	\param		tri		[in] the other triangle
+ *	\return		true if same triangle
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool IndexedTriangle::Equal(const IndexedTriangle& tri) const
+{
+	// Test all vertex references
+	return (HasVertex(tri.mVRef[0]) &&
+			HasVertex(tri.mVRef[1]) &&
+			HasVertex(tri.mVRef[2]));
+}

src/external/open_dynamics_engine-ef/ode/IceIndexedTriangle.h

@@ -0,0 +1,72 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a handy indexed triangle class.
+ *	\file		IceIndexedTriangle.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 17, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEINDEXEDTRIANGLE_H__
+#define __ICEINDEXEDTRIANGLE_H__
+
+	// Forward declarations
+#ifdef _MSC_VER
+	enum CubeIndex;
+#else
+	typedef int CubeIndex;
+#endif
+
+	// An indexed triangle class.
+	class ICEMATHS_API IndexedTriangle
+	{
+		public:
+		//! Constructor
+		inline_					IndexedTriangle()									{}
+		//! Constructor
+		inline_					IndexedTriangle(udword r0, udword r1, udword r2)	{ mVRef[0]=r0; mVRef[1]=r1; mVRef[2]=r2; }
+		//! Copy constructor
+		inline_					IndexedTriangle(const IndexedTriangle& triangle)
+								{
+									mVRef[0] = triangle.mVRef[0];
+									mVRef[1] = triangle.mVRef[1];
+									mVRef[2] = triangle.mVRef[2];
+								}
+		//! Destructor
+		inline_					~IndexedTriangle()									{}
+		//! Vertex-references
+				udword			mVRef[3];
+
+		// Methods
+				void			Flip();
+				float			Area(const Point* verts)											const;
+				float			Perimeter(const Point* verts)										const;
+				float			Compacity(const Point* verts)										const;
+				void			Normal(const Point* verts, Point& normal)							const;
+				void			DenormalizedNormal(const Point* verts, Point& normal)				const;
+				void			Center(const Point* verts, Point& center)							const;
+				void			CenteredNormal(const Point* verts, Point& normal)					const;
+				void			RandomPoint(const Point* verts, Point& random)						const;
+				bool			IsVisible(const Point* verts, const Point& source)					const;
+				bool			BackfaceCulling(const Point* verts, const Point& source)			const;
+				float			ComputeOcclusionPotential(const Point* verts, const Point& view)	const;
+				bool			ReplaceVertex(udword oldref, udword newref);
+				bool			IsDegenerate()														const;
+				bool			HasVertex(udword ref)												const;
+				bool			HasVertex(udword ref, udword* index)								const;
+				ubyte			FindEdge(udword vref0, udword vref1)								const;
+				udword			OppositeVertex(udword vref0, udword vref1)							const;
+		inline_	udword			OppositeVertex(ubyte edgenb)										const	{ return mVRef[2-edgenb];	}
+				void			GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2)	const;
+				float			MinEdgeLength(const Point* verts)									const;
+				float			MaxEdgeLength(const Point* verts)									const;
+				void			ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx=null)	const;
+				float			Angle(const IndexedTriangle& tri, const Point* verts)				const;
+		inline_	Plane			PlaneEquation(const Point* verts)									const	{ return Plane(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]);	}
+				bool			Equal(const IndexedTriangle& tri)									const;
+				CubeIndex		ComputeCubeIndex(const Point* verts)								const;
+	};
+
+#endif // __ICEINDEXEDTRIANGLE_H__

src/external/open_dynamics_engine-ef/ode/IceLSS.h

@@ -0,0 +1,75 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for line-swept spheres.
+ *	\file		IceLSS.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICELSS_H__
+#define __ICELSS_H__
+
+	class ICEMATHS_API LSS : public Segment
+	{
+		public:
+		//! Constructor
+		inline_			LSS()																	{}
+		//! Constructor
+		inline_			LSS(const Segment& seg, float radius) : Segment(seg), mRadius(radius)	{}
+		//! Destructor
+		inline_			~LSS()																	{}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes an OBB surrounding the LSS.
+		 *	\param		box		[out] the OBB
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				void	ComputeOBB(OBB& box);
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if a point is contained within the LSS.
+		 *	\param		pt	[in] the point to test
+		 *	\return		true if inside the LSS
+		 *	\warning	point and LSS must be in same space
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	bool	Contains(const Point& pt)	const	{ return SquareDistance(pt) <= mRadius*mRadius;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if a sphere is contained within the LSS.
+		 *	\param		sphere	[in] the sphere to test
+		 *	\return		true if inside the LSS
+		 *	\warning	sphere and LSS must be in same space
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	bool	Contains(const Sphere& sphere)
+						{
+							float d = mRadius - sphere.mRadius;
+							if(d>=0.0f)	return SquareDistance(sphere.mCenter) <= d*d;
+							else		return false;
+						}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if an LSS is contained within the LSS.
+		 *	\param		lss		[in] the LSS to test
+		 *	\return		true if inside the LSS
+		 *	\warning	both LSS must be in same space
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	bool	Contains(const LSS& lss)
+						{
+							// We check the LSS contains the two spheres at the start and end of the sweep
+							return Contains(Sphere(lss.mP0, lss.mRadius)) && Contains(Sphere(lss.mP0, lss.mRadius));
+						}
+
+				float	mRadius;	//!< Sphere radius
+	};
+
+#endif // __ICELSS_H__

src/external/open_dynamics_engine-ef/ode/IceMatrix3x3.cpp

@@ -0,0 +1,56 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for 3x3 matrices.
+ *	\file		IceMatrix3x3.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	3x3 matrix.
+ *	DirectX-compliant, ie row-column order, ie m[Row][Col].
+ *	Same as:
+ *	m11  m12  m13  first row.
+ *	m21  m22  m23  second row.
+ *	m31  m32  m33  third row.
+ *	Stored in memory as m11 m12 m13 m21...
+ *
+ *	Multiplication rules:
+ *
+ *	[x'y'z'] = [xyz][M]
+ *
+ *	x' = x*m11 + y*m21 + z*m31
+ *	y' = x*m12 + y*m22 + z*m32
+ *	z' = x*m13 + y*m23 + z*m33
+ *
+ *	\class		Matrix3x3
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+// Cast operator
+Matrix3x3::operator Matrix4x4() const
+{
+	return Matrix4x4(
+	m[0][0],	m[0][1],	m[0][2],	0.0f,
+	m[1][0],	m[1][1],	m[1][2],	0.0f,
+	m[2][0],	m[2][1],	m[2][2],	0.0f,
+	0.0f,		0.0f,		0.0f,		1.0f);
+}

src/external/open_dynamics_engine-ef/ode/IceMatrix3x3.h

@@ -0,0 +1,496 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for 3x3 matrices.
+ *	\file		IceMatrix3x3.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEMATRIX3X3_H__
+#define __ICEMATRIX3X3_H__
+
+	// Forward declarations
+	class Quat;
+
+	#define	MATRIX3X3_EPSILON		(1.0e-7f)
+
+	class ICEMATHS_API Matrix3x3
+	{
+		public:
+		//! Empty constructor
+		inline_					Matrix3x3()									{}
+		//! Constructor from 9 values
+		inline_					Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
+								{
+									m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;
+									m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;
+									m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;
+								}
+		//! Copy constructor
+		inline_					Matrix3x3(const Matrix3x3& mat)				{ CopyMemory(m, &mat.m, 9*sizeof(float));	}
+		//! Destructor
+		inline_					~Matrix3x3()								{}
+
+		//! Assign values
+		inline_	void			Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
+								{
+									m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;
+									m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;
+									m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;
+								}
+
+		//! Sets the scale from a Point. The point is put on the diagonal.
+		inline_	void			SetScale(const Point& p)					{ m[0][0] = p.x;	m[1][1] = p.y;	m[2][2] = p.z;	}
+
+		//! Sets the scale from floats. Values are put on the diagonal.
+		inline_	void			SetScale(float sx, float sy, float sz)		{ m[0][0] = sx;		m[1][1] = sy;	m[2][2] = sz;	}
+
+		//! Scales from a Point. Each row is multiplied by a component.
+		inline_	void			Scale(const Point& p)
+								{
+									m[0][0] *= p.x;	m[0][1] *= p.x;	m[0][2] *= p.x;
+									m[1][0] *= p.y;	m[1][1] *= p.y;	m[1][2] *= p.y;
+									m[2][0] *= p.z;	m[2][1] *= p.z;	m[2][2] *= p.z;
+								}
+
+		//! Scales from floats. Each row is multiplied by a value.
+		inline_	void			Scale(float sx, float sy, float sz)
+								{
+									m[0][0] *= sx;	m[0][1] *= sx;	m[0][2] *= sx;
+									m[1][0] *= sy;	m[1][1] *= sy;	m[1][2] *= sy;
+									m[2][0] *= sz;	m[2][1] *= sz;	m[2][2] *= sz;
+								}
+
+		//! Copy from a Matrix3x3
+		inline_	void			Copy(const Matrix3x3& source)				{ CopyMemory(m, source.m, 9*sizeof(float));			}
+
+		// Row-column access
+		//! Returns a row.
+		inline_	void			GetRow(const udword r, Point& p)	const	{ p.x = m[r][0];	p.y = m[r][1];	p.z = m[r][2];	}
+		//! Returns a row.
+		inline_	const Point&	GetRow(const udword r)				const	{ return *(const Point*)&m[r][0];	}
+		//! Returns a row.
+		inline_	Point&			GetRow(const udword r)						{ return *(Point*)&m[r][0];			}
+		//! Sets a row.
+		inline_	void			SetRow(const udword r, const Point& p)		{ m[r][0] = p.x;	m[r][1] = p.y;	m[r][2] = p.z;	}
+		//! Returns a column.
+		inline_	void			GetCol(const udword c, Point& p)	const	{ p.x = m[0][c];	p.y = m[1][c];	p.z = m[2][c];	}
+		//! Sets a column.
+		inline_	void			SetCol(const udword c, const Point& p)		{ m[0][c] = p.x;	m[1][c] = p.y;	m[2][c] = p.z;	}
+
+		//! Computes the trace. The trace is the sum of the 3 diagonal components.
+		inline_	float			Trace()					const				{ return m[0][0] + m[1][1] + m[2][2];				}
+		//! Clears the matrix.
+		inline_	void			Zero()										{ ZeroMemory(&m, sizeof(m));						}
+		//! Sets the identity matrix.
+		inline_	void			Identity()									{ Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; 		}
+		//! Checks for identity
+		inline_	bool			IsIdentity()			const
+								{
+									if(IR(m[0][0])!=IEEE_1_0)	return false;
+									if(IR(m[0][1])!=0)			return false;
+									if(IR(m[0][2])!=0)			return false;
+
+									if(IR(m[1][0])!=0)			return false;
+									if(IR(m[1][1])!=IEEE_1_0)	return false;
+									if(IR(m[1][2])!=0)			return false;
+
+									if(IR(m[2][0])!=0)			return false;
+									if(IR(m[2][1])!=0)			return false;
+									if(IR(m[2][2])!=IEEE_1_0)	return false;
+
+									return true;
+								}
+
+		//! Checks matrix validity
+		inline_	BOOL			IsValid()				const
+								{
+									for(udword j=0;j<3;j++)
+									{
+										for(udword i=0;i<3;i++)
+										{
+											if(!IsValidFloat(m[j][i]))	return FALSE;
+										}
+									}
+									return TRUE;
+								}
+
+		//! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix)
+		//!	[  0.0  -a.z   a.y ]
+		//!	[  a.z   0.0  -a.x ]
+		//!	[ -a.y   a.x   0.0 ]
+		//! This is also called a "cross matrix" since for any vectors A and B,
+		//! A^B = Skew(A) * B = - B * Skew(A);
+		inline_	void			SkewSymmetric(const Point& a)
+								{
+									m[0][0] = 0.0f;
+									m[0][1] = -a.z;
+									m[0][2] = a.y;
+
+									m[1][0] = a.z;
+									m[1][1] = 0.0f;
+									m[1][2] = -a.x;
+
+									m[2][0] = -a.y;
+									m[2][1] = a.x;
+									m[2][2] = 0.0f;
+								}
+
+		//! Negates the matrix
+		inline_	void			Neg()
+								{
+									m[0][0] = -m[0][0];	m[0][1] = -m[0][1];	m[0][2] = -m[0][2];
+									m[1][0] = -m[1][0];	m[1][1] = -m[1][1];	m[1][2] = -m[1][2];
+									m[2][0] = -m[2][0];	m[2][1] = -m[2][1];	m[2][2] = -m[2][2];
+								}
+
+		//! Neg from another matrix
+		inline_	void			Neg(const Matrix3x3& mat)
+								{
+									m[0][0] = -mat.m[0][0];	m[0][1] = -mat.m[0][1];	m[0][2] = -mat.m[0][2];
+									m[1][0] = -mat.m[1][0];	m[1][1] = -mat.m[1][1];	m[1][2] = -mat.m[1][2];
+									m[2][0] = -mat.m[2][0];	m[2][1] = -mat.m[2][1];	m[2][2] = -mat.m[2][2];
+								}
+
+		//! Add another matrix
+		inline_	void			Add(const Matrix3x3& mat)
+								{
+									m[0][0] += mat.m[0][0];	m[0][1] += mat.m[0][1];	m[0][2] += mat.m[0][2];
+									m[1][0] += mat.m[1][0];	m[1][1] += mat.m[1][1];	m[1][2] += mat.m[1][2];
+									m[2][0] += mat.m[2][0];	m[2][1] += mat.m[2][1];	m[2][2] += mat.m[2][2];
+								}
+
+		//! Sub another matrix
+		inline_	void			Sub(const Matrix3x3& mat)
+								{
+									m[0][0] -= mat.m[0][0];	m[0][1]	-= mat.m[0][1];	m[0][2] -= mat.m[0][2];
+									m[1][0] -= mat.m[1][0];	m[1][1] -= mat.m[1][1];	m[1][2] -= mat.m[1][2];
+									m[2][0] -= mat.m[2][0];	m[2][1] -= mat.m[2][1];	m[2][2] -= mat.m[2][2];
+								}
+		//! Mac
+		inline_	void			Mac(const Matrix3x3& a, const Matrix3x3& b, float s)
+								{
+									m[0][0] = a.m[0][0] + b.m[0][0] * s;
+									m[0][1] = a.m[0][1] + b.m[0][1] * s;
+									m[0][2] = a.m[0][2] + b.m[0][2] * s;
+
+									m[1][0] = a.m[1][0] + b.m[1][0] * s;
+									m[1][1] = a.m[1][1] + b.m[1][1] * s;
+									m[1][2] = a.m[1][2] + b.m[1][2] * s;
+
+									m[2][0] = a.m[2][0] + b.m[2][0] * s;
+									m[2][1] = a.m[2][1] + b.m[2][1] * s;
+									m[2][2] = a.m[2][2] + b.m[2][2] * s;
+								}
+		//! Mac
+		inline_	void			Mac(const Matrix3x3& a, float s)
+								{
+									m[0][0] += a.m[0][0] * s;	m[0][1] += a.m[0][1] * s;	m[0][2] += a.m[0][2] * s;
+									m[1][0] += a.m[1][0] * s;	m[1][1] += a.m[1][1] * s;	m[1][2] += a.m[1][2] * s;
+									m[2][0] += a.m[2][0] * s;	m[2][1] += a.m[2][1] * s;	m[2][2] += a.m[2][2] * s;
+								}
+
+		//! this = A * s
+		inline_	void			Mult(const Matrix3x3& a, float s)
+								{
+									m[0][0] = a.m[0][0] * s;	m[0][1] = a.m[0][1] * s;	m[0][2] = a.m[0][2] * s;
+									m[1][0] = a.m[1][0] * s;	m[1][1] = a.m[1][1] * s;	m[1][2] = a.m[1][2] * s;
+									m[2][0] = a.m[2][0] * s;	m[2][1] = a.m[2][1] * s;	m[2][2] = a.m[2][2] * s;
+								}
+
+		inline_	void			Add(const Matrix3x3& a, const Matrix3x3& b)
+								{
+									m[0][0] = a.m[0][0] + b.m[0][0];	m[0][1] = a.m[0][1] + b.m[0][1];	m[0][2] = a.m[0][2] + b.m[0][2];
+									m[1][0] = a.m[1][0] + b.m[1][0];	m[1][1] = a.m[1][1] + b.m[1][1];	m[1][2] = a.m[1][2] + b.m[1][2];
+									m[2][0] = a.m[2][0] + b.m[2][0];	m[2][1] = a.m[2][1] + b.m[2][1];	m[2][2] = a.m[2][2] + b.m[2][2];
+								}
+
+		inline_	void			Sub(const Matrix3x3& a, const Matrix3x3& b)
+								{
+									m[0][0] = a.m[0][0] - b.m[0][0];	m[0][1] = a.m[0][1] - b.m[0][1];	m[0][2] = a.m[0][2] - b.m[0][2];
+									m[1][0] = a.m[1][0] - b.m[1][0];	m[1][1] = a.m[1][1] - b.m[1][1];	m[1][2] = a.m[1][2] - b.m[1][2];
+									m[2][0] = a.m[2][0] - b.m[2][0];	m[2][1] = a.m[2][1] - b.m[2][1];	m[2][2] = a.m[2][2] - b.m[2][2];
+								}
+
+		//! this = a * b
+		inline_	void			Mult(const Matrix3x3& a, const Matrix3x3& b)
+								{
+									m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
+									m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
+									m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2];
+									m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
+									m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];
+									m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2];
+									m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
+									m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];
+									m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2];
+								}
+
+		//! this = transpose(a) * b
+		inline_	void			MultAtB(const Matrix3x3& a, const Matrix3x3& b)
+								{
+									m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0];
+									m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1];
+									m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2];
+									m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0];
+									m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1];
+									m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2];
+									m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0];
+									m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1];
+									m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2];
+								}
+
+		//! this = a * transpose(b)
+		inline_	void			MultABt(const Matrix3x3& a, const Matrix3x3& b)
+								{
+									m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2];
+									m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2];
+									m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2];
+									m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2];
+									m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2];
+									m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2];
+									m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2];
+									m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2];
+									m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2];
+								}
+
+		//! Makes a rotation matrix mapping vector "from" to vector "to".
+				Matrix3x3&		FromTo(const Point& from, const Point& to);
+
+		//! Set a rotation matrix around the X axis.
+		//!		 1		0		0
+		//!	RX = 0		cx		sx
+		//!		 0		-sx		cx
+				void			RotX(float angle);
+		//! Set a rotation matrix around the Y axis.
+		//!		 cy		0		-sy
+		//!	RY = 0		1		0
+		//!		 sy		0		cy
+				void			RotY(float angle);
+		//! Set a rotation matrix around the Z axis.
+		//!		 cz		sz		0
+		//!	RZ = -sz	cz		0
+		//!		 0		0		1
+				void			RotZ(float angle);
+		//!			cy		sx.sy		-sy.cx
+		//!	RY.RX	0		cx			sx
+		//!			sy		-sx.cy		cx.cy
+				void			RotYX(float y, float x);
+
+		//! Make a rotation matrix about an arbitrary axis
+				Matrix3x3&		Rot(float angle, const Point& axis);
+
+		//! Transpose the matrix.
+				void			Transpose()
+								{
+									IR(m[1][0]) ^= IR(m[0][1]);	IR(m[0][1]) ^= IR(m[1][0]);	IR(m[1][0]) ^= IR(m[0][1]);
+									IR(m[2][0]) ^= IR(m[0][2]);	IR(m[0][2]) ^= IR(m[2][0]);	IR(m[2][0]) ^= IR(m[0][2]);
+									IR(m[2][1]) ^= IR(m[1][2]);	IR(m[1][2]) ^= IR(m[2][1]);	IR(m[2][1]) ^= IR(m[1][2]);
+								}
+
+		//! this = Transpose(a)
+				void			Transpose(const Matrix3x3& a)
+								{
+									m[0][0] = a.m[0][0];	m[0][1] = a.m[1][0];	m[0][2] = a.m[2][0];
+									m[1][0] = a.m[0][1];	m[1][1] = a.m[1][1];	m[1][2] = a.m[2][1];
+									m[2][0] = a.m[0][2];	m[2][1] = a.m[1][2];	m[2][2] = a.m[2][2];
+								}
+
+		//! Compute the determinant of the matrix. We use the rule of Sarrus.
+				float			Determinant()					const
+								{
+									return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1])
+										-  (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]);
+								}
+/*
+		//! Compute a cofactor. Used for matrix inversion.
+				float			CoFactor(ubyte row, ubyte column)	const
+				{
+					static sdword gIndex[3+2] = { 0, 1, 2, 0, 1 };
+					return	(m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]);
+				}
+*/
+		//! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted.
+				Matrix3x3&		Invert()
+								{
+									float Det = Determinant();	// Must be !=0
+									float OneOverDet = 1.0f / Det;
+
+									Matrix3x3 Temp;
+									Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet;
+									Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet;
+									Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet;
+									Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet;
+									Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet;
+									Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet;
+									Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet;
+									Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet;
+									Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet;
+
+									*this = Temp;
+
+									return	*this;
+								}
+
+				Matrix3x3&		Normalize();
+
+		//! this = exp(a)
+				Matrix3x3&		Exp(const Matrix3x3& a);
+
+void FromQuat(const Quat &q);
+void FromQuatL2(const Quat &q, float l2);
+
+		// Arithmetic operators
+		//! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3;
+		inline_	Matrix3x3		operator+(const Matrix3x3& mat)	const
+								{
+									return Matrix3x3(
+									m[0][0] + mat.m[0][0],	m[0][1] + mat.m[0][1],	m[0][2] + mat.m[0][2],
+									m[1][0] + mat.m[1][0],	m[1][1] + mat.m[1][1],	m[1][2] + mat.m[1][2],
+									m[2][0] + mat.m[2][0],	m[2][1] + mat.m[2][1],	m[2][2] + mat.m[2][2]);
+								}
+
+		//! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3;
+		inline_	Matrix3x3		operator-(const Matrix3x3& mat)	const
+								{
+									return Matrix3x3(
+									m[0][0] - mat.m[0][0],	m[0][1] - mat.m[0][1],	m[0][2] - mat.m[0][2],
+									m[1][0] - mat.m[1][0],	m[1][1] - mat.m[1][1],	m[1][2] - mat.m[1][2],
+									m[2][0] - mat.m[2][0],	m[2][1] - mat.m[2][1],	m[2][2] - mat.m[2][2]);
+								}
+
+		//! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3;
+		inline_	Matrix3x3		operator*(const Matrix3x3& mat)	const
+								{
+									return Matrix3x3(
+									m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0],
+									m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1],
+									m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2],
+
+									m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0],
+									m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1],
+									m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2],
+
+									m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0],
+									m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1],
+									m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]);
+								}
+
+		//! Operator for Point Mul = Matrix3x3 * Point;
+		inline_	Point			operator*(const Point& v)		const		{ return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); }
+
+		//! Operator for Matrix3x3 Mul = Matrix3x3 * float;
+		inline_	Matrix3x3		operator*(float s)				const
+								{
+									return Matrix3x3(
+									m[0][0]*s,	m[0][1]*s,	m[0][2]*s,
+									m[1][0]*s,	m[1][1]*s,	m[1][2]*s,
+									m[2][0]*s,	m[2][1]*s,	m[2][2]*s);
+								}
+
+		//! Operator for Matrix3x3 Mul = float * Matrix3x3;
+		inline_	friend Matrix3x3 operator*(float s, const Matrix3x3& mat)
+								{
+									return Matrix3x3(
+									s*mat.m[0][0],	s*mat.m[0][1],	s*mat.m[0][2],
+									s*mat.m[1][0],	s*mat.m[1][1],	s*mat.m[1][2],
+									s*mat.m[2][0],	s*mat.m[2][1],	s*mat.m[2][2]);
+								}
+
+		//! Operator for Matrix3x3 Div = Matrix3x3 / float;
+		inline_	Matrix3x3		operator/(float s)				const
+								{
+									if (s)	s = 1.0f / s;
+									return Matrix3x3(
+									m[0][0]*s,	m[0][1]*s,	m[0][2]*s,
+									m[1][0]*s,	m[1][1]*s,	m[1][2]*s,
+									m[2][0]*s,	m[2][1]*s,	m[2][2]*s);
+								}
+
+		//! Operator for Matrix3x3 Div = float / Matrix3x3;
+		inline_	friend Matrix3x3 operator/(float s, const Matrix3x3& mat)
+								{
+									return Matrix3x3(
+									s/mat.m[0][0],	s/mat.m[0][1],	s/mat.m[0][2],
+									s/mat.m[1][0],	s/mat.m[1][1],	s/mat.m[1][2],
+									s/mat.m[2][0],	s/mat.m[2][1],	s/mat.m[2][2]);
+								}
+
+		//! Operator for Matrix3x3 += Matrix3x3
+		inline_	Matrix3x3&		operator+=(const Matrix3x3& mat)
+								{
+									m[0][0] += mat.m[0][0];		m[0][1] += mat.m[0][1];		m[0][2] += mat.m[0][2];
+									m[1][0] += mat.m[1][0];		m[1][1] += mat.m[1][1];		m[1][2] += mat.m[1][2];
+									m[2][0] += mat.m[2][0];		m[2][1] += mat.m[2][1];		m[2][2] += mat.m[2][2];
+									return	*this;
+								}
+
+		//! Operator for Matrix3x3 -= Matrix3x3
+		inline_	Matrix3x3&		operator-=(const Matrix3x3& mat)
+								{
+									m[0][0] -= mat.m[0][0];		m[0][1] -= mat.m[0][1];		m[0][2] -= mat.m[0][2];
+									m[1][0] -= mat.m[1][0];		m[1][1] -= mat.m[1][1];		m[1][2] -= mat.m[1][2];
+									m[2][0] -= mat.m[2][0];		m[2][1] -= mat.m[2][1];		m[2][2] -= mat.m[2][2];
+									return	*this;
+								}
+
+		//! Operator for Matrix3x3 *= Matrix3x3
+		inline_	Matrix3x3&		operator*=(const Matrix3x3& mat)
+								{
+									Point TempRow;
+
+									GetRow(0, TempRow);
+									m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
+									m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
+									m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
+
+									GetRow(1, TempRow);
+									m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
+									m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
+									m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
+
+									GetRow(2, TempRow);
+									m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
+									m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
+									m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
+									return	*this;
+								}
+
+		//! Operator for Matrix3x3 *= float
+		inline_	Matrix3x3&		operator*=(float s)
+								{
+									m[0][0] *= s;	m[0][1] *= s;	m[0][2] *= s;
+									m[1][0] *= s;	m[1][1] *= s;	m[1][2] *= s;
+									m[2][0] *= s;	m[2][1] *= s;	m[2][2] *= s;
+									return	*this;
+								}
+
+		//! Operator for Matrix3x3 /= float
+		inline_	Matrix3x3&		operator/=(float s)
+								{
+									if (s)	s = 1.0f / s;
+									m[0][0] *= s;	m[0][1] *= s;	m[0][2] *= s;
+									m[1][0] *= s;	m[1][1] *= s;	m[1][2] *= s;
+									m[2][0] *= s;	m[2][1] *= s;	m[2][2] *= s;
+									return	*this;
+								}
+
+		// Cast operators
+		//! Cast a Matrix3x3 to a Matrix4x4.
+								operator Matrix4x4()	const;
+		//! Cast a Matrix3x3 to a Quat.
+								operator Quat()			const;
+
+		inline_	const Point&	operator[](int row)		const	{ return *(const Point*)&m[row][0];	}
+		inline_	Point&			operator[](int row)				{ return *(Point*)&m[row][0];		}
+
+		public:
+
+				float			m[3][3];
+	};
+
+#endif // __ICEMATRIX3X3_H__
+

src/external/open_dynamics_engine-ef/ode/IceMatrix4x4.cpp

@@ -0,0 +1,142 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for 4x4 matrices.
+ *	\file		IceMatrix4x4.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	4x4 matrix.
+ *	DirectX-compliant, ie row-column order, ie m[Row][Col].
+ *	Same as:
+ *	m11  m12  m13  m14	first row.
+ *	m21  m22  m23  m24	second row.
+ *	m31  m32  m33  m34	third row.
+ *	m41  m42  m43  m44	fourth row.
+ *	Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
+ *	Stored in memory as m11 m12 m13 m14 m21...
+ *
+ *	Multiplication rules:
+ *
+ *	[x'y'z'1] = [xyz1][M]
+ *
+ *	x' = x*m11 + y*m21 + z*m31 + m41
+ *	y' = x*m12 + y*m22 + z*m32 + m42
+ *	z' = x*m13 + y*m23 + z*m33 + m43
+ *	1' =     0 +     0 +     0 + m44
+ *
+ *	\class		Matrix4x4
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Inverts a PR matrix. (which only contains a rotation and a translation)
+ *	This is faster and less subject to FPU errors than the generic inversion code.
+ *
+ *	\relates	Matrix4x4
+ *	\fn			InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
+ *	\param		dest	[out] destination matrix
+ *	\param		src		[in] source matrix
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
+{
+	dest.m[0][0] = src.m[0][0];
+	dest.m[1][0] = src.m[0][1];
+	dest.m[2][0] = src.m[0][2];
+	dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);
+
+	dest.m[0][1] = src.m[1][0];
+	dest.m[1][1] = src.m[1][1];
+	dest.m[2][1] = src.m[1][2];
+	dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);
+
+	dest.m[0][2] = src.m[2][0];
+	dest.m[1][2] = src.m[2][1];
+	dest.m[2][2] = src.m[2][2];
+	dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);
+
+	dest.m[0][3] = 0.0f;
+	dest.m[1][3] = 0.0f;
+	dest.m[2][3] = 0.0f;
+	dest.m[3][3] = 1.0f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Compute the cofactor of the Matrix at a specified location
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Matrix4x4::CoFactor(udword row, udword col) const
+{
+	return	 (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
+				m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
+				m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
+			-  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
+				m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
+				m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Compute the determinant of the Matrix
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Matrix4x4::Determinant() const
+{
+	return	m[0][0] * CoFactor(0, 0) +
+			m[0][1] * CoFactor(0, 1) +
+			m[0][2] * CoFactor(0, 2) +
+			m[0][3] * CoFactor(0, 3);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Compute the inverse of the matrix
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Matrix4x4& Matrix4x4::Invert()
+{
+	float Det = Determinant();
+	Matrix4x4 Temp;
+
+	if(fabsf(Det) < MATRIX4X4_EPSILON)
+		return	*this;		// The matrix is not invertible! Singular case!
+
+	float IDet = 1.0f / Det;
+
+	Temp.m[0][0] = CoFactor(0,0) * IDet;
+	Temp.m[1][0] = CoFactor(0,1) * IDet;
+	Temp.m[2][0] = CoFactor(0,2) * IDet;
+	Temp.m[3][0] = CoFactor(0,3) * IDet;
+	Temp.m[0][1] = CoFactor(1,0) * IDet;
+	Temp.m[1][1] = CoFactor(1,1) * IDet;
+	Temp.m[2][1] = CoFactor(1,2) * IDet;
+	Temp.m[3][1] = CoFactor(1,3) * IDet;
+	Temp.m[0][2] = CoFactor(2,0) * IDet;
+	Temp.m[1][2] = CoFactor(2,1) * IDet;
+	Temp.m[2][2] = CoFactor(2,2) * IDet;
+	Temp.m[3][2] = CoFactor(2,3) * IDet;
+	Temp.m[0][3] = CoFactor(3,0) * IDet;
+	Temp.m[1][3] = CoFactor(3,1) * IDet;
+	Temp.m[2][3] = CoFactor(3,2) * IDet;
+	Temp.m[3][3] = CoFactor(3,3) * IDet;
+
+	*this = Temp;
+
+	return	*this;
+}

src/external/open_dynamics_engine-ef/ode/IceMatrix4x4.h

@@ -0,0 +1,455 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for 4x4 matrices.
+ *	\file		IceMatrix4x4.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEMATRIX4X4_H__
+#define __ICEMATRIX4X4_H__
+
+	// Forward declarations
+	class PRS;
+	class PR;
+
+	#define	MATRIX4X4_EPSILON		(1.0e-7f)
+
+	class ICEMATHS_API Matrix4x4
+	{
+//				void	LUBackwardSubstitution( sdword *indx, float* b );
+//				void	LUDecomposition( sdword* indx, float* d );
+
+		public:
+		//! Empty constructor.
+		inline_						Matrix4x4()									{}
+		//! Constructor from 16 values
+		inline_						Matrix4x4(	float m00, float m01, float m02, float m03,
+												float m10, float m11, float m12, float m13,
+												float m20, float m21, float m22, float m23,
+												float m30, float m31, float m32, float m33)
+									{
+										m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;	m[0][3] = m03;
+										m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;	m[1][3] = m13;
+										m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;	m[2][3] = m23;
+										m[3][0] = m30;	m[3][1] = m31;	m[3][2] = m32;	m[3][3] = m33;
+									}
+		//! Copy constructor
+		inline_						Matrix4x4(const Matrix4x4& mat)				{ CopyMemory(m, &mat.m, 16*sizeof(float));	}
+		//! Destructor.
+		inline_						~Matrix4x4()								{}
+
+		//! Assign values (rotation only)
+		inline_	Matrix4x4&			Set(	float m00, float m01, float m02,
+											float m10, float m11, float m12,
+											float m20, float m21, float m22)
+									{
+										m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;
+										m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;
+										m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;
+										return *this;
+									}
+		//! Assign values
+		inline_	Matrix4x4&			Set(	float m00, float m01, float m02, float m03,
+											float m10, float m11, float m12, float m13,
+											float m20, float m21, float m22, float m23,
+											float m30, float m31, float m32, float m33)
+									{
+										m[0][0] = m00;	m[0][1] = m01;	m[0][2] = m02;	m[0][3] = m03;
+										m[1][0] = m10;	m[1][1] = m11;	m[1][2] = m12;	m[1][3] = m13;
+										m[2][0] = m20;	m[2][1] = m21;	m[2][2] = m22;	m[2][3] = m23;
+										m[3][0] = m30;	m[3][1] = m31;	m[3][2] = m32;	m[3][3] = m33;
+										return *this;
+									}
+
+		//! Copy from a Matrix4x4
+		inline_	void				Copy(const Matrix4x4& source)				{ CopyMemory(m, source.m, 16*sizeof(float));	}
+
+		// Row-column access
+		//! Returns a row.
+		inline_	void				GetRow(const udword r, HPoint& p)	const	{ p.x=m[r][0];	p.y=m[r][1];	p.z=m[r][2];	p.w=m[r][3];		}
+		//! Returns a row.
+		inline_	void				GetRow(const udword r, Point& p)	const	{ p.x=m[r][0];	p.y=m[r][1];	p.z=m[r][2];						}
+		//! Returns a row.
+		inline_	const HPoint&		GetRow(const udword r)				const	{ return *(const HPoint*)&m[r][0];									}
+		//! Returns a row.
+		inline_	HPoint&				GetRow(const udword r)						{ return *(HPoint*)&m[r][0];										}
+		//! Sets a row.
+		inline_	void				SetRow(const udword r, const HPoint& p)		{ m[r][0]=p.x;	m[r][1]=p.y;	m[r][2]=p.z;	m[r][3]=p.w;		}
+		//! Sets a row.
+		inline_	void				SetRow(const udword r, const Point& p)		{ m[r][0]=p.x;	m[r][1]=p.y;	m[r][2]=p.z;	m[r][3]= (r!=3) ? 0.0f : 1.0f;		}
+		//! Returns a column.
+		inline_	void				GetCol(const udword c, HPoint& p)	const	{ p.x=m[0][c];	p.y=m[1][c];	p.z=m[2][c];	p.w=m[3][c];		}
+		//! Returns a column.
+		inline_	void				GetCol(const udword c, Point& p)	const	{ p.x=m[0][c];	p.y=m[1][c];	p.z=m[2][c];						}
+		//! Sets a column.
+		inline_	void				SetCol(const udword c, const HPoint& p)		{ m[0][c]=p.x;	m[1][c]=p.y;	m[2][c]=p.z;	m[3][c]=p.w;		}
+		//! Sets a column.
+		inline_	void				SetCol(const udword c, const Point& p)		{ m[0][c]=p.x;	m[1][c]=p.y;	m[2][c]=p.z;	m[3][c]= (c!=3) ? 0.0f : 1.0f;	}
+
+		// Translation
+		//! Returns the translation part of the matrix.
+		inline_	const HPoint&		GetTrans()							const	{ return GetRow(3);								}
+		//! Gets the translation part of the matrix
+		inline_	void				GetTrans(Point& p)					const	{ p.x=m[3][0];	p.y=m[3][1];	p.z=m[3][2];	}
+		//! Sets the translation part of the matrix, from a Point.
+		inline_	void				SetTrans(const Point& p)					{ m[3][0]=p.x;	m[3][1]=p.y;	m[3][2]=p.z;	}
+		//! Sets the translation part of the matrix, from a HPoint.
+		inline_	void				SetTrans(const HPoint& p)					{ m[3][0]=p.x;	m[3][1]=p.y;	m[3][2]=p.z;	m[3][3]=p.w;	}
+		//! Sets the translation part of the matrix, from floats.
+		inline_	void				SetTrans(float tx, float ty, float tz)		{ m[3][0]=tx;	m[3][1]=ty;		m[3][2]=tz;		}
+
+		// Scale
+		//! Sets the scale from a Point. The point is put on the diagonal.
+		inline_	void				SetScale(const Point& p)					{ m[0][0]=p.x;	m[1][1]=p.y;	m[2][2]=p.z;	}
+		//! Sets the scale from floats. Values are put on the diagonal.
+		inline_	void				SetScale(float sx, float sy, float sz)		{ m[0][0]=sx;	m[1][1]=sy;		m[2][2]=sz;		}
+		//! Scales from a Point. Each row is multiplied by a component.
+				void				Scale(const Point& p)
+									{
+										m[0][0] *= p.x;	m[1][0] *= p.y;	m[2][0] *= p.z;
+										m[0][1] *= p.x;	m[1][1] *= p.y;	m[2][1] *= p.z;
+										m[0][2] *= p.x;	m[1][2] *= p.y;	m[2][2] *= p.z;
+									}
+		//! Scales from floats. Each row is multiplied by a value.
+				void				Scale(float sx, float sy, float sz)
+									{
+										m[0][0] *= sx;	m[1][0] *= sy;	m[2][0] *= sz;
+										m[0][1] *= sx;	m[1][1] *= sy;	m[2][1] *= sz;
+										m[0][2] *= sx;	m[1][2] *= sy;	m[2][2] *= sz;
+									}
+/*
+		//! Returns a row.
+		inline_	HPoint		GetRow(const udword row)	const			{ return mRow[row];														}
+		//! Sets a row.
+		inline_	Matrix4x4&	SetRow(const udword row, const HPoint& p)	{ mRow[row] = p;	return *this;										}
+		//! Sets a row.
+						Matrix4x4&	SetRow(const udword row, const Point& p)
+						{
+							m[row][0] = p.x;
+							m[row][1] = p.y;
+							m[row][2] = p.z;
+							m[row][3] = (row != 3) ? 0.0f : 1.0f;
+							return	*this;
+						}
+		//! Returns a column.
+						HPoint		GetCol(const udword col)		const
+						{
+							HPoint	Res;
+							Res.x = m[0][col];
+							Res.y = m[1][col];
+							Res.z = m[2][col];
+							Res.w = m[3][col];
+							return	Res;
+						}
+		//! Sets a column.
+						Matrix4x4&	SetCol(const udword col, const HPoint& p)
+						{
+							m[0][col] = p.x;
+							m[1][col] = p.y;
+							m[2][col] = p.z;
+							m[3][col] = p.w;
+							return	*this;
+						}
+		//! Sets a column.
+						Matrix4x4&	SetCol(const udword col, const Point& p)
+						{
+							m[0][col] = p.x;
+							m[1][col] = p.y;
+							m[2][col] = p.z;
+							m[3][col] = (col != 3) ? 0.0f : 1.0f;
+							return	*this;
+						}
+*/
+		//! Computes the trace. The trace is the sum of the 4 diagonal components.
+		inline_	float				Trace()			const			{ return m[0][0] + m[1][1] + m[2][2] + m[3][3];			}
+		//! Computes the trace of the upper 3x3 matrix.
+		inline_	float				Trace3x3()		const			{ return m[0][0] + m[1][1] + m[2][2];					}
+		//! Clears the matrix.
+		inline_	void				Zero()							{ ZeroMemory(&m,  sizeof(m));							}
+		//! Sets the identity matrix.
+		inline_	void				Identity()						{ Zero(); m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;	}
+		//! Checks for identity
+		inline_	bool				IsIdentity()	const
+									{
+										if(IR(m[0][0])!=IEEE_1_0)	return false;
+										if(IR(m[0][1])!=0)			return false;
+										if(IR(m[0][2])!=0)			return false;
+										if(IR(m[0][3])!=0)			return false;
+
+										if(IR(m[1][0])!=0)			return false;
+										if(IR(m[1][1])!=IEEE_1_0)	return false;
+										if(IR(m[1][2])!=0)			return false;
+										if(IR(m[1][3])!=0)			return false;
+
+										if(IR(m[2][0])!=0)			return false;
+										if(IR(m[2][1])!=0)			return false;
+										if(IR(m[2][2])!=IEEE_1_0)	return false;
+										if(IR(m[2][3])!=0)			return false;
+
+										if(IR(m[3][0])!=0)			return false;
+										if(IR(m[3][1])!=0)			return false;
+										if(IR(m[3][2])!=0)			return false;
+										if(IR(m[3][3])!=IEEE_1_0)	return false;
+										return true;
+									}
+
+		//! Checks matrix validity
+		inline_	BOOL				IsValid()		const
+									{
+										for(udword j=0;j<4;j++)
+										{
+											for(udword i=0;i<4;i++)
+											{
+												if(!IsValidFloat(m[j][i]))	return FALSE;
+											}
+										}
+										return TRUE;
+									}
+
+		//! Sets a rotation matrix around the X axis.
+				void				RotX(float angle)	{ float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[1][1] = m[2][2] = Cos; m[2][1] = -Sin;	m[1][2] = Sin;	}
+		//! Sets a rotation matrix around the Y axis.
+				void				RotY(float angle)	{ float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[2][2] = Cos; m[2][0] = Sin;	m[0][2] = -Sin;	}
+		//! Sets a rotation matrix around the Z axis.
+				void				RotZ(float angle)	{ float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[1][1] = Cos; m[1][0] = -Sin;	m[0][1] = Sin;	}
+
+		//! Makes a rotation matrix about an arbitrary axis
+				Matrix4x4&			Rot(float angle, Point& p1, Point& p2);
+
+		//! Transposes the matrix.
+				void				Transpose()
+									{
+										IR(m[1][0]) ^= IR(m[0][1]);		IR(m[0][1]) ^= IR(m[1][0]);		IR(m[1][0]) ^= IR(m[0][1]);
+										IR(m[2][0]) ^= IR(m[0][2]);		IR(m[0][2]) ^= IR(m[2][0]);		IR(m[2][0]) ^= IR(m[0][2]);
+										IR(m[3][0]) ^= IR(m[0][3]);		IR(m[0][3]) ^= IR(m[3][0]);		IR(m[3][0]) ^= IR(m[0][3]);
+										IR(m[1][2]) ^= IR(m[2][1]);		IR(m[2][1]) ^= IR(m[1][2]);		IR(m[1][2]) ^= IR(m[2][1]);
+										IR(m[1][3]) ^= IR(m[3][1]);		IR(m[3][1]) ^= IR(m[1][3]);		IR(m[1][3]) ^= IR(m[3][1]);
+										IR(m[2][3]) ^= IR(m[3][2]);		IR(m[3][2]) ^= IR(m[2][3]);		IR(m[2][3]) ^= IR(m[3][2]);
+									}
+
+		//! Computes a cofactor. Used for matrix inversion.
+				float				CoFactor(udword row, udword col)	const;
+		//! Computes the determinant of the matrix.
+				float				Determinant()	const;
+		//! Inverts the matrix. Determinant must be different from zero, else matrix can't be inverted.
+				Matrix4x4&			Invert();
+//				Matrix&	ComputeAxisMatrix(Point& axis, float angle);
+
+		// Cast operators
+		//! Casts a Matrix4x4 to a Matrix3x3.
+		inline_	operator			Matrix3x3()	const
+									{
+										return Matrix3x3(
+										m[0][0],	m[0][1],	m[0][2],
+										m[1][0],	m[1][1],	m[1][2],
+										m[2][0],	m[2][1],	m[2][2]);
+									}
+		//! Casts a Matrix4x4 to a Quat.
+				operator			Quat()	const;
+		//! Casts a Matrix4x4 to a PR.
+				operator			PR()	const;
+
+		// Arithmetic operators
+		//! Operator for Matrix4x4 Plus = Matrix4x4 + Matrix4x4;
+		inline_	Matrix4x4			operator+(const Matrix4x4& mat)	const
+									{
+										return Matrix4x4(
+										m[0][0]+mat.m[0][0], m[0][1]+mat.m[0][1], m[0][2]+mat.m[0][2], m[0][3]+mat.m[0][3], 
+										m[1][0]+mat.m[1][0], m[1][1]+mat.m[1][1], m[1][2]+mat.m[1][2], m[1][3]+mat.m[1][3], 
+										m[2][0]+mat.m[2][0], m[2][1]+mat.m[2][1], m[2][2]+mat.m[2][2], m[2][3]+mat.m[2][3], 
+										m[3][0]+mat.m[3][0], m[3][1]+mat.m[3][1], m[3][2]+mat.m[3][2], m[3][3]+mat.m[3][3]);
+									}
+
+		//! Operator for Matrix4x4 Minus = Matrix4x4 - Matrix4x4;
+		inline_	Matrix4x4			operator-(const Matrix4x4& mat)	const
+									{
+										return Matrix4x4(
+										m[0][0]-mat.m[0][0], m[0][1]-mat.m[0][1], m[0][2]-mat.m[0][2], m[0][3]-mat.m[0][3], 
+										m[1][0]-mat.m[1][0], m[1][1]-mat.m[1][1], m[1][2]-mat.m[1][2], m[1][3]-mat.m[1][3], 
+										m[2][0]-mat.m[2][0], m[2][1]-mat.m[2][1], m[2][2]-mat.m[2][2], m[2][3]-mat.m[2][3], 
+										m[3][0]-mat.m[3][0], m[3][1]-mat.m[3][1], m[3][2]-mat.m[3][2], m[3][3]-mat.m[3][3]);
+									}
+
+		//! Operator for Matrix4x4 Mul = Matrix4x4 * Matrix4x4;
+		inline_	Matrix4x4			operator*(const Matrix4x4& mat)	const
+									{
+										return Matrix4x4(
+										m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0] + m[0][3]*mat.m[3][0],
+										m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1] + m[0][3]*mat.m[3][1],
+										m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2] + m[0][3]*mat.m[3][2],
+										m[0][0]*mat.m[0][3] + m[0][1]*mat.m[1][3] + m[0][2]*mat.m[2][3] + m[0][3]*mat.m[3][3],
+
+										m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0] + m[1][3]*mat.m[3][0],
+										m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1] + m[1][3]*mat.m[3][1],
+										m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2] + m[1][3]*mat.m[3][2],
+										m[1][0]*mat.m[0][3] + m[1][1]*mat.m[1][3] + m[1][2]*mat.m[2][3] + m[1][3]*mat.m[3][3],
+
+										m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0] + m[2][3]*mat.m[3][0],
+										m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1] + m[2][3]*mat.m[3][1],
+										m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2] + m[2][3]*mat.m[3][2],
+										m[2][0]*mat.m[0][3] + m[2][1]*mat.m[1][3] + m[2][2]*mat.m[2][3] + m[2][3]*mat.m[3][3],
+
+										m[3][0]*mat.m[0][0] + m[3][1]*mat.m[1][0] + m[3][2]*mat.m[2][0] + m[3][3]*mat.m[3][0],
+										m[3][0]*mat.m[0][1] + m[3][1]*mat.m[1][1] + m[3][2]*mat.m[2][1] + m[3][3]*mat.m[3][1],
+										m[3][0]*mat.m[0][2] + m[3][1]*mat.m[1][2] + m[3][2]*mat.m[2][2] + m[3][3]*mat.m[3][2],
+										m[3][0]*mat.m[0][3] + m[3][1]*mat.m[1][3] + m[3][2]*mat.m[2][3] + m[3][3]*mat.m[3][3]);
+									}
+
+		//! Operator for HPoint Mul = Matrix4x4 * HPoint;
+		inline_	HPoint				operator*(const HPoint& v)		const	{ return HPoint(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v, GetRow(3)|v); }
+
+		//! Operator for Point Mul = Matrix4x4 * Point;
+		inline_	Point				operator*(const Point& v)		const
+									{
+										return Point(	m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3],
+														m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3],
+														m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3]	);
+									}
+
+		//! Operator for Matrix4x4 Scale = Matrix4x4 * float;
+		inline_	Matrix4x4			operator*(float s)				const
+									{
+										return Matrix4x4(
+										m[0][0]*s,	m[0][1]*s,	m[0][2]*s,	m[0][3]*s,
+										m[1][0]*s,	m[1][1]*s,	m[1][2]*s,	m[1][3]*s,
+										m[2][0]*s,	m[2][1]*s,	m[2][2]*s,	m[2][3]*s,
+										m[3][0]*s,	m[3][1]*s,	m[3][2]*s,	m[3][3]*s);
+									}
+
+		//! Operator for Matrix4x4 Scale = float * Matrix4x4;
+		inline_	friend Matrix4x4	operator*(float s, const Matrix4x4& mat)
+									{
+										return Matrix4x4(
+										s*mat.m[0][0],	s*mat.m[0][1],	s*mat.m[0][2],	s*mat.m[0][3],
+										s*mat.m[1][0],	s*mat.m[1][1],	s*mat.m[1][2],	s*mat.m[1][3],
+										s*mat.m[2][0],	s*mat.m[2][1],	s*mat.m[2][2],	s*mat.m[2][3],
+										s*mat.m[3][0],	s*mat.m[3][1],	s*mat.m[3][2],	s*mat.m[3][3]);
+									}
+
+		//! Operator for Matrix4x4 Div = Matrix4x4 / float;
+		inline_	Matrix4x4			operator/(float s)				const
+									{
+										if(s) s = 1.0f / s;
+
+										return Matrix4x4(
+										m[0][0]*s,	m[0][1]*s,	m[0][2]*s,	m[0][3]*s,
+										m[1][0]*s,	m[1][1]*s,	m[1][2]*s,	m[1][3]*s,
+										m[2][0]*s,	m[2][1]*s,	m[2][2]*s,	m[2][3]*s,
+										m[3][0]*s,	m[3][1]*s,	m[3][2]*s,	m[3][3]*s);
+									}
+
+		//! Operator for Matrix4x4 Div = float / Matrix4x4;
+		inline_	friend Matrix4x4	operator/(float s, const Matrix4x4& mat)
+									{
+										return Matrix4x4(
+										s/mat.m[0][0],	s/mat.m[0][1],	s/mat.m[0][2],	s/mat.m[0][3],
+										s/mat.m[1][0],	s/mat.m[1][1],	s/mat.m[1][2],	s/mat.m[1][3],
+										s/mat.m[2][0],	s/mat.m[2][1],	s/mat.m[2][2],	s/mat.m[2][3],
+										s/mat.m[3][0],	s/mat.m[3][1],	s/mat.m[3][2],	s/mat.m[3][3]);
+									}
+
+		//! Operator for Matrix4x4 += Matrix4x4;
+		inline_	Matrix4x4&			operator+=(const Matrix4x4& mat)
+									{
+										m[0][0]+=mat.m[0][0];	m[0][1]+=mat.m[0][1];	m[0][2]+=mat.m[0][2];	m[0][3]+=mat.m[0][3];
+										m[1][0]+=mat.m[1][0];	m[1][1]+=mat.m[1][1];	m[1][2]+=mat.m[1][2];	m[1][3]+=mat.m[1][3];
+										m[2][0]+=mat.m[2][0];	m[2][1]+=mat.m[2][1];	m[2][2]+=mat.m[2][2];	m[2][3]+=mat.m[2][3];
+										m[3][0]+=mat.m[3][0];	m[3][1]+=mat.m[3][1];	m[3][2]+=mat.m[3][2];	m[3][3]+=mat.m[3][3];
+										return	*this;
+									}
+
+		//! Operator for Matrix4x4 -= Matrix4x4;
+		inline_	Matrix4x4&			operator-=(const Matrix4x4& mat)
+									{
+										m[0][0]-=mat.m[0][0];	m[0][1]-=mat.m[0][1];	m[0][2]-=mat.m[0][2];	m[0][3]-=mat.m[0][3];
+										m[1][0]-=mat.m[1][0];	m[1][1]-=mat.m[1][1];	m[1][2]-=mat.m[1][2];	m[1][3]-=mat.m[1][3];
+										m[2][0]-=mat.m[2][0];	m[2][1]-=mat.m[2][1];	m[2][2]-=mat.m[2][2];	m[2][3]-=mat.m[2][3];
+										m[3][0]-=mat.m[3][0];	m[3][1]-=mat.m[3][1];	m[3][2]-=mat.m[3][2];	m[3][3]-=mat.m[3][3];
+										return	*this;
+									}
+
+		//! Operator for Matrix4x4 *= Matrix4x4;
+				Matrix4x4&			operator*=(const Matrix4x4& mat)
+									{
+										HPoint TempRow;
+
+										GetRow(0, TempRow);
+										m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
+										m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
+										m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
+										m[0][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
+
+										GetRow(1, TempRow);
+										m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
+										m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
+										m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
+										m[1][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
+
+										GetRow(2, TempRow);
+										m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
+										m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
+										m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
+										m[2][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
+
+										GetRow(3, TempRow);
+										m[3][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
+										m[3][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
+										m[3][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
+										m[3][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
+
+										return	*this;
+									}
+
+		//! Operator for Matrix4x4 *= float;
+		inline_	Matrix4x4&		operator*=(float s)
+								{
+									m[0][0]*=s;	m[0][1]*=s;	m[0][2]*=s;	m[0][3]*=s;
+									m[1][0]*=s;	m[1][1]*=s;	m[1][2]*=s;	m[1][3]*=s;
+									m[2][0]*=s;	m[2][1]*=s;	m[2][2]*=s;	m[2][3]*=s;
+									m[3][0]*=s;	m[3][1]*=s;	m[3][2]*=s;	m[3][3]*=s;
+									return	*this;
+								}
+
+		//! Operator for Matrix4x4 /= float;
+		inline_	Matrix4x4&		operator/=(float s)
+								{
+									if(s)  s = 1.0f / s;
+									m[0][0]*=s;	m[0][1]*=s;	m[0][2]*=s;	m[0][3]*=s;
+									m[1][0]*=s;	m[1][1]*=s;	m[1][2]*=s;	m[1][3]*=s;
+									m[2][0]*=s;	m[2][1]*=s;	m[2][2]*=s;	m[2][3]*=s;
+									m[3][0]*=s;	m[3][1]*=s;	m[3][2]*=s;	m[3][3]*=s;
+									return	*this;
+								}
+
+		inline_	const HPoint&	operator[](int row)		const	{ return *(const HPoint*)&m[row][0];	}
+		inline_	HPoint&			operator[](int row)				{ return *(HPoint*)&m[row][0];			}
+
+		public:
+
+				float			m[4][4];
+	};
+
+	//! Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix
+	inline_ void TransformPoint4x3(Point& dest, const Point& source, const Matrix4x4& rot)
+	{
+		dest.x = rot.m[3][0] + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
+		dest.y = rot.m[3][1] + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
+		dest.z = rot.m[3][2] + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
+	}
+
+	//! Quickly rotates a vector, using the 3x3 part of a 4x4 matrix
+	inline_ void TransformPoint3x3(Point& dest, const Point& source, const Matrix4x4& rot)
+	{
+		dest.x = source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
+		dest.y = source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
+		dest.z = source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
+	}
+
+	ICEMATHS_API void InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src);
+
+#endif // __ICEMATRIX4X4_H__
+

src/external/open_dynamics_engine-ef/ode/IceMemoryMacros.h

@@ -0,0 +1,109 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains all memory macros.
+ *	\file		IceMemoryMacros.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEMEMORYMACROS_H__
+#define __ICEMEMORYMACROS_H__
+
+#undef ZeroMemory
+#undef CopyMemory
+#undef MoveMemory
+#undef FillMemory
+
+	//!	Clears a buffer.
+	//!	\param		addr	[in] buffer address
+	//!	\param		size	[in] buffer length
+	//!	\see		FillMemory
+	//!	\see		StoreDwords
+	//!	\see		CopyMemory
+	//!	\see		MoveMemory
+	inline_ void ZeroMemory(void* addr, udword size)					{ memset(addr, 0, size);		}
+
+	//!	Fills a buffer with a given byte.
+	//!	\param		dest	[in] buffer address
+	//!	\param		size	[in] buffer length
+	//!	\param		val		[in] the byte value
+	//!	\see		StoreDwords
+	//!	\see		ZeroMemory
+	//!	\see		CopyMemory
+	//!	\see		MoveMemory
+	inline_ void FillMemory(void* dest, udword size, ubyte val)			{ memset(dest, val, size);		}
+
+	//!	Fills a buffer with a given dword.
+	//!	\param		dest	[in] buffer address
+	//!	\param		nb		[in] number of dwords to write
+	//!	\param		value	[in] the dword value
+	//!	\see		FillMemory
+	//!	\see		ZeroMemory
+	//!	\see		CopyMemory
+	//!	\see		MoveMemory
+	//!	\warning	writes nb*4 bytes !
+	inline_ void StoreDwords(udword* dest, udword nb, udword value)
+	{
+		// The asm code below **SHOULD** be equivalent to one of those C versions
+		// or the other if your compiled is good: (checked on VC++ 6.0)
+		//
+		//	1) while(nb--)	*dest++ = value;
+		//
+		//	2) for(udword i=0;i<nb;i++)	dest[i] = value;
+		//
+#if defined(_MSC_VER) && 0  // ericf disabling
+		_asm push eax
+		_asm push ecx
+		_asm push edi
+		_asm mov edi, dest
+		_asm mov ecx, nb
+		_asm mov eax, value
+		_asm rep stosd
+		_asm pop edi
+		_asm pop ecx
+		_asm pop eax
+#else
+		while(nb--) *dest++ = value;
+#endif
+	}
+
+	//!	Copies a buffer.
+	//!	\param		dest	[in] destination buffer address
+	//!	\param		src	[in] source buffer address
+	//!	\param		size	[in] buffer length
+	//!	\see		ZeroMemory
+	//!	\see		FillMemory
+	//!	\see		StoreDwords
+	//!	\see		MoveMemory
+	inline_ void CopyMemory(void* dest, const void* src, udword size)	{ memcpy(dest, src, size);		}
+
+	//!	Moves a buffer.
+	//!	\param		dest	[in] destination buffer address
+	//!	\param		src	[in] source buffer address
+	//!	\param		size	[in] buffer length
+	//!	\see		ZeroMemory
+	//!	\see		FillMemory
+	//!	\see		StoreDwords
+	//!	\see		CopyMemory
+	inline_ void MoveMemory(void* dest, const void* src, udword size)	{ memmove(dest, src, size);		}
+
+	#define SIZEOFOBJECT		sizeof(*this)									//!< Gives the size of current object. Avoid some mistakes (e.g. "sizeof(this)").
+	//#define CLEAROBJECT		{ memset(this, 0, SIZEOFOBJECT);	}			//!< Clears current object. Laziness is my business. HANDLE WITH CARE.
+	#define DELETESINGLE(x)		if (x) { delete x;				x = null; }		//!< Deletes an instance of a class.
+	#define DELETEARRAY(x)		if (x) { delete []x;			x = null; }		//!< Deletes an array.
+	#define SAFE_RELEASE(x)		if (x) { (x)->Release();		(x) = null; }	//!< Safe D3D-style release
+	#define SAFE_DESTRUCT(x)	if (x) { (x)->SelfDestruct();	(x) = null; }	//!< Safe ICE-style release
+
+#ifdef __ICEERROR_H__
+	#define CHECKALLOC(x)		if(!x) return SetIceError("Out of memory.", EC_OUT_OF_MEMORY);	//!< Standard alloc checking. HANDLE WITH CARE.
+#else
+	#define CHECKALLOC(x)		if(!x) return false;
+#endif
+
+	//! Standard allocation cycle
+	#define SAFE_ALLOC(ptr, type, count)	DELETEARRAY(ptr);	ptr = new type[count];	CHECKALLOC(ptr);
+
+#endif // __ICEMEMORYMACROS_H__

src/external/open_dynamics_engine-ef/ode/IceOBB.cpp

@@ -0,0 +1,333 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains OBB-related code.
+ *	\file		IceOBB.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	An Oriented Bounding Box (OBB).
+ *	\class		OBB
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Tests if a point is contained within the OBB.
+ *	\param		p	[in] the world point to test
+ *	\return		true if inside the OBB
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool OBB::ContainsPoint(const Point& p) const
+{
+	// Point in OBB test using lazy evaluation and early exits
+
+	// Translate to box space
+	Point RelPoint = p - mCenter;
+
+	// Point * mRot maps from box space to world space
+	// mRot * Point maps from world space to box space (what we need here)
+
+	float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z;
+	if(f >= mExtents.x || f <= -mExtents.x) return false;
+
+	f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z;
+	if(f >= mExtents.y || f <= -mExtents.y) return false;
+
+	f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z;
+	if(f >= mExtents.z || f <= -mExtents.z) return false;
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds an OBB from an AABB and a world transform.
+ *	\param		aabb	[in] the aabb
+ *	\param		mat		[in] the world transform
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBB::Create(const AABB& aabb, const Matrix4x4& mat)
+{
+	// Note: must be coherent with Rotate()
+
+	aabb.GetCenter(mCenter);
+	aabb.GetExtents(mExtents);
+	// Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity).
+
+	// So following what's done in Rotate:
+	// - x-form the center
+	mCenter *= mat;
+	// - combine rotation with identity, i.e. just use given matrix
+	mRot = mat;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the obb planes.
+ *	\param		planes	[out] 6 box planes
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool OBB::ComputePlanes(Plane* planes)	const
+{
+	// Checkings
+	if(!planes)	return false;
+
+	Point Axis0 = mRot[0];
+	Point Axis1 = mRot[1];
+	Point Axis2 = mRot[2];
+
+	// Writes normals
+	planes[0].n = Axis0;
+	planes[1].n = -Axis0;
+	planes[2].n = Axis1;
+	planes[3].n = -Axis1;
+	planes[4].n = Axis2;
+	planes[5].n = -Axis2;
+
+	// Compute a point on each plane
+	Point p0 = mCenter + Axis0 * mExtents.x;
+	Point p1 = mCenter - Axis0 * mExtents.x;
+	Point p2 = mCenter + Axis1 * mExtents.y;
+	Point p3 = mCenter - Axis1 * mExtents.y;
+	Point p4 = mCenter + Axis2 * mExtents.z;
+	Point p5 = mCenter - Axis2 * mExtents.z;
+
+	// Compute d
+	planes[0].d = -(planes[0].n|p0);
+	planes[1].d = -(planes[1].n|p1);
+	planes[2].d = -(planes[2].n|p2);
+	planes[3].d = -(planes[3].n|p3);
+	planes[4].d = -(planes[4].n|p4);
+	planes[5].d = -(planes[5].n|p5);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the obb points.
+ *	\param		pts	[out] 8 box points
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool OBB::ComputePoints(Point* pts)	const
+{
+	// Checkings
+	if(!pts)	return false;
+
+	Point Axis0 = mRot[0];
+	Point Axis1 = mRot[1];
+	Point Axis2 = mRot[2];
+
+	Axis0 *= mExtents.x;
+	Axis1 *= mExtents.y;
+	Axis2 *= mExtents.z;
+
+	//     7+------+6			0 = ---
+	//     /|     /|			1 = +--
+	//    / |    / |			2 = ++-
+	//   / 4+---/--+5			3 = -+-
+	// 3+------+2 /    y   z	4 = --+
+	//  | /    | /     |  /		5 = +-+
+	//  |/     |/      |/		6 = +++
+	// 0+------+1      *---x	7 = -++
+
+	pts[0] = mCenter - Axis0 - Axis1 - Axis2;
+	pts[1] = mCenter + Axis0 - Axis1 - Axis2;
+	pts[2] = mCenter + Axis0 + Axis1 - Axis2;
+	pts[3] = mCenter - Axis0 + Axis1 - Axis2;
+	pts[4] = mCenter - Axis0 - Axis1 + Axis2;
+	pts[5] = mCenter + Axis0 - Axis1 + Axis2;
+	pts[6] = mCenter + Axis0 + Axis1 + Axis2;
+	pts[7] = mCenter - Axis0 + Axis1 + Axis2;
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes vertex normals.
+ *	\param		pts	[out] 8 box points
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool OBB::ComputeVertexNormals(Point* pts)	const
+{
+	static float VertexNormals[] =
+	{
+		-INVSQRT3,	-INVSQRT3,	-INVSQRT3,
+		INVSQRT3,	-INVSQRT3,	-INVSQRT3,
+		INVSQRT3,	INVSQRT3,	-INVSQRT3,
+		-INVSQRT3,	INVSQRT3,	-INVSQRT3,
+		-INVSQRT3,	-INVSQRT3,	INVSQRT3,
+		INVSQRT3,	-INVSQRT3,	INVSQRT3,
+		INVSQRT3,	INVSQRT3,	INVSQRT3,
+		-INVSQRT3,	INVSQRT3,	INVSQRT3
+	};
+
+	if(!pts)	return false;
+
+	const Point* VN = (const Point*)VertexNormals;
+	for(udword i=0;i<8;i++)
+	{
+		pts[i] = VN[i] * mRot;
+	}
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Returns edges.
+ *	\return		24 indices (12 edges) indexing the list returned by ComputePoints()
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const udword* OBB::GetEdges() const
+{
+	static udword Indices[] = {
+	0, 1,	1, 2,	2, 3,	3, 0,
+	7, 6,	6, 5,	5, 4,	4, 7,
+	1, 5,	6, 2,
+	3, 7,	4, 0
+	};
+	return Indices;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Returns local edge normals.
+ *	\return		edge normals in local space
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const Point* OBB::GetLocalEdgeNormals() const
+{
+	static float EdgeNormals[] =
+	{
+		0,			-INVSQRT2,	-INVSQRT2,	// 0-1
+		INVSQRT2,	0,			-INVSQRT2,	// 1-2
+		0,			INVSQRT2,	-INVSQRT2,	// 2-3
+		-INVSQRT2,	0,			-INVSQRT2,	// 3-0
+
+		0,			INVSQRT2,	INVSQRT2,	// 7-6
+		INVSQRT2,	0,			INVSQRT2,	// 6-5
+		0,			-INVSQRT2,	INVSQRT2,	// 5-4
+		-INVSQRT2,	0,			INVSQRT2,	// 4-7
+
+		INVSQRT2,	-INVSQRT2,	0,			// 1-5
+		INVSQRT2,	INVSQRT2,	0,			// 6-2
+		-INVSQRT2,	INVSQRT2,	0,			// 3-7
+		-INVSQRT2,	-INVSQRT2,	0			// 4-0
+	};
+	return (const Point*)EdgeNormals;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Returns world edge normal
+ *	\param		edge_index		[in] 0 <= edge index < 12
+ *	\param		world_normal	[out] edge normal in world space
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const
+{
+	ASSERT(edge_index<12);
+	world_normal = GetLocalEdgeNormals()[edge_index] * mRot;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes an LSS surrounding the OBB.
+ *	\param		lss		[out] the LSS
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBB::ComputeLSS(LSS& lss) const
+{
+	Point Axis0 = mRot[0];
+	Point Axis1 = mRot[1];
+	Point Axis2 = mRot[2];
+
+	switch(mExtents.LargestAxis())
+	{
+		case 0:
+			lss.mRadius = (mExtents.y + mExtents.z)*0.5f;
+			lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius);
+			lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius);
+			break;
+		case 1:
+			lss.mRadius = (mExtents.x + mExtents.z)*0.5f;
+			lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius);
+			lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius);
+			break;
+		case 2:
+			lss.mRadius = (mExtents.x + mExtents.y)*0.5f;
+			lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius);
+			lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius);
+			break;
+	default:
+		break;
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the OBB is inside another OBB.
+ *	\param		box		[in] the other OBB
+ *	\return		TRUE if we're inside the other box
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL OBB::IsInside(const OBB& box) const
+{
+	// Make a 4x4 from the box & inverse it
+	Matrix4x4 M0Inv;
+	{
+		Matrix4x4 M0 = box.mRot;
+		M0.SetTrans(box.mCenter);
+		InvertPRMatrix(M0Inv, M0);
+	}
+
+	// With our inversed 4x4, create box1 in space of box0
+	OBB _1in0;
+	Rotate(M0Inv, _1in0);
+
+	// This should cancel out box0's rotation, i.e. it's now an AABB.
+	// => Center(0,0,0), Rot(identity)
+
+	// The two boxes are in the same space so now we can compare them.
+
+	// Create the AABB of (box1 in space of box0)
+	const Matrix3x3& mtx = _1in0.mRot;
+
+	float f = fabsf(mtx.m[0][0] * mExtents.x) + fabsf(mtx.m[1][0] * mExtents.y) + fabsf(mtx.m[2][0] * mExtents.z) - box.mExtents.x;
+	if(f > _1in0.mCenter.x)		return FALSE;
+	if(-f < _1in0.mCenter.x)	return FALSE;
+
+	f = fabsf(mtx.m[0][1] * mExtents.x) + fabsf(mtx.m[1][1] * mExtents.y) + fabsf(mtx.m[2][1] * mExtents.z) - box.mExtents.y;
+	if(f > _1in0.mCenter.y)		return FALSE;
+	if(-f < _1in0.mCenter.y)	return FALSE;
+
+	f = fabsf(mtx.m[0][2] * mExtents.x) + fabsf(mtx.m[1][2] * mExtents.y) + fabsf(mtx.m[2][2] * mExtents.z) - box.mExtents.z;
+	if(f > _1in0.mCenter.z)		return FALSE;
+	if(-f < _1in0.mCenter.z)	return FALSE;
+
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/IceOBB.h

@@ -0,0 +1,177 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains OBB-related code. (oriented bounding box)
+ *	\file		IceOBB.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 13, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEOBB_H__
+#define __ICEOBB_H__
+
+	// Forward declarations
+	class LSS;
+
+	class ICEMATHS_API OBB
+	{
+		public:
+		//! Constructor
+		inline_						OBB()		{}
+		//! Constructor
+		inline_						OBB(const Point& center, const Point& extents, const Matrix3x3& rot) : mCenter(center), mExtents(extents), mRot(rot)	{}
+		//! Destructor
+		inline_						~OBB()		{}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an empty OBB.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				void				SetEmpty()
+									{
+										mCenter.Zero();
+										mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
+										mRot.Identity();
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Tests if a point is contained within the OBB.
+		 *	\param		p	[in] the world point to test
+		 *	\return		true if inside the OBB
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				bool				ContainsPoint(const Point& p)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Builds an OBB from an AABB and a world transform.
+		 *	\param		aabb	[in] the aabb
+		 *	\param		mat		[in] the world transform
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				void				Create(const AABB& aabb, const Matrix4x4& mat);
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Recomputes the OBB after an arbitrary transform by a 4x4 matrix.
+		 *	\param		mtx		[in] the transform matrix
+		 *	\param		obb		[out] the transformed OBB
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	void				Rotate(const Matrix4x4& mtx, OBB& obb)	const
+									{
+										// The extents remain constant
+										obb.mExtents = mExtents;
+										// The center gets x-formed
+										obb.mCenter = mCenter * mtx;
+										// Combine rotations
+										obb.mRot = mRot * Matrix3x3(mtx);
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the OBB is valid.
+		 *	\return		true if the box is valid
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	BOOL				IsValid()	const
+									{
+										// Consistency condition for (Center, Extents) boxes: Extents >= 0.0f
+										if(mExtents.x < 0.0f)	return FALSE;
+										if(mExtents.y < 0.0f)	return FALSE;
+										if(mExtents.z < 0.0f)	return FALSE;
+										return TRUE;
+									}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the obb planes.
+		 *	\param		planes	[out] 6 box planes
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				bool				ComputePlanes(Plane* planes)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the obb points.
+		 *	\param		pts	[out] 8 box points
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				bool				ComputePoints(Point* pts)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes vertex normals.
+		 *	\param		pts	[out] 8 box points
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				bool				ComputeVertexNormals(Point* pts)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Returns edges.
+		 *	\return		24 indices (12 edges) indexing the list returned by ComputePoints()
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				const udword*		GetEdges()	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Returns local edge normals.
+		 *	\return		edge normals in local space
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				const Point*		GetLocalEdgeNormals()	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Returns world edge normal
+		 *	\param		edge_index		[in] 0 <= edge index < 12
+		 *	\param		world_normal	[out] edge normal in world space
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				void				ComputeWorldEdgeNormal(udword edge_index, Point& world_normal)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes an LSS surrounding the OBB.
+		 *	\param		lss		[out] the LSS
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				void				ComputeLSS(LSS& lss)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the OBB is inside another OBB.
+		 *	\param		box		[in] the other OBB
+		 *	\return		TRUE if we're inside the other box
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+				BOOL				IsInside(const OBB& box)	const;
+
+		inline_	const Point&		GetCenter()		const	{ return mCenter;	}
+		inline_	const Point&		GetExtents()	const	{ return mExtents;	}
+		inline_	const Matrix3x3&	GetRot()		const	{ return mRot;		}
+
+		inline_	void				GetRotatedExtents(Matrix3x3& extents)	const
+									{
+										extents = mRot;
+										extents.Scale(mExtents);
+									}
+
+				Point				mCenter;		//!< B for Box
+				Point				mExtents;		//!< B for Bounding
+				Matrix3x3			mRot;			//!< O for Oriented
+
+				// Orientation is stored in row-major format,
+				// i.e. rows = eigen vectors of the covariance matrix
+	};
+
+#endif	// __ICEOBB_H__

src/external/open_dynamics_engine-ef/ode/IcePairs.h

@@ -0,0 +1,45 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a simple pair class.
+ *	\file		IcePairs.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 13, 2003
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEPAIRS_H__
+#define __ICEPAIRS_H__
+
+	//! A generic couple structure
+	struct ICECORE_API Pair
+	{
+		inline_	Pair()	{}
+		inline_	Pair(udword i0, udword i1) : id0(i0), id1(i1)	{}
+
+		udword	id0;	//!< First index of the pair
+		udword	id1;	//!< Second index of the pair
+	};
+
+	class ICECORE_API Pairs : private Container
+	{
+		public:
+		// Constructor / Destructor
+								Pairs()							{}
+								~Pairs()						{}
+
+		inline_	udword			GetNbPairs()		const		{ return GetNbEntries()>>1;					}
+		inline_	const Pair*		GetPairs()			const		{ return (const Pair*)GetEntries();			}
+		inline_	const Pair*		GetPair(udword i)	const		{ return (const Pair*)&GetEntries()[i+i];	}
+
+		inline_	BOOL			HasPairs()			const		{ return IsNotEmpty();						}
+
+		inline_	void			ResetPairs()					{ Reset();									}
+		inline_	void			DeleteLastPair()				{ DeleteLastEntry();	DeleteLastEntry();	}
+
+		inline_	void			AddPair(const Pair& p)			{ Add(p.id0).Add(p.id1);					}
+		inline_	void			AddPair(udword id0, udword id1)	{ Add(id0).Add(id1);						}
+	};
+
+#endif // __ICEPAIRS_H__

src/external/open_dynamics_engine-ef/ode/IcePlane.cpp

@@ -0,0 +1,53 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for planes.
+ *	\file		IcePlane.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Plane class.
+ *	\class		Plane
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the plane equation from 3 points.
+ *	\param		p0	[in] first point
+ *	\param		p1	[in] second point
+ *	\param		p2	[in] third point
+ *	\return		Self-reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Plane& Plane::Set(const Point& p0, const Point& p1, const Point& p2)
+{
+	Point Edge0 = p1 - p0;
+	Point Edge1 = p2 - p0;
+
+	n = Edge0 ^ Edge1;
+	n.Normalize();
+
+	d = -(p0 | n);
+
+	return	*this;
+}

src/external/open_dynamics_engine-ef/ode/IcePlane.h

@@ -0,0 +1,113 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for planes.
+ *	\file		IcePlane.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEPLANE_H__
+#define __ICEPLANE_H__
+
+	#define PLANE_EPSILON		(1.0e-7f)
+
+	class ICEMATHS_API Plane
+	{
+		public:
+		//! Constructor
+		inline_			Plane()															{												}
+		//! Constructor from a normal and a distance
+		inline_			Plane(float nx, float ny, float nz, float d)					{ Set(nx, ny, nz, d);							}
+		//! Constructor from a point on the plane and a normal
+		inline_			Plane(const Point& p, const Point& n)							{ Set(p, n);									}
+		//! Constructor from three points
+		inline_			Plane(const Point& p0, const Point& p1, const Point& p2)		{ Set(p0, p1, p2);								}
+		//! Constructor from a normal and a distance
+		inline_			Plane(const Point& _n, float _d)								{ n = _n; d = _d;								}
+		//! Copy constructor
+		inline_			Plane(const Plane& plane) : n(plane.n), d(plane.d)				{												}
+		//! Destructor
+		inline_			~Plane()														{												}
+
+		inline_	Plane&	Zero()															{ n.Zero(); d = 0.0f;			return *this;	}
+		inline_	Plane&	Set(float nx, float ny, float nz, float _d)						{ n.Set(nx, ny, nz); d = _d;	return *this;	}
+		inline_	Plane&	Set(const Point& p, const Point& _n)							{ n = _n; d = - p | _n;			return *this;	}
+				Plane&	Set(const Point& p0, const Point& p1, const Point& p2);
+
+		inline_	float	Distance(const Point& p)			const						{ return (p | n) + d;							}
+		inline_	bool	Belongs(const Point& p)				const						{ return fabsf(Distance(p)) < PLANE_EPSILON;	}
+
+		inline_	void	Normalize()
+						{
+							float Denom = 1.0f / n.Magnitude();
+							n.x	*= Denom;
+							n.y	*= Denom;
+							n.z	*= Denom;
+							d	*= Denom;
+						}
+		public:
+		// Members
+				Point	n;		//!< The normal to the plane
+				float	d;		//!< The distance from the origin
+
+		// Cast operators
+		inline_			operator Point()					const						{ return n;										}
+		inline_			operator HPoint()					const						{ return HPoint(n, d);							}
+
+		// Arithmetic operators
+		inline_	Plane	operator*(const Matrix4x4& m)		const
+						{
+							// Old code from Irion. Kept for reference.
+							Plane Ret(*this);
+							return Ret *= m;
+						}
+
+		inline_	Plane&	operator*=(const Matrix4x4& m)
+						{
+							// Old code from Irion. Kept for reference.
+							Point n2 = HPoint(n, 0.0f) * m;
+							d = -((Point) (HPoint( -d*n, 1.0f ) * m) | n2);
+							n = n2;
+							return *this;
+						}
+	};
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster.
+	 *	\param		transformed	[out] transformed plane
+	 *	\param		plane		[in] source plane
+	 *	\param		transform	[in] transform matrix
+	 *	\warning	the plane normal must be unit-length
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	inline_	void TransformPlane(Plane& transformed, const Plane& plane, const Matrix4x4& transform)
+	{
+		// Rotate the normal using the rotation part of the 4x4 matrix
+		transformed.n = plane.n * Matrix3x3(transform);
+
+		// Compute new d
+		transformed.d = plane.d - (Point(transform.GetTrans())|transformed.n);
+	}
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster.
+	 *	\param		plane		[in/out] source plane (transformed on return)
+	 *	\param		transform	[in] transform matrix
+	 *	\warning	the plane normal must be unit-length
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	inline_	void TransformPlane(Plane& plane, const Matrix4x4& transform)
+	{
+		// Rotate the normal using the rotation part of the 4x4 matrix
+		plane.n *= Matrix3x3(transform);
+
+		// Compute new d
+		plane.d -= Point(transform.GetTrans())|plane.n;
+	}
+
+#endif // __ICEPLANE_H__

src/external/open_dynamics_engine-ef/ode/IcePoint.cpp

@@ -0,0 +1,201 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for 3D vectors.
+ *	\file		IcePoint.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	3D point.
+ *
+ *	The name is "Point" instead of "Vector" since a vector is N-dimensional, whereas a point is an implicit "vector of dimension 3".
+ *	So the choice was between "Point" and "Vector3", the first one looked better (IMHO).
+ *
+ *	Some people, then, use a typedef to handle both points & vectors using the same class: typedef Point Vector3;
+ *	This is bad since it opens the door to a lot of confusion while reading the code. I know it may sounds weird but check this out:
+ *
+ *	\code
+ *		Point P0,P1 = some 3D points;
+ *		Point Delta = P1 - P0;
+ *	\endcode
+ *
+ *	This compiles fine, although you should have written:
+ *
+ *	\code
+ *		Point P0,P1 = some 3D points;
+ *		Vector3 Delta = P1 - P0;
+ *	\endcode
+ *
+ *	Subtle things like this are not caught at compile-time, and when you find one in the code, you never know whether it's a mistake
+ *	from the author or something you don't get.
+ *
+ *	One way to handle it at compile-time would be to use different classes for Point & Vector3, only overloading operator "-" for vectors.
+ *	But then, you get a lot of redundant code in thoses classes, and basically it's really a lot of useless work.
+ *
+ *	Another way would be to use homogeneous points: w=1 for points, w=0 for vectors. That's why the HPoint class exists. Now, to store
+ *	your model's vertices and in most cases, you really want to use Points to save ram.
+ *
+ *	\class		Point
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Creates a positive unit random vector.
+ *	\return		Self-reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Point& Point::PositiveUnitRandomVector()
+{
+	x = UnitRandomFloat();
+	y = UnitRandomFloat();
+	z = UnitRandomFloat();
+	Normalize();
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Creates a unit random vector.
+ *	\return		Self-reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Point& Point::UnitRandomVector()
+{
+	x = UnitRandomFloat() - 0.5f;
+	y = UnitRandomFloat() - 0.5f;
+	z = UnitRandomFloat() - 0.5f;
+	Normalize();
+	return *this;
+}
+
+// Cast operator
+// WARNING: not inlined
+Point::operator HPoint() const	{ return HPoint(x, y, z, 0.0f); }
+
+Point& Point::Refract(const Point& eye, const Point& n, float refractindex, Point& refracted)
+{
+	//	Point EyePt = eye position
+	//	Point p = current vertex
+	//	Point n = vertex normal
+	//	Point rv = refracted vector
+	//	Eye vector - doesn't need to be normalized
+	Point Env;
+	Env.x = eye.x - x;
+	Env.y = eye.y - y;
+	Env.z = eye.z - z;
+
+	float NDotE = n|Env;
+	float NDotN = n|n;
+	NDotE /= refractindex;
+
+	// Refracted vector
+	refracted = n*NDotE - Env*NDotN;
+
+	return *this;
+}
+
+Point& Point::ProjectToPlane(const Plane& p)
+{
+	*this-= (p.d + (*this|p.n))*p.n;
+	return *this;
+}
+
+void Point::ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const
+{
+	projected = HPoint(x, y, z, 1.0f) * mat;
+	projected.w = 1.0f / projected.w;
+
+	projected.x*=projected.w;
+	projected.y*=projected.w;
+	projected.z*=projected.w;
+
+	projected.x *= halfrenderwidth;		projected.x += halfrenderwidth;
+	projected.y *= -halfrenderheight;	projected.y += halfrenderheight;
+}
+
+void Point::SetNotUsed()
+{
+	// We use a particular integer pattern : 0xffffffff everywhere. This is a NAN.
+	IR(x) = 0xffffffff;
+	IR(y) = 0xffffffff;
+	IR(z) = 0xffffffff;
+}
+
+BOOL Point::IsNotUsed()	const
+{
+	if(IR(x)!=0xffffffff)	return FALSE;
+	if(IR(y)!=0xffffffff)	return FALSE;
+	if(IR(z)!=0xffffffff)	return FALSE;
+	return TRUE;
+}
+
+Point& Point::Mult(const Matrix3x3& mat, const Point& a)
+{
+	x = a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2];
+	y = a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2];
+	z = a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2];
+	return *this;
+}
+
+Point& Point::Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2)
+{
+	x = a1.x * mat1.m[0][0] + a1.y * mat1.m[0][1] + a1.z * mat1.m[0][2] + a2.x * mat2.m[0][0] + a2.y * mat2.m[0][1] + a2.z * mat2.m[0][2];
+	y = a1.x * mat1.m[1][0] + a1.y * mat1.m[1][1] + a1.z * mat1.m[1][2] + a2.x * mat2.m[1][0] + a2.y * mat2.m[1][1] + a2.z * mat2.m[1][2];
+	z = a1.x * mat1.m[2][0] + a1.y * mat1.m[2][1] + a1.z * mat1.m[2][2] + a2.x * mat2.m[2][0] + a2.y * mat2.m[2][1] + a2.z * mat2.m[2][2];
+	return *this;
+}
+
+Point& Point::Mac(const Matrix3x3& mat, const Point& a)
+{
+	x += a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2];
+	y += a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2];
+	z += a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2];
+	return *this;
+}
+
+Point& Point::TransMult(const Matrix3x3& mat, const Point& a)
+{
+	x = a.x * mat.m[0][0] + a.y * mat.m[1][0] + a.z * mat.m[2][0];
+	y = a.x * mat.m[0][1] + a.y * mat.m[1][1] + a.z * mat.m[2][1];
+	z = a.x * mat.m[0][2] + a.y * mat.m[1][2] + a.z * mat.m[2][2];
+	return *this;
+}
+
+Point& Point::Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos)
+{
+	x = r.x * rotpos.m[0][0] + r.y * rotpos.m[0][1] + r.z * rotpos.m[0][2] + linpos.x;
+	y = r.x * rotpos.m[1][0] + r.y * rotpos.m[1][1] + r.z * rotpos.m[1][2] + linpos.y;
+	z = r.x * rotpos.m[2][0] + r.y * rotpos.m[2][1] + r.z * rotpos.m[2][2] + linpos.z;
+	return *this;
+}
+
+Point& Point::InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos)
+{
+	float sx = r.x - linpos.x;
+	float sy = r.y - linpos.y;
+	float sz = r.z - linpos.z;
+	x = sx * rotpos.m[0][0] + sy * rotpos.m[1][0] + sz * rotpos.m[2][0];
+	y = sx * rotpos.m[0][1] + sy * rotpos.m[1][1] + sz * rotpos.m[2][1];
+	z = sx * rotpos.m[0][2] + sy * rotpos.m[1][2] + sz * rotpos.m[2][2];
+	return *this;
+}

src/external/open_dynamics_engine-ef/ode/IcePoint.h

@@ -0,0 +1,528 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for 3D vectors.
+ *	\file		IcePoint.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEPOINT_H__
+#define __ICEPOINT_H__
+
+	// Forward declarations
+	class HPoint;
+	class Plane;
+	class Matrix3x3;
+	class Matrix4x4;
+
+	#define CROSS2D(a, b)	(a.x*b.y - b.x*a.y)
+
+	const float EPSILON2 = 1.0e-20f;
+
+	class ICEMATHS_API Point
+	{
+		public:
+
+		//! Empty constructor
+		inline_					Point()														{}
+		//! Constructor from a single float
+//		inline_					Point(float val) : x(val), y(val), z(val)					{}
+// Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug.......
+		//! Constructor from floats
+		inline_					Point(float _x, float _y, float _z) : x(_x), y(_y), z(_z)	{}
+		//! Constructor from array
+		inline_					Point(const float f[3]) : x(f[_X]), y(f[_Y]), z(f[_Z])		{}
+		//! Copy constructor
+		inline_					Point(const Point& p) : x(p.x), y(p.y), z(p.z)				{}
+		//! Destructor
+		inline_					~Point()													{}
+
+		//! Clears the vector
+		inline_	Point&			Zero()									{ x =			y =			z = 0.0f;			return *this;	}
+
+		//! + infinity
+		inline_	Point&			SetPlusInfinity()						{ x =			y =			z = MAX_FLOAT;		return *this;	}
+		//! - infinity
+		inline_	Point&			SetMinusInfinity()						{ x =			y =			z = MIN_FLOAT;		return *this;	}
+
+		//! Sets positive unit random vector
+				Point&			PositiveUnitRandomVector();
+		//! Sets unit random vector
+				Point&			UnitRandomVector();
+
+		//! Assignment from values
+		inline_	Point&			Set(float _x, float _y, float _z)		{ x  = _x;		y  = _y;	z  = _z;			return *this;	}
+		//! Assignment from array
+		inline_	Point&			Set(const float f[3])					{ x  = f[_X];	y  = f[_Y];	z  = f[_Z];			return *this;	}
+		//! Assignment from another point
+		inline_	Point&			Set(const Point& src)					{ x  = src.x;	y  = src.y;	z  = src.z;			return *this;	}
+
+		//! Adds a vector
+		inline_	Point&			Add(const Point& p)						{ x += p.x;		y += p.y;	z += p.z;			return *this;	}
+		//! Adds a vector
+		inline_	Point&			Add(float _x, float _y, float _z)		{ x += _x;		y += _y;	z += _z;			return *this;	}
+		//! Adds a vector
+		inline_	Point&			Add(const float f[3])					{ x += f[_X];	y += f[_Y];	z += f[_Z];			return *this;	}
+		//! Adds vectors
+		inline_	Point&			Add(const Point& p, const Point& q)		{ x = p.x+q.x;	y = p.y+q.y;	z = p.z+q.z;	return *this;	}
+
+		//! Subtracts a vector
+		inline_	Point&			Sub(const Point& p)						{ x -= p.x;		y -= p.y;	z -= p.z;			return *this;	}
+		//! Subtracts a vector
+		inline_	Point&			Sub(float _x, float _y, float _z)		{ x -= _x;		y -= _y;	z -= _z;			return *this;	}
+		//! Subtracts a vector
+		inline_	Point&			Sub(const float f[3])					{ x -= f[_X];	y -= f[_Y];	z -= f[_Z];			return *this;	}
+		//! Subtracts vectors
+		inline_	Point&			Sub(const Point& p, const Point& q)		{ x = p.x-q.x;	y = p.y-q.y;	z = p.z-q.z;	return *this;	}
+
+		//! this = -this
+		inline_	Point&			Neg()									{ x = -x;		y = -y;			z = -z;			return *this;	}
+		//! this = -a
+		inline_	Point&			Neg(const Point& a)						{ x = -a.x;		y = -a.y;		z = -a.z;		return *this;	}
+
+		//! Multiplies by a scalar
+		inline_	Point&			Mult(float s)							{ x *= s;		y *= s;		z *= s;				return *this;	}
+
+		//! this = a * scalar
+		inline_	Point&			Mult(const Point& a, float scalar)
+								{
+									x = a.x * scalar;
+									y = a.y * scalar;
+									z = a.z * scalar;
+									return *this;
+								}
+
+		//! this = a + b * scalar
+		inline_	Point&			Mac(const Point& a, const Point& b, float scalar)
+								{
+									x = a.x + b.x * scalar;
+									y = a.y + b.y * scalar;
+									z = a.z + b.z * scalar;
+									return *this;
+								}
+
+		//! this = this + a * scalar
+		inline_	Point&			Mac(const Point& a, float scalar)
+								{
+									x += a.x * scalar;
+									y += a.y * scalar;
+									z += a.z * scalar;
+									return *this;
+								}
+
+		//! this = a - b * scalar
+		inline_	Point&			Msc(const Point& a, const Point& b, float scalar)
+								{
+									x = a.x - b.x * scalar;
+									y = a.y - b.y * scalar;
+									z = a.z - b.z * scalar;
+									return *this;
+								}
+
+		//! this = this - a * scalar
+		inline_	Point&			Msc(const Point& a, float scalar)
+								{
+									x -= a.x * scalar;
+									y -= a.y * scalar;
+									z -= a.z * scalar;
+									return *this;
+								}
+
+		//! this = a + b * scalarb + c * scalarc
+		inline_	Point&			Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
+								{
+									x = a.x + b.x * scalarb + c.x * scalarc;
+									y = a.y + b.y * scalarb + c.y * scalarc;
+									z = a.z + b.z * scalarb + c.z * scalarc;
+									return *this;
+								}
+
+		//! this = a - b * scalarb - c * scalarc
+		inline_	Point&			Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
+								{
+									x = a.x - b.x * scalarb - c.x * scalarc;
+									y = a.y - b.y * scalarb - c.y * scalarc;
+									z = a.z - b.z * scalarb - c.z * scalarc;
+									return *this;
+								}
+
+		//! this = mat * a
+		inline_	Point&			Mult(const Matrix3x3& mat, const Point& a);
+
+		//! this = mat1 * a1 + mat2 * a2
+		inline_	Point&			Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2);
+
+		//! this = this + mat * a
+		inline_	Point&			Mac(const Matrix3x3& mat, const Point& a);
+
+		//! this = transpose(mat) * a
+		inline_	Point&			TransMult(const Matrix3x3& mat, const Point& a);
+
+		//! Linear interpolate between two vectors: this = a + t * (b - a)
+		inline_	Point&			Lerp(const Point& a, const Point& b, float t)
+								{
+									x = a.x + t * (b.x - a.x);
+									y = a.y + t * (b.y - a.y);
+									z = a.z + t * (b.z - a.z);
+									return *this;
+								}
+
+		//! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2.
+		//! this =	p0 * (2t^2 - t^3 - t)/2
+		//!			+ p1 * (3t^3 - 5t^2 + 2)/2
+		//!			+ p2 * (4t^2 - 3t^3 + t)/2
+		//!			+ p3 * (t^3 - t^2)/2
+		inline_	Point&			Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t)
+								{
+									float t2 = t * t;
+									float t3 = t2 * t;
+									float kp0 = (2.0f * t2 - t3 - t) * 0.5f;
+									float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f;
+									float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f;
+									float kp3 = (t3 - t2) * 0.5f;
+									x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3;
+									y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3;
+									z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3;
+									return *this;
+								}
+
+		//! this = rotpos * r + linpos
+		inline_	Point&			Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
+
+		//! this = trans(rotpos) * (r - linpos)
+		inline_	Point&			InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
+
+		//! Returns MIN(x, y, z);
+		inline_	float			Min()				const		{ return MIN(x, MIN(y, z));												}
+		//! Returns MAX(x, y, z);
+		inline_	float			Max()				const		{ return MAX(x, MAX(y, z));												}
+		//! Sets each element to be componentwise minimum
+		inline_	Point&			Min(const Point& p)				{ x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z);	return *this;	}
+		//! Sets each element to be componentwise maximum
+		inline_	Point&			Max(const Point& p)				{ x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z);	return *this;	}
+
+		//! Clamps each element
+		inline_	Point&			Clamp(float min, float max)
+								{
+									if(x<min)	x=min;	if(x>max)	x=max;
+									if(y<min)	y=min;	if(y>max)	y=max;
+									if(z<min)	z=min;	if(z>max)	z=max;
+									return *this;
+								}
+
+		//! Computes square magnitude
+		inline_	float			SquareMagnitude()	const		{ return x*x + y*y + z*z;												}
+		//! Computes magnitude
+		inline_	float			Magnitude()			const		{ return sqrtf(x*x + y*y + z*z);										}
+		//! Computes volume
+		inline_	float			Volume()			const		{ return x * y * z;														}
+
+		//! Checks the point is near zero
+		inline_	bool			ApproxZero()		const		{ return SquareMagnitude() < EPSILON2;									}
+
+		//! Tests for exact zero vector
+		inline_	BOOL			IsZero()			const
+								{
+									if(IR(x) || IR(y) || IR(z))	return FALSE;
+									return TRUE;
+								}
+
+		//! Checks point validity
+		inline_	BOOL			IsValid()			const
+								{
+									if(!IsValidFloat(x))	return FALSE;
+									if(!IsValidFloat(y))	return FALSE;
+									if(!IsValidFloat(z))	return FALSE;
+									return TRUE;
+								}
+
+		//! Slighty moves the point
+				void			Tweak(udword coord_mask, udword tweak_mask)
+								{
+									if(coord_mask&1)	{ udword Dummy = IR(x);	Dummy^=tweak_mask;	x = FR(Dummy); }
+									if(coord_mask&2)	{ udword Dummy = IR(y);	Dummy^=tweak_mask;	y = FR(Dummy); }
+									if(coord_mask&4)	{ udword Dummy = IR(z);	Dummy^=tweak_mask;	z = FR(Dummy); }
+								}
+
+		#define TWEAKMASK		0x3fffff
+		#define TWEAKNOTMASK	~TWEAKMASK
+		//! Slighty moves the point out
+		inline_	void			TweakBigger()
+								{
+									udword	Dummy = (IR(x)&TWEAKNOTMASK);	if(!IS_NEGATIVE_FLOAT(x))	Dummy+=TWEAKMASK+1;	x = FR(Dummy);
+											Dummy = (IR(y)&TWEAKNOTMASK);	if(!IS_NEGATIVE_FLOAT(y))	Dummy+=TWEAKMASK+1;	y = FR(Dummy);
+											Dummy = (IR(z)&TWEAKNOTMASK);	if(!IS_NEGATIVE_FLOAT(z))	Dummy+=TWEAKMASK+1;	z = FR(Dummy);
+								}
+
+		//! Slighty moves the point in
+		inline_	void			TweakSmaller()
+								{
+									udword	Dummy = (IR(x)&TWEAKNOTMASK);	if(IS_NEGATIVE_FLOAT(x))	Dummy+=TWEAKMASK+1;	x = FR(Dummy);
+											Dummy = (IR(y)&TWEAKNOTMASK);	if(IS_NEGATIVE_FLOAT(y))	Dummy+=TWEAKMASK+1;	y = FR(Dummy);
+											Dummy = (IR(z)&TWEAKNOTMASK);	if(IS_NEGATIVE_FLOAT(z))	Dummy+=TWEAKMASK+1;	z = FR(Dummy);
+								}
+
+		//! Normalizes the vector
+		inline_	Point&			Normalize()
+								{
+									float M = x*x + y*y + z*z;
+									if(M)
+									{
+										M = 1.0f / sqrtf(M);
+										x *= M;
+										y *= M;
+										z *= M;
+									}
+									return *this;
+								}
+
+		//! Sets vector length
+		inline_	Point&			SetLength(float length)
+								{
+									float NewLength = length / Magnitude();
+									x *= NewLength;
+									y *= NewLength;
+									z *= NewLength;
+									return *this;
+								}
+
+		//! Clamps vector length
+		inline_	Point&			ClampLength(float limit_length)
+								{
+									if(limit_length>=0.0f)	// Magnitude must be positive
+									{
+										float CurrentSquareLength = SquareMagnitude();
+
+										if(CurrentSquareLength > limit_length * limit_length)
+										{
+											float Coeff = limit_length / sqrtf(CurrentSquareLength);
+											x *= Coeff;
+											y *= Coeff;
+											z *= Coeff;
+										}
+									}
+									return *this;
+								}
+
+		//! Computes distance to another point
+		inline_	float			Distance(const Point& b)			const
+								{
+									return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
+								}
+
+		//! Computes square distance to another point
+		inline_	float			SquareDistance(const Point& b)		const
+								{
+									return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
+								}
+
+		//! Dot product dp = this|a
+		inline_	float			Dot(const Point& p)					const		{	return p.x * x + p.y * y + p.z * z;				}
+
+		//! Cross product this = a x b
+		inline_	Point&			Cross(const Point& a, const Point& b)
+								{
+									x = a.y * b.z - a.z * b.y;
+									y = a.z * b.x - a.x * b.z;
+									z = a.x * b.y - a.y * b.x;
+									return *this;
+								}
+
+		//! Vector code ( bitmask = sign(z) | sign(y) | sign(x) )
+		inline_	udword			VectorCode()						const
+								{
+									return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29);
+								}
+
+		//! Returns largest axis
+		inline_	PointComponent	LargestAxis()						const
+								{
+									const float* Vals = &x;
+									PointComponent m = _X;
+									if(Vals[_Y] > Vals[m]) m = _Y;
+									if(Vals[_Z] > Vals[m]) m = _Z;
+									return m;
+								}
+
+		//! Returns closest axis
+		inline_	PointComponent	ClosestAxis()						const
+								{
+									const float* Vals = &x;
+									PointComponent m = _X;
+									if(AIR(Vals[_Y]) > AIR(Vals[m])) m = _Y;
+									if(AIR(Vals[_Z]) > AIR(Vals[m])) m = _Z;
+									return m;
+								}
+
+		//! Returns smallest axis
+		inline_	PointComponent	SmallestAxis()						const
+								{
+									const float* Vals = &x;
+									PointComponent m = _X;
+									if(Vals[_Y] < Vals[m]) m = _Y;
+									if(Vals[_Z] < Vals[m]) m = _Z;
+									return m;
+								}
+
+		//! Refracts the point
+				Point&			Refract(const Point& eye, const Point& n, float refractindex, Point& refracted);
+
+		//! Projects the point onto a plane
+				Point&			ProjectToPlane(const Plane& p);
+
+		//! Projects the point onto the screen
+				void			ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const;
+
+		//! Unfolds the point onto a plane according to edge(a,b)
+				Point&			Unfold(Plane& p, Point& a, Point& b);
+
+		//! Hash function from Ville Miettinen
+		inline_	udword			GetHashValue()						const
+								{
+									const udword* h = (const udword*)(this);
+									udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff;	// avoid problems with +-0
+									return (f>>22)^(f>>12)^(f);
+								}
+
+		//! Stuff magic values in the point, marking it as explicitely not used.
+				void			SetNotUsed();
+		//! Checks the point is marked as not used
+				BOOL			IsNotUsed()							const;
+
+		// Arithmetic operators
+
+		//! Unary operator for Point Negate = - Point
+		inline_	Point			operator-()							const		{ return Point(-x, -y, -z);							}
+
+		//! Operator for Point Plus = Point + Point.
+		inline_	Point			operator+(const Point& p)			const		{ return Point(x + p.x, y + p.y, z + p.z);			}
+		//! Operator for Point Minus = Point - Point.
+		inline_	Point			operator-(const Point& p)			const		{ return Point(x - p.x, y - p.y, z - p.z);			}
+
+		//! Operator for Point Mul   = Point * Point.
+		inline_	Point			operator*(const Point& p)			const		{ return Point(x * p.x, y * p.y, z * p.z);			}
+		//! Operator for Point Scale = Point * float.
+		inline_	Point			operator*(float s)					const		{ return Point(x * s,   y * s,   z * s );			}
+		//! Operator for Point Scale = float * Point.
+		inline_ friend	Point	operator*(float s, const Point& p)				{ return Point(s * p.x, s * p.y, s * p.z);			}
+
+		//! Operator for Point Div   = Point / Point.
+		inline_	Point			operator/(const Point& p)			const		{ return Point(x / p.x, y / p.y, z / p.z);			}
+		//! Operator for Point Scale = Point / float.
+		inline_	Point			operator/(float s)					const		{ s = 1.0f / s; return Point(x * s, y * s, z * s);	}
+		//! Operator for Point Scale = float / Point.
+		inline_	friend	Point	operator/(float s, const Point& p)				{ return Point(s / p.x, s / p.y, s / p.z);			}
+
+		//! Operator for float DotProd = Point | Point.
+		inline_	float			operator|(const Point& p)			const		{ return x*p.x + y*p.y + z*p.z;						}
+		//! Operator for Point VecProd = Point ^ Point.
+		inline_	Point			operator^(const Point& p)			const
+								{
+									return Point(
+									y * p.z - z * p.y,
+									z * p.x - x * p.z,
+									x * p.y - y * p.x );
+								}
+
+		//! Operator for Point += Point.
+		inline_	Point&			operator+=(const Point& p)						{ x += p.x; y += p.y; z += p.z;	return *this;		}
+		//! Operator for Point += float.
+		inline_	Point&			operator+=(float s)								{ x += s;   y += s;   z += s;	return *this;		}
+
+		//! Operator for Point -= Point.
+		inline_	Point&			operator-=(const Point& p)						{ x -= p.x; y -= p.y; z -= p.z;	return *this;		}
+		//! Operator for Point -= float.
+		inline_	Point&			operator-=(float s)								{ x -= s;   y -= s;   z -= s;	return *this;		}
+
+		//! Operator for Point *= Point.
+		inline_	Point&			operator*=(const Point& p)						{ x *= p.x; y *= p.y; z *= p.z;	return *this;		}
+		//! Operator for Point *= float.
+		inline_	Point&			operator*=(float s)								{ x *= s; y *= s; z *= s;		return *this;		}
+
+		//! Operator for Point /= Point.
+		inline_	Point&			operator/=(const Point& p)						{ x /= p.x; y /= p.y; z /= p.z;	return *this;		}
+		//! Operator for Point /= float.
+		inline_	Point&			operator/=(float s)								{ s = 1.0f/s; x *= s; y *= s; z *= s; return *this; }
+
+		// Logical operators
+
+		//! Operator for "if(Point==Point)"
+		inline_	bool			operator==(const Point& p)			const		{ return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z)));	}
+		//! Operator for "if(Point!=Point)"
+		inline_	bool			operator!=(const Point& p)			const		{ return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z)));	}
+
+		// Arithmetic operators
+
+		//! Operator for Point Mul = Point * Matrix3x3.
+		inline_	Point			operator*(const Matrix3x3& mat)		const
+								{
+									class ShadowMatrix3x3{ public: float m[3][3]; };	// To allow inlining
+									const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
+
+									return Point(
+									x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0],
+									x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1],
+									x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] );
+								}
+
+		//! Operator for Point Mul = Point * Matrix4x4.
+		inline_	Point			operator*(const Matrix4x4& mat)		const
+								{
+									class ShadowMatrix4x4{ public: float m[4][4]; };	// To allow inlining
+									const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
+
+									return Point(
+									x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0],
+									x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1],
+									x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]);
+								}
+
+		//! Operator for Point *= Matrix3x3.
+		inline_	Point&			operator*=(const Matrix3x3& mat)
+								{
+									class ShadowMatrix3x3{ public: float m[3][3]; };	// To allow inlining
+									const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
+
+									float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0];
+									float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
+									float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];
+
+									x = xp;	y = yp;	z = zp;
+
+									return *this;
+								}
+
+		//! Operator for Point *= Matrix4x4.
+		inline_	Point&			operator*=(const Matrix4x4& mat)
+								{
+									class ShadowMatrix4x4{ public: float m[4][4]; };	// To allow inlining
+									const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
+
+									float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0];
+									float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1];
+									float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2];
+
+									x = xp;	y = yp;	z = zp;
+
+									return *this;
+								}
+
+		// Cast operators
+
+		//! Cast a Point to a HPoint. w is set to zero.
+								operator	HPoint()				const;
+
+		inline_					operator	const	float*() const	{ return &x; }
+		inline_					operator			float*()		{ return &x; }
+
+		public:
+				float			x, y, z;
+	};
+
+	FUNCTION ICEMATHS_API void Normalize1(Point& a);
+	FUNCTION ICEMATHS_API void Normalize2(Point& a);
+
+#endif //__ICEPOINT_H__

src/external/open_dynamics_engine-ef/ode/IcePreprocessor.h

@@ -0,0 +1,132 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains preprocessor stuff. This should be the first included header.
+ *	\file		IcePreprocessor.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEPREPROCESSOR_H__
+#define __ICEPREPROCESSOR_H__
+
+	// Check platform
+	#if defined( _WIN32 ) || defined( WIN32 )
+		// #pragma message("Compiling on Windows...")
+		#define PLATFORM_WINDOWS
+	#else
+		// don't issue pragmas on unknown platforms
+		// #pragma message("Compiling on unknown platform...")
+	#endif
+
+	// Check compiler
+	#if defined(_MSC_VER)
+		// #pragma message("Compiling with VC++...")
+		#define COMPILER_VISUAL_CPP
+	#else
+		// don't issue pragmas on unknown platforms
+		// #pragma message("Compiling with unknown compiler...")
+	#endif
+
+	// Check compiler options. If this file is included in user-apps, this
+	// shouldn't be needed, so that they can use what they like best.
+	#ifndef ICE_DONT_CHECK_COMPILER_OPTIONS
+		#ifdef COMPILER_VISUAL_CPP
+			#if defined(_CHAR_UNSIGNED)
+			#endif
+
+			#if defined(_CPPRTTI)
+				#error Please disable RTTI...
+			#endif
+
+			#if defined(_CPPUNWIND)
+				#error Please disable exceptions...
+			#endif
+
+			#if defined(_MT)
+				// Multithreading
+			#endif
+		#endif
+	#endif
+
+	// Check debug mode
+	#ifdef	DEBUG			// May be defined instead of _DEBUG. Let's fix it.
+		#ifndef	_DEBUG
+			#define _DEBUG
+		#endif
+	#endif
+
+	#ifdef  _DEBUG
+	// Here you may define items for debug builds
+	#endif
+
+	#ifndef THIS_FILE
+		#define THIS_FILE			__FILE__
+	#endif
+
+	#ifndef ICE_NO_DLL
+		#ifdef ICECORE_EXPORTS
+			#define ICECORE_API			__declspec(dllexport)
+		#else
+			#define ICECORE_API			__declspec(dllimport)
+		#endif
+	#else
+			#define ICECORE_API
+	#endif
+
+	// Don't override new/delete
+//	#define DEFAULT_NEWDELETE
+	#define DONT_TRACK_MEMORY_LEAKS
+
+	#define FUNCTION				extern "C"
+
+	// Cosmetic stuff [mainly useful with multiple inheritance]
+	#define	override(base_class)	virtual
+
+	// Our own inline keyword, so that:
+	// - we can switch to __forceinline to check it's really better or not
+	// - we can remove __forceinline if the compiler doesn't support it
+//	#define inline_				__forceinline
+//	#define inline_				inline
+
+	// Contributed by Bruce Mitchener
+		#if defined(COMPILER_VISUAL_CPP)
+			#define inline_			__forceinline
+//			#define inline_			inline
+		#elif defined(__GNUC__) && __GNUC__ < 3
+			#define inline_ inline
+		#elif defined(__GNUC__)
+			#define inline_ inline __attribute__ ((always_inline))
+		#else
+			#define inline_ inline
+		#endif
+
+	// Down the hatch
+#ifdef _MSC_VER
+	#pragma inline_depth( 255 )
+#endif
+
+	#ifdef COMPILER_VISUAL_CPP
+		#pragma intrinsic(memcmp)
+		#pragma intrinsic(memcpy)
+		#pragma intrinsic(memset)
+		#pragma intrinsic(strcat)
+		#pragma intrinsic(strcmp)
+		#pragma intrinsic(strcpy)
+		#pragma intrinsic(strlen)
+		#pragma intrinsic(abs)
+		#pragma intrinsic(labs)
+	#endif
+
+	// ANSI compliance
+	#ifdef  _DEBUG
+		// Remove painful warning in debug
+		inline_ bool __False__(){ return false; }
+		#define for if(__False__()){}	else for
+	#else
+		#define for if(0){}	else for
+	#endif
+
+#endif // __ICEPREPROCESSOR_H__

src/external/open_dynamics_engine-ef/ode/IceRandom.cpp

@@ -0,0 +1,42 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for random generators.
+ *	\file		IceRandom.cpp
+ *	\author		Pierre Terdiman
+ *	\date		August, 9, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceCore;
+
+void IceCore::	SRand(udword seed)
+{
+	srand(seed);
+}
+
+udword IceCore::Rand()
+{
+	return rand();
+}
+
+
+static BasicRandom gRandomGenerator(42);
+
+udword IceCore::GetRandomIndex(udword max_index)
+{
+	// We don't use rand() since it's limited to RAND_MAX
+	udword Index = gRandomGenerator.Randomize();
+	return Index % max_index;
+}

src/external/open_dynamics_engine-ef/ode/IceRandom.h

@@ -0,0 +1,42 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for random generators.
+ *	\file		IceRandom.h
+ *	\author		Pierre Terdiman
+ *	\date		August, 9, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICERANDOM_H__
+#define __ICERANDOM_H__
+
+	FUNCTION ICECORE_API	void	SRand(udword seed);
+	FUNCTION ICECORE_API	udword	Rand();
+
+	//! Returns a unit random floating-point value
+	inline_ float UnitRandomFloat()	{ return float(Rand()) * ONE_OVER_RAND_MAX;	}
+
+	//! Returns a random index so that 0<= index < max_index
+	ICECORE_API	udword GetRandomIndex(udword max_index);
+
+	class ICECORE_API BasicRandom
+	{
+		public:
+
+		//! Constructor
+		inline_				BasicRandom(udword seed=0)	: mRnd(seed)	{}
+		//! Destructor
+		inline_				~BasicRandom()								{}
+
+		inline_	void		SetSeed(udword seed)		{ mRnd = seed;											}
+		inline_	udword		GetCurrentValue()	const	{ return mRnd;											}
+		inline_	udword		Randomize()					{ mRnd = mRnd * 2147001325 + 715136305; return mRnd;	}
+
+		private:
+				udword		mRnd;
+	};
+
+#endif // __ICERANDOM_H__
+

src/external/open_dynamics_engine-ef/ode/IceRay.cpp

@@ -0,0 +1,92 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for rays.
+ *	\file		IceRay.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Ray class.
+ *	A ray is a half-line P(t) = mOrig + mDir * t, with 0 <= t <= +infinity
+ *	\class		Ray
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/*
+	O = Origin = impact point
+	i = normalized vector along the x axis
+	j = normalized vector along the y axis = actually the normal vector in O
+	D = Direction vector, norm |D| = 1
+	N = Projection of D on y axis, norm |N| = normal reaction
+	T = Projection of D on x axis, norm |T| = tangential reaction
+	R = Reflexion vector
+
+              ^y
+              |
+              |
+              |
+       _  _  _| _ _ _
+       *      *      *|
+        \     |     /
+         \    |N   /  |
+         R\   |   /D
+           \  |  /    |
+            \ | /
+    _________\|/______*_______>x
+               O    T
+
+	Let define theta = angle between D and N. Then cos(theta) = |N| / |D| = |N| since D is normalized.
+
+	j|D = |j|*|D|*cos(theta) => |N| = j|D
+
+	Then we simply have:
+
+	D = N + T
+
+	To compute tangential reaction :
+
+	T = D - N
+
+	To compute reflexion vector :
+
+	R = N - T = N - (D-N) = 2*N - D
+*/
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+float Ray::SquareDistance(const Point& point, float* t)	const
+{
+	Point Diff = point - mOrig;
+	float fT = Diff | mDir;
+
+	if(fT<=0.0f)
+	{
+		fT = 0.0f;
+	}
+	else
+	{
+		fT /= mDir.SquareMagnitude();
+		Diff -= fT*mDir;
+	}
+
+	if(t) *t = fT;
+
+	return Diff.SquareMagnitude();
+}

src/external/open_dynamics_engine-ef/ode/IceRay.h

@@ -0,0 +1,98 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for rays.
+ *	\file		IceRay.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICERAY_H__
+#define __ICERAY_H__
+
+	class ICEMATHS_API Ray
+	{
+		public:
+		//! Constructor
+		inline_					Ray()																{}
+		//! Constructor
+		inline_					Ray(const Point& orig, const Point& dir) : mOrig(orig), mDir(dir)	{}
+		//! Copy constructor
+		inline_					Ray(const Ray& ray) : mOrig(ray.mOrig), mDir(ray.mDir)				{}
+		//! Destructor
+		inline_					~Ray()																{}
+
+						float	SquareDistance(const Point& point, float* t=null)	const;
+		inline_			float	Distance(const Point& point, float* t=null)			const			{ return sqrtf(SquareDistance(point, t));	}
+
+						Point	mOrig;		//!< Ray origin
+						Point	mDir;		//!< Normalized direction
+	};
+
+	inline_ void ComputeReflexionVector(Point& reflected, const Point& incoming_dir, const Point& outward_normal)
+	{
+		reflected = incoming_dir - outward_normal * 2.0f * (incoming_dir|outward_normal);
+	}
+
+	inline_ void ComputeReflexionVector(Point& reflected, const Point& source, const Point& impact, const Point& normal)
+	{
+		Point V = impact - source;
+		reflected = V - normal * 2.0f * (V|normal);
+	}
+
+	inline_ void DecomposeVector(Point& normal_compo, Point& tangent_compo, const Point& outward_dir, const Point& outward_normal)
+	{
+		normal_compo = outward_normal * (outward_dir|outward_normal);
+		tangent_compo = outward_dir - normal_compo;
+	}
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Transforms a direction vector from world space to local space
+	 *	\param		local_dir	[out] direction vector in local space
+	 *	\param		world_dir	[in] direction vector in world space
+	 *	\param		world		[in] world transform
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	inline_ void ComputeLocalDirection(Point& local_dir, const Point& world_dir, const Matrix4x4& world)
+	{
+		// Get world direction back in local space
+//		Matrix3x3 InvWorld = world;
+//		local_dir = InvWorld * world_dir;
+		local_dir = Matrix3x3(world) * world_dir;
+	}
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Transforms a position vector from world space to local space
+	 *	\param		local_pt	[out] position vector in local space
+	 *	\param		world_pt	[in] position vector in world space
+	 *	\param		world		[in] world transform
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	inline_ void ComputeLocalPoint(Point& local_pt, const Point& world_pt, const Matrix4x4& world)
+	{
+		// Get world vertex back in local space
+		Matrix4x4 InvWorld = world;
+		InvWorld.Invert();
+		local_pt = world_pt * InvWorld;
+	}
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Transforms a ray from world space to local space
+	 *	\param		local_ray	[out] ray in local space
+	 *	\param		world_ray	[in] ray in world space
+	 *	\param		world		[in] world transform
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	inline_ void ComputeLocalRay(Ray& local_ray, const Ray& world_ray, const Matrix4x4& world)
+	{
+		// Get world ray back in local space
+		ComputeLocalDirection(local_ray.mDir, world_ray.mDir, world);
+		ComputeLocalPoint(local_ray.mOrig, world_ray.mOrig, world);
+	}
+
+#endif // __ICERAY_H__

src/external/open_dynamics_engine-ef/ode/IceRevisitedRadix.cpp

@@ -0,0 +1,528 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains source code from the article "Radix Sort Revisited".
+ *	\file		IceRevisitedRadix.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Revisited Radix Sort.
+ *	This is my new radix routine:
+ *  - it uses indices and doesn't recopy the values anymore, hence wasting less ram
+ *  - it creates all the histograms in one run instead of four
+ *  - it sorts words faster than dwords and bytes faster than words
+ *  - it correctly sorts negative floating-point values by patching the offsets
+ *  - it automatically takes advantage of temporal coherence
+ *  - multiple keys support is a side effect of temporal coherence
+ *  - it may be worth recoding in asm... (mainly to use FCOMI, FCMOV, etc) [it's probably memory-bound anyway]
+ *
+ *	History:
+ *	- 08.15.98: very first version
+ *	- 04.04.00: recoded for the radix article
+ *	- 12.xx.00: code lifting
+ *	- 09.18.01: faster CHECK_PASS_VALIDITY thanks to Mark D. Shattuck (who provided other tips, not included here)
+ *	- 10.11.01: added local ram support
+ *	- 01.20.02: bugfix! In very particular cases the last pass was skipped in the float code-path, leading to incorrect sorting......
+ *	- 01.02.02:	- "mIndices" renamed => "mRanks". That's a rank sorter after all.
+ *				- ranks are not "reset" anymore, but implicit on first calls
+ *	- 07.05.02:	- offsets rewritten with one less indirection.
+ *	- 11.03.02:	- "bool" replaced with RadixHint enum
+ *
+ *	\class		RadixSort
+ *	\author		Pierre Terdiman
+ *	\version	1.4
+ *	\date		August, 15, 1998
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/*
+To do:
+	- add an offset parameter between two input values (avoid some data recopy sometimes)
+	- unroll ? asm ?
+	- 11 bits trick & 3 passes as Michael did
+	- prefetch stuff the day I have a P3
+	- make a version with 16-bits indices ?
+*/
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceCore;
+
+#define INVALIDATE_RANKS	mCurrentSize|=0x80000000
+#define VALIDATE_RANKS		mCurrentSize&=0x7fffffff
+#define CURRENT_SIZE		(mCurrentSize&0x7fffffff)
+#define INVALID_RANKS		(mCurrentSize&0x80000000)
+
+#define CHECK_RESIZE(n)																		\
+	if(n!=mPreviousSize)																	\
+	{																						\
+				if(n>mCurrentSize)	Resize(n);												\
+		else						ResetRanks();											\
+		mPreviousSize = n;																	\
+	}
+
+#define CREATE_HISTOGRAMS(type, buffer)														\
+	/* Clear counters/histograms */															\
+	ZeroMemory(mHistogram, 256*4*sizeof(udword));											\
+																							\
+	/* Prepare to count */																	\
+	ubyte* p = (ubyte*)input;																\
+	ubyte* pe = &p[nb*4];																	\
+	udword* h0= &mHistogram[0];		/* Histogram for first pass (LSB)	*/					\
+	udword* h1= &mHistogram[256];	/* Histogram for second pass		*/					\
+	udword* h2= &mHistogram[512];	/* Histogram for third pass			*/					\
+	udword* h3= &mHistogram[768];	/* Histogram for last pass (MSB)	*/					\
+																							\
+	bool AlreadySorted = true;	/* Optimism... */											\
+																							\
+	if(INVALID_RANKS)																		\
+	{																						\
+		/* Prepare for temporal coherence */												\
+		type* Running = (type*)buffer;														\
+		type PrevVal = *Running;															\
+																							\
+		while(p!=pe)																		\
+		{																					\
+			/* Read input buffer in previous sorted order */								\
+			type Val = *Running++;															\
+			/* Check whether already sorted or not */										\
+			if(Val<PrevVal)	{ AlreadySorted = false; break; } /* Early out */				\
+			/* Update for next iteration */													\
+			PrevVal = Val;																	\
+																							\
+			/* Create histograms */															\
+			h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;									\
+		}																					\
+																							\
+		/* If all input values are already sorted, we just have to return and leave the */	\
+		/* previous list unchanged. That way the routine may take advantage of temporal */	\
+		/* coherence, for example when used to sort transparent faces.					*/	\
+		if(AlreadySorted)																	\
+		{																					\
+			mNbHits++;																		\
+			for(udword i=0;i<nb;i++)	mRanks[i] = i;										\
+			return *this;																	\
+		}																					\
+	}																						\
+	else																					\
+	{																						\
+		/* Prepare for temporal coherence */												\
+		udword* Indices = mRanks;															\
+		type PrevVal = (type)buffer[*Indices];												\
+																							\
+		while(p!=pe)																		\
+		{																					\
+			/* Read input buffer in previous sorted order */								\
+			type Val = (type)buffer[*Indices++];											\
+			/* Check whether already sorted or not */										\
+			if(Val<PrevVal)	{ AlreadySorted = false; break; } /* Early out */				\
+			/* Update for next iteration */													\
+			PrevVal = Val;																	\
+																							\
+			/* Create histograms */															\
+			h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;									\
+		}																					\
+																							\
+		/* If all input values are already sorted, we just have to return and leave the */	\
+		/* previous list unchanged. That way the routine may take advantage of temporal */	\
+		/* coherence, for example when used to sort transparent faces.					*/	\
+		if(AlreadySorted)	{ mNbHits++; return *this;	}									\
+	}																						\
+																							\
+	/* Else there has been an early out and we must finish computing the histograms */		\
+	while(p!=pe)																			\
+	{																						\
+		/* Create histograms without the previous overhead */								\
+		h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;										\
+	}
+
+#define CHECK_PASS_VALIDITY(pass)															\
+	/* Shortcut to current counters */														\
+	udword* CurCount = &mHistogram[pass<<8];												\
+																							\
+	/* Reset flag. The sorting pass is supposed to be performed. (default) */				\
+	bool PerformPass = true;																\
+																							\
+	/* Check pass validity */																\
+																							\
+	/* If all values have the same byte, sorting is useless. */								\
+	/* It may happen when sorting bytes or words instead of dwords. */						\
+	/* This routine actually sorts words faster than dwords, and bytes */					\
+	/* faster than words. Standard running time (O(4*n))is reduced to O(2*n) */				\
+	/* for words and O(n) for bytes. Running time for floats depends on actual values... */	\
+																							\
+	/* Get first byte */																	\
+	ubyte UniqueVal = *(((ubyte*)input)+pass);												\
+																							\
+	/* Check that byte's counter */															\
+	if(CurCount[UniqueVal]==nb)	PerformPass=false;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+RadixSort::RadixSort() :  mCurrentSize(0), mRanks(null), mRanks2(null), mTotalCalls(0), mNbHits(0)
+{
+#ifndef RADIX_LOCAL_RAM
+	// Allocate input-independent ram
+	mHistogram	= new udword[256*4];
+	mOffset		= new udword[256];
+#endif
+	// Initialize indices
+	INVALIDATE_RANKS;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+RadixSort::~RadixSort()
+{
+	// Release everything
+#ifndef RADIX_LOCAL_RAM
+	DELETEARRAY(mOffset);
+	DELETEARRAY(mHistogram);
+#endif
+	DELETEARRAY(mRanks2);
+	DELETEARRAY(mRanks);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Resizes the inner lists.
+ *	\param		nb	[in] new size (number of dwords)
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool RadixSort::Resize(udword nb)
+{
+	// Free previously used ram
+	DELETEARRAY(mRanks2);
+	DELETEARRAY(mRanks);
+
+	// Get some fresh one
+	mRanks	= new udword[nb];	CHECKALLOC(mRanks);
+	mRanks2	= new udword[nb];	CHECKALLOC(mRanks2);
+
+	return true;
+}
+
+inline_ void RadixSort::CheckResize(udword nb)
+{
+	udword CurSize = CURRENT_SIZE;
+	if(nb!=CurSize)
+	{
+		if(nb>CurSize)	Resize(nb);
+		mCurrentSize = nb;
+		INVALIDATE_RANKS;
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Main sort routine.
+ *	This one is for integer values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data.
+ *	\param		input	[in] a list of integer values to sort
+ *	\param		nb		[in] number of values to sort, must be < 2^31
+ *	\param		hint	[in] RADIX_SIGNED to handle negative values, RADIX_UNSIGNED if you know your input buffer only contains positive values
+ *	\return		Self-Reference
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+RadixSort& RadixSort::Sort(const udword* input, udword nb, RadixHint hint)
+{
+	// Checkings
+	if(!input || !nb || nb&0x80000000)	return *this;
+
+	// Stats
+	mTotalCalls++;
+
+	// Resize lists if needed
+	CheckResize(nb);
+
+#ifdef RADIX_LOCAL_RAM
+	// Allocate histograms & offsets on the stack
+	udword mHistogram[256*4];
+//	udword mOffset[256];
+	udword* mLink[256];
+#endif
+
+	// Create histograms (counters). Counters for all passes are created in one run.
+	// Pros:	read input buffer once instead of four times
+	// Cons:	mHistogram is 4Kb instead of 1Kb
+	// We must take care of signed/unsigned values for temporal coherence.... I just
+	// have 2 code paths even if just a single opcode changes. Self-modifying code, someone?
+	if(hint==RADIX_UNSIGNED)	{ CREATE_HISTOGRAMS(udword, input);	}
+	else						{ CREATE_HISTOGRAMS(sdword, input);	}
+
+	// Compute #negative values involved if needed
+	udword NbNegativeValues = 0;
+	if(hint==RADIX_SIGNED)
+	{
+		// An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128
+		// last values of the last histogram. Last histogram because that's the one for the Most Significant Byte,
+		// responsible for the sign. 128 last values because the 128 first ones are related to positive numbers.
+		udword* h3= &mHistogram[768];
+		for(udword i=128;i<256;i++)	NbNegativeValues += h3[i];	// 768 for last histogram, 128 for negative part
+	}
+
+	// Radix sort, j is the pass number (0=LSB, 3=MSB)
+	for(udword j=0;j<4;j++)
+	{
+		CHECK_PASS_VALIDITY(j);
+
+		// Sometimes the fourth (negative) pass is skipped because all numbers are negative and the MSB is 0xFF (for example). This is
+		// not a problem, numbers are correctly sorted anyway.
+		if(PerformPass)
+		{
+			// Should we care about negative values?
+			if(j!=3 || hint==RADIX_UNSIGNED)
+			{
+				// Here we deal with positive values only
+
+				// Create offsets
+//				mOffset[0] = 0;
+//				for(udword i=1;i<256;i++)		mOffset[i] = mOffset[i-1] + CurCount[i-1];
+				mLink[0] = mRanks2;
+				for(udword i=1;i<256;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];
+			}
+			else
+			{
+				// This is a special case to correctly handle negative integers. They're sorted in the right order but at the wrong place.
+
+				// Create biased offsets, in order for negative numbers to be sorted as well
+//				mOffset[0] = NbNegativeValues;												// First positive number takes place after the negative ones
+				mLink[0] = &mRanks2[NbNegativeValues];										// First positive number takes place after the negative ones
+//				for(udword i=1;i<128;i++)		mOffset[i] = mOffset[i-1] + CurCount[i-1];	// 1 to 128 for positive numbers
+				for(udword i=1;i<128;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];		// 1 to 128 for positive numbers
+
+				// Fixing the wrong place for negative values
+//				mOffset[128] = 0;
+				mLink[128] = mRanks2;
+//				for(i=129;i<256;i++)			mOffset[i] = mOffset[i-1] + CurCount[i-1];
+				for(udword i=129;i<256;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];
+			}
+
+			// Perform Radix Sort
+			ubyte* InputBytes	= (ubyte*)input;
+			InputBytes += j;
+			if(INVALID_RANKS)
+			{
+//				for(udword i=0;i<nb;i++)	mRanks2[mOffset[InputBytes[i<<2]]++] = i;
+				for(udword i=0;i<nb;i++)	*mLink[InputBytes[i<<2]]++ = i;
+				VALIDATE_RANKS;
+			}
+			else
+			{
+				udword* Indices		= mRanks;
+				udword* IndicesEnd	= &mRanks[nb];
+				while(Indices!=IndicesEnd)
+				{
+					udword id = *Indices++;
+//					mRanks2[mOffset[InputBytes[id<<2]]++] = id;
+					*mLink[InputBytes[id<<2]]++ = id;
+				}
+			}
+
+			// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+			udword* Tmp	= mRanks;	mRanks = mRanks2; mRanks2 = Tmp;
+		}
+	}
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Main sort routine.
+ *	This one is for floating-point values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data.
+ *	param		input			[in] a list of floating-point values to sort
+ *	\param		nb				[in] number of values to sort, must be < 2^31
+ *	\return		Self-Reference
+ *	\warning	only sorts IEEE floating-point values
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+RadixSort& RadixSort::Sort(const float* input2, udword nb)
+{
+	// Checkings
+	if(!input2 || !nb || nb&0x80000000)	return *this;
+
+	// Stats
+	mTotalCalls++;
+
+	udword* input = (udword*)input2;
+
+	// Resize lists if needed
+	CheckResize(nb);
+
+#ifdef RADIX_LOCAL_RAM
+	// Allocate histograms & offsets on the stack
+	udword mHistogram[256*4];
+//	udword mOffset[256];
+	udword* mLink[256];
+#endif
+
+	// Create histograms (counters). Counters for all passes are created in one run.
+	// Pros:	read input buffer once instead of four times
+	// Cons:	mHistogram is 4Kb instead of 1Kb
+	// Floating-point values are always supposed to be signed values, so there's only one code path there.
+	// Please note the floating point comparison needed for temporal coherence! Although the resulting asm code
+	// is dreadful, this is surprisingly not such a performance hit - well, I suppose that's a big one on first
+	// generation Pentiums....We can't make comparison on integer representations because, as Chris said, it just
+	// wouldn't work with mixed positive/negative values....
+	{ CREATE_HISTOGRAMS(float, input2); }
+
+	// Compute #negative values involved if needed
+	udword NbNegativeValues = 0;
+	// An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128
+	// last values of the last histogram. Last histogram because that's the one for the Most Significant Byte,
+	// responsible for the sign. 128 last values because the 128 first ones are related to positive numbers.
+	udword* h3= &mHistogram[768];
+	for(udword i=128;i<256;i++)	NbNegativeValues += h3[i];	// 768 for last histogram, 128 for negative part
+
+	// Radix sort, j is the pass number (0=LSB, 3=MSB)
+	for(udword j=0;j<4;j++)
+	{
+		// Should we care about negative values?
+		if(j!=3)
+		{
+			// Here we deal with positive values only
+			CHECK_PASS_VALIDITY(j);
+
+			if(PerformPass)
+			{
+				// Create offsets
+//				mOffset[0] = 0;
+				mLink[0] = mRanks2;
+//				for(udword i=1;i<256;i++)		mOffset[i] = mOffset[i-1] + CurCount[i-1];
+				for(udword i=1;i<256;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];
+
+				// Perform Radix Sort
+				ubyte* InputBytes = (ubyte*)input;
+				InputBytes += j;
+				if(INVALID_RANKS)
+				{
+//					for(i=0;i<nb;i++)	mRanks2[mOffset[InputBytes[i<<2]]++] = i;
+					for(udword i=0;i<nb;i++)	*mLink[InputBytes[i<<2]]++ = i;
+					VALIDATE_RANKS;
+				}
+				else
+				{
+					udword* Indices		= mRanks;
+					udword* IndicesEnd	= &mRanks[nb];
+					while(Indices!=IndicesEnd)
+					{
+						udword id = *Indices++;
+//						mRanks2[mOffset[InputBytes[id<<2]]++] = id;
+						*mLink[InputBytes[id<<2]]++ = id;
+					}
+				}
+
+				// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+				udword* Tmp	= mRanks;	mRanks = mRanks2; mRanks2 = Tmp;
+			}
+		}
+		else
+		{
+			// This is a special case to correctly handle negative values
+			CHECK_PASS_VALIDITY(j);
+
+			if(PerformPass)
+			{
+				// Create biased offsets, in order for negative numbers to be sorted as well
+//				mOffset[0] = NbNegativeValues;												// First positive number takes place after the negative ones
+				mLink[0] = &mRanks2[NbNegativeValues];										// First positive number takes place after the negative ones
+//				for(udword i=1;i<128;i++)		mOffset[i] = mOffset[i-1] + CurCount[i-1];	// 1 to 128 for positive numbers
+				for(udword i=1;i<128;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];		// 1 to 128 for positive numbers
+
+				// We must reverse the sorting order for negative numbers!
+//				mOffset[255] = 0;
+				mLink[255] = mRanks2;
+//				for(i=0;i<127;i++)		mOffset[254-i] = mOffset[255-i] + CurCount[255-i];	// Fixing the wrong order for negative values
+				for(udword i=0;i<127;i++)	mLink[254-i] = mLink[255-i] + CurCount[255-i];		// Fixing the wrong order for negative values
+//				for(i=128;i<256;i++)	mOffset[i] += CurCount[i];							// Fixing the wrong place for negative values
+				for(udword i=128;i<256;i++)	mLink[i] += CurCount[i];							// Fixing the wrong place for negative values
+
+				// Perform Radix Sort
+				if(INVALID_RANKS)
+				{
+					for(udword i=0;i<nb;i++)
+					{
+						udword Radix = input[i]>>24;							// Radix byte, same as above. AND is useless here (udword).
+						// ### cmp to be killed. Not good. Later.
+//						if(Radix<128)		mRanks2[mOffset[Radix]++] = i;		// Number is positive, same as above
+//						else				mRanks2[--mOffset[Radix]] = i;		// Number is negative, flip the sorting order
+						if(Radix<128)		*mLink[Radix]++ = i;		// Number is positive, same as above
+						else				*(--mLink[Radix]) = i;		// Number is negative, flip the sorting order
+					}
+					VALIDATE_RANKS;
+				}
+				else
+				{
+					for(udword i=0;i<nb;i++)
+					{
+						udword Radix = input[mRanks[i]]>>24;							// Radix byte, same as above. AND is useless here (udword).
+						// ### cmp to be killed. Not good. Later.
+//						if(Radix<128)		mRanks2[mOffset[Radix]++] = mRanks[i];		// Number is positive, same as above
+//						else				mRanks2[--mOffset[Radix]] = mRanks[i];		// Number is negative, flip the sorting order
+						if(Radix<128)		*mLink[Radix]++ = mRanks[i];		// Number is positive, same as above
+						else				*(--mLink[Radix]) = mRanks[i];		// Number is negative, flip the sorting order
+					}
+				}
+				// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+				udword* Tmp	= mRanks;	mRanks = mRanks2; mRanks2 = Tmp;
+			}
+			else
+			{
+				// The pass is useless, yet we still have to reverse the order of current list if all values are negative.
+				if(UniqueVal>=128)
+				{
+					if(INVALID_RANKS)
+					{
+						// ###Possible?
+						for(udword i=0;i<nb;i++)	mRanks2[i] = nb-i-1;
+						VALIDATE_RANKS;
+					}
+					else
+					{
+						for(udword i=0;i<nb;i++)	mRanks2[i] = mRanks[nb-i-1];
+					}
+
+					// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+					udword* Tmp	= mRanks;	mRanks = mRanks2; mRanks2 = Tmp;
+				}
+			}
+		}
+	}
+	return *this;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the ram used.
+ *	\return		memory used in bytes
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword RadixSort::GetUsedRam() const
+{
+	udword UsedRam = sizeof(RadixSort);
+#ifndef RADIX_LOCAL_RAM
+	UsedRam += 256*4*sizeof(udword);			// Histograms
+	UsedRam += 256*sizeof(udword);				// Offsets
+#endif
+	UsedRam += 2*CURRENT_SIZE*sizeof(udword);	// 2 lists of indices
+	return UsedRam;
+}

src/external/open_dynamics_engine-ef/ode/IceRevisitedRadix.h

@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains source code from the article "Radix Sort Revisited".
+ *	\file		IceRevisitedRadix.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICERADIXSORT_H__
+#define __ICERADIXSORT_H__
+
+	//! Allocate histograms & offsets locally
+	#define RADIX_LOCAL_RAM
+
+	enum RadixHint
+	{
+		RADIX_SIGNED,		//!< Input values are signed
+		RADIX_UNSIGNED,		//!< Input values are unsigned
+
+		RADIX_FORCE_DWORD = 0x7fffffff
+	};
+
+	class ICECORE_API RadixSort
+	{
+		public:
+		// Constructor/Destructor
+								RadixSort();
+								~RadixSort();
+		// Sorting methods
+				RadixSort&		Sort(const udword* input, udword nb, RadixHint hint=RADIX_SIGNED);
+				RadixSort&		Sort(const float* input, udword nb);
+
+		//! Access to results. mRanks is a list of indices in sorted order, i.e. in the order you may further process your data
+		inline_	const udword*	GetRanks()			const	{ return mRanks;		}
+
+		//! mIndices2 gets trashed on calling the sort routine, but otherwise you can recycle it the way you want.
+		inline_	udword*			GetRecyclable()		const	{ return mRanks2;		}
+
+		// Stats
+				udword			GetUsedRam()		const;
+		//! Returns the total number of calls to the radix sorter.
+		inline_	udword			GetNbTotalCalls()	const	{ return mTotalCalls;	}
+		//! Returns the number of eraly exits due to temporal coherence.
+		inline_	udword			GetNbHits()			const	{ return mNbHits;		}
+
+		private:
+#ifndef RADIX_LOCAL_RAM
+				udword*			mHistogram;			//!< Counters for each byte
+				udword*			mOffset;			//!< Offsets (nearly a cumulative distribution function)
+#endif
+				udword			mCurrentSize;		//!< Current size of the indices list
+				udword*			mRanks;				//!< Two lists, swapped each pass
+				udword*			mRanks2;
+		// Stats
+				udword			mTotalCalls;		//!< Total number of calls to the sort routine
+				udword			mNbHits;			//!< Number of early exits due to coherence
+		// Internal methods
+				void			CheckResize(udword nb);
+				bool			Resize(udword nb);
+	};
+
+#endif // __ICERADIXSORT_H__

src/external/open_dynamics_engine-ef/ode/IceSegment.cpp

@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for segments.
+ *	\file		IceSegment.cpp
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Segment class.
+ *	A segment is defined by S(t) = mP0 * (1 - t) + mP1 * t, with 0 <= t <= 1
+ *	Alternatively, a segment is S(t) = Origin + t * Direction for 0 <= t <= 1.
+ *	Direction is not necessarily unit length. The end points are Origin = mP0 and Origin + Direction = mP1.
+ *
+ *	\class		Segment
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+float Segment::SquareDistance(const Point& point, float* t)	const
+{
+	Point Diff = point - mP0;
+	Point Dir = mP1 - mP0;
+	float fT = Diff | Dir;
+
+	if(fT<=0.0f)
+	{
+		fT = 0.0f;
+	}
+	else
+	{
+		float SqrLen= Dir.SquareMagnitude();
+		if(fT>=SqrLen)
+		{
+			fT = 1.0f;
+			Diff -= Dir;
+		}
+		else
+		{
+			fT /= SqrLen;
+			Diff -= fT*Dir;
+		}
+	}
+
+	if(t)	*t = fT;
+
+	return Diff.SquareMagnitude();
+}

src/external/open_dynamics_engine-ef/ode/IceSegment.h

@@ -0,0 +1,55 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for segments.
+ *	\file		IceSegment.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICESEGMENT_H__
+#define __ICESEGMENT_H__
+
+	class ICEMATHS_API Segment
+	{
+		public:
+		//! Constructor
+		inline_					Segment()															{}
+		//! Constructor
+		inline_					Segment(const Point& p0, const Point& p1) : mP0(p0), mP1(p1)		{}
+		//! Copy constructor
+		inline_					Segment(const Segment& seg) : mP0(seg.mP0), mP1(seg.mP1)			{}
+		//! Destructor
+		inline_					~Segment()															{}
+
+		inline_	const	Point&	GetOrigin()						const	{ return mP0;						}
+		inline_			Point	ComputeDirection()				const	{ return mP1 - mP0;					}
+		inline_			void	ComputeDirection(Point& dir)	const	{ dir = mP1 - mP0;					}
+		inline_			float	ComputeLength()					const	{ return mP1.Distance(mP0);			}
+		inline_			float	ComputeSquareLength()			const	{ return mP1.SquareDistance(mP0);	}
+
+		inline_			void	SetOriginDirection(const Point& origin, const Point& direction)
+								{
+									mP0 = mP1 = origin;
+									mP1 += direction;
+								}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes a point on the segment
+		 *	\param		pt	[out] point on segment
+		 *	\param		t	[in] point's parameter [t=0 => pt = mP0, t=1 => pt = mP1]
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			void	ComputePoint(Point& pt, float t)	const	{	pt = mP0 + t * (mP1 - mP0);		}
+
+						float	SquareDistance(const Point& point, float* t=null)	const;
+		inline_			float	Distance(const Point& point, float* t=null)			const			{ return sqrtf(SquareDistance(point, t));	}
+
+						Point	mP0;		//!< Start of segment
+						Point	mP1;		//!< End of segment
+	};
+
+#endif // __ICESEGMENT_H__

src/external/open_dynamics_engine-ef/ode/IceTriList.h

@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a triangle container.
+ *	\file	  IceTriList.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICETRILIST_H__
+#define __ICETRILIST_H__
+
+	class ICEMATHS_API TriList : public Container
+	{
+		public:
+		// Constructor / Destructor
+								TriList()					{}
+								~TriList()					{}
+
+		inline_	udword			GetNbTriangles()	const	{ return GetNbEntries()/9;			}
+		inline_	Triangle*		GetTriangles()		const	{ return (Triangle*)GetEntries();	}
+
+				void			AddTri(const Triangle& tri)
+								{
+									Add(tri.mVerts[0].x).Add(tri.mVerts[0].y).Add(tri.mVerts[0].z);
+									Add(tri.mVerts[1].x).Add(tri.mVerts[1].y).Add(tri.mVerts[1].z);
+									Add(tri.mVerts[2].x).Add(tri.mVerts[2].y).Add(tri.mVerts[2].z);
+								}
+
+				void			AddTri(const Point& p0, const Point& p1, const Point& p2)
+								{
+									Add(p0.x).Add(p0.y).Add(p0.z);
+									Add(p1.x).Add(p1.y).Add(p1.z);
+									Add(p2.x).Add(p2.y).Add(p2.z);
+								}
+	};
+
+	class ICEMATHS_API TriangleList : public Container
+	{
+		public:
+		// Constructor / Destructor
+									TriangleList()				{}
+									~TriangleList()				{}
+
+		inline_	udword				GetNbTriangles()	const	{ return GetNbEntries()/3;				}
+		inline_	IndexedTriangle*	GetTriangles()		const	{ return (IndexedTriangle*)GetEntries();}
+
+				void				AddTriangle(const IndexedTriangle& tri)
+									{
+										Add(tri.mVRef[0]).Add(tri.mVRef[1]).Add(tri.mVRef[2]);
+									}
+
+				void				AddTriangle(udword vref0, udword vref1, udword vref2)
+									{
+										Add(vref0).Add(vref1).Add(vref2);
+									}
+	};
+
+#endif //__ICETRILIST_H__

src/external/open_dynamics_engine-ef/ode/IceTriangle.cpp

@@ -0,0 +1,295 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a handy triangle class.
+ *	\file		IceTriangle.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 17, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceMaths;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a triangle class.
+ *
+ *	class		Tri
+ *	\author		Pierre Terdiman
+ *	\version	1.0
+ *	\date		08.15.98
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+static sdword VPlaneSideEps(const Point& v, const Plane& plane, float epsilon)
+{
+	// Compute distance from current vertex to the plane
+	float Dist = plane.Distance(v);
+	// Compute side:
+	// 1	= the vertex is on the positive side of the plane
+	// -1	= the vertex is on the negative side of the plane
+	// 0	= the vertex is on the plane (within epsilon)
+	return Dist > epsilon ? 1 : Dist < -epsilon ? -1 : 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Flips the winding order.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Triangle::Flip()
+{
+	Point Tmp = mVerts[1];
+	mVerts[1] = mVerts[2];
+	mVerts[2] = Tmp;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle area.
+ *	\return		the area
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Triangle::Area() const
+{
+	const Point& p0 = mVerts[0];
+	const Point& p1 = mVerts[1];
+	const Point& p2 = mVerts[2];
+	return ((p0 - p1)^(p0 - p2)).Magnitude() * 0.5f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle perimeter.
+ *	\return		the perimeter
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Triangle::Perimeter()	const
+{
+	const Point& p0 = mVerts[0];
+	const Point& p1 = mVerts[1];
+	const Point& p2 = mVerts[2];
+	return		p0.Distance(p1)
+			+	p0.Distance(p2)
+			+	p1.Distance(p2);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle compacity.
+ *	\return		the compacity
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Triangle::Compacity() const
+{
+	float P = Perimeter();
+	if(P==0.0f)	return 0.0f;
+	return (4.0f*PI*Area()/(P*P));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle normal.
+ *	\param		normal	[out] the computed normal
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Triangle::Normal(Point& normal) const
+{
+	const Point& p0 = mVerts[0];
+	const Point& p1 = mVerts[1];
+	const Point& p2 = mVerts[2];
+	normal = ((p0 - p1)^(p0 - p2)).Normalize();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle denormalized normal.
+ *	\param		normal	[out] the computed normal
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Triangle::DenormalizedNormal(Point& normal) const
+{
+	const Point& p0 = mVerts[0];
+	const Point& p1 = mVerts[1];
+	const Point& p2 = mVerts[2];
+	normal = ((p0 - p1)^(p0 - p2));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle center.
+ *	\param		center	[out] the computed center
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Triangle::Center(Point& center) const
+{
+	const Point& p0 = mVerts[0];
+	const Point& p1 = mVerts[1];
+	const Point& p2 = mVerts[2];
+	center = (p0 + p1 + p2)*INV3;
+}
+
+PartVal Triangle::TestAgainstPlane(const Plane& plane, float epsilon) const
+{
+	bool Pos = false, Neg = false;
+
+	// Loop through all vertices
+	for(udword i=0;i<3;i++)
+	{
+		// Compute side:
+		sdword Side = VPlaneSideEps(mVerts[i], plane, epsilon);
+
+				if (Side < 0)	Neg = true;
+		else	if (Side > 0)	Pos = true;
+	}
+
+			if (!Pos && !Neg)	return TRI_ON_PLANE;
+	else	if (Pos && Neg)		return TRI_INTERSECT;
+	else	if (Pos && !Neg)	return TRI_PLUS_SPACE;
+	else	if (!Pos && Neg)	return TRI_MINUS_SPACE;
+
+	// What?!
+	return TRI_FORCEDWORD;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle moment.
+ *	\param		m	[out] the moment
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+void Triangle::ComputeMoment(Moment& m)
+{
+	// Compute the area of the triangle
+	m.mArea = Area();
+
+	// Compute the centroid
+	Center(m.mCentroid);
+
+	// Second-order components. Handle zero-area faces.
+	Point& p = mVerts[0];
+	Point& q = mVerts[1];
+	Point& r = mVerts[2];
+	if(m.mArea==0.0f)
+	{
+		// This triangle has zero area. The second order components would be eliminated with the usual formula, so, for the
+		// sake of robustness we use an alternative form. These are the centroid and second-order components of the triangle's vertices.
+		m.mCovariance.m[0][0] = (p.x*p.x + q.x*q.x + r.x*r.x);
+		m.mCovariance.m[0][1] = (p.x*p.y + q.x*q.y + r.x*r.y);
+		m.mCovariance.m[0][2] = (p.x*p.z + q.x*q.z + r.x*r.z);
+		m.mCovariance.m[1][1] = (p.y*p.y + q.y*q.y + r.y*r.y);
+		m.mCovariance.m[1][2] = (p.y*p.z + q.y*q.z + r.y*r.z);
+		m.mCovariance.m[2][2] = (p.z*p.z + q.z*q.z + r.z*r.z);
+		m.mCovariance.m[2][1] = m.mCovariance.m[1][2];
+		m.mCovariance.m[1][0] = m.mCovariance.m[0][1];
+		m.mCovariance.m[2][0] = m.mCovariance.m[0][2];
+	}
+	else
+	{
+		const float OneOverTwelve = 1.0f / 12.0f;
+		m.mCovariance.m[0][0] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.x + p.x*p.x + q.x*q.x + r.x*r.x) * OneOverTwelve;
+		m.mCovariance.m[0][1] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.y + p.x*p.y + q.x*q.y + r.x*r.y) * OneOverTwelve;
+		m.mCovariance.m[1][1] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.y + p.y*p.y + q.y*q.y + r.y*r.y) * OneOverTwelve;
+		m.mCovariance.m[0][2] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.z + p.x*p.z + q.x*q.z + r.x*r.z) * OneOverTwelve;
+		m.mCovariance.m[1][2] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.z + p.y*p.z + q.y*q.z + r.y*r.z) * OneOverTwelve;
+		m.mCovariance.m[2][2] = m.mArea * (9.0f * m.mCentroid.z*m.mCentroid.z + p.z*p.z + q.z*q.z + r.z*r.z) * OneOverTwelve;
+		m.mCovariance.m[2][1] = m.mCovariance.m[1][2];
+		m.mCovariance.m[1][0] = m.mCovariance.m[0][1];
+		m.mCovariance.m[2][0] = m.mCovariance.m[0][2];
+	}
+}
+*/
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle's smallest edge length.
+ *	\return		the smallest edge length
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Triangle::MinEdgeLength()	const
+{
+	float Min = MAX_FLOAT;
+	float Length01 = mVerts[0].Distance(mVerts[1]);
+	float Length02 = mVerts[0].Distance(mVerts[2]);
+	float Length12 = mVerts[1].Distance(mVerts[2]);
+	if(Length01 < Min)	Min = Length01;
+	if(Length02 < Min)	Min = Length02;
+	if(Length12 < Min)	Min = Length12;
+	return Min;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the triangle's largest edge length.
+ *	\return		the largest edge length
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float Triangle::MaxEdgeLength()	const
+{
+	float Max = MIN_FLOAT;
+	float Length01 = mVerts[0].Distance(mVerts[1]);
+	float Length02 = mVerts[0].Distance(mVerts[2]);
+	float Length12 = mVerts[1].Distance(mVerts[2]);
+	if(Length01 > Max)	Max = Length01;
+	if(Length02 > Max)	Max = Length02;
+	if(Length12 > Max)	Max = Length12;
+	return Max;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes a point on the triangle according to the stabbing information.
+ *	\param		u			[in] point's barycentric coordinates
+ *  \param    v      [in] point's barycentric coordinates
+ *	\param		pt			[out] point on triangle
+ *	\param		nearvtx		[out] index of nearest vertex
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Triangle::ComputePoint(float u, float v, Point& pt, udword* nearvtx)	const
+{
+	// Compute point coordinates
+	pt = (1.0f - u - v)*mVerts[0] + u*mVerts[1] + v*mVerts[2];
+
+	// Compute nearest vertex if needed
+	if(nearvtx)
+	{
+		// Compute distance vector
+		Point d(mVerts[0].SquareDistance(pt),	// Distance^2 from vertex 0 to point on the face
+				mVerts[1].SquareDistance(pt),	// Distance^2 from vertex 1 to point on the face
+				mVerts[2].SquareDistance(pt));	// Distance^2 from vertex 2 to point on the face
+
+		// Get smallest distance
+		*nearvtx = d.SmallestAxis();
+	}
+}
+
+void Triangle::Inflate(float fat_coeff, bool constant_border)
+{
+	// Compute triangle center
+	Point TriangleCenter;
+	Center(TriangleCenter);
+
+	// Don't normalize?
+	// Normalize => add a constant border, regardless of triangle size
+	// Don't => add more to big triangles
+	for(udword i=0;i<3;i++)
+	{
+		Point v = mVerts[i] - TriangleCenter;
+
+		if(constant_border)	v.Normalize();
+
+		mVerts[i] += v * fat_coeff;
+	}
+}

src/external/open_dynamics_engine-ef/ode/IceTriangle.h

@@ -0,0 +1,68 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a handy triangle class.
+ *	\file		IceTriangle.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 17, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICETRIANGLE_H__
+#define __ICETRIANGLE_H__
+
+	// Forward declarations
+	class Moment;
+
+	// Partitioning values
+	enum PartVal
+	{
+		TRI_MINUS_SPACE		= 0,			//!< Triangle is in the negative space
+		TRI_PLUS_SPACE		= 1,			//!< Triangle is in the positive space
+		TRI_INTERSECT		= 2,			//!< Triangle intersects plane
+		TRI_ON_PLANE		= 3,			//!< Triangle and plane are coplanar
+
+		TRI_FORCEDWORD		= 0x7fffffff
+	};
+
+	// A triangle class.
+	class ICEMATHS_API Triangle
+	{
+		public:
+		//! Constructor
+		inline_					Triangle()													{}
+		//! Constructor
+		inline_					Triangle(const Point& p0, const Point& p1, const Point& p2)	{ mVerts[0]=p0; mVerts[1]=p1; mVerts[2]=p2; }
+		//! Copy constructor
+		inline_					Triangle(const Triangle& triangle)
+								{
+									mVerts[0] = triangle.mVerts[0];
+									mVerts[1] = triangle.mVerts[1];
+									mVerts[2] = triangle.mVerts[2];
+								}
+		//! Destructor
+		inline_					~Triangle()													{}
+		//! Vertices
+				Point			mVerts[3];
+
+		// Methods
+				void			Flip();
+				float			Area() const;
+				float			Perimeter()	const;
+				float			Compacity()	const;
+				void			Normal(Point& normal) const;
+				void			DenormalizedNormal(Point& normal) const;
+				void			Center(Point& center) const;
+		inline_	Plane			PlaneEquation() const	{ return Plane(mVerts[0], mVerts[1], mVerts[2]);	}
+
+				PartVal			TestAgainstPlane(const Plane& plane, float epsilon) const;
+//				float			Distance(Point& cp, Point& cq, Tri& tri);
+				void			ComputeMoment(Moment& m);
+				float			MinEdgeLength() const;
+				float			MaxEdgeLength() const;
+				void			ComputePoint(float u, float v, Point& pt, udword* nearvtx=null)	const;
+				void			Inflate(float fat_coeff, bool constant_border);
+	};
+
+#endif // __ICETRIANGLE_H__

src/external/open_dynamics_engine-ef/ode/IceTypes.h

@@ -0,0 +1,171 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains custom types.
+ *	\file		IceTypes.h
+ *	\author		Pierre Terdiman
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICETYPES_H__
+#define __ICETYPES_H__
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Things to help us compile on non-windows platforms
+
+#if defined(__MACOSX__) || defined(__APPLE__)
+#undef bool
+#define bool char
+#undef true
+#define true ((bool)-1)
+#undef false
+#define false ((bool)0)
+#endif // mac stuff
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+	#define USE_HANDLE_MANAGER
+
+	// Constants
+	#define	PI					3.1415926535897932384626433832795028841971693993751f	//!< PI
+	#define	HALFPI				1.57079632679489661923f									//!< 0.5 * PI
+	#define	TWOPI				6.28318530717958647692f									//!< 2.0 * PI
+	#define	INVPI				0.31830988618379067154f									//!< 1.0 / PI
+
+	#define	RADTODEG			57.2957795130823208768f									//!< 180.0 / PI, convert radians to degrees
+	#define	DEGTORAD			0.01745329251994329577f									//!< PI / 180.0, convert degrees to radians
+
+	#define	EXP					2.71828182845904523536f									//!< e
+	#define	INVLOG2				3.32192809488736234787f									//!< 1.0 / log10(2)
+	#define	LN2					0.693147180559945f										//!< ln(2)
+	#define	INVLN2				1.44269504089f											//!< 1.0f / ln(2)
+
+	#define	INV3				0.33333333333333333333f									//!< 1/3
+	#define	INV6				0.16666666666666666666f									//!< 1/6
+	#define	INV7				0.14285714285714285714f									//!< 1/7
+	#define	INV9				0.11111111111111111111f									//!< 1/9
+	#define	INV255				0.00392156862745098039f									//!< 1/255
+
+	#define	SQRT2				1.41421356237f											//!< sqrt(2)
+	#define	INVSQRT2			0.707106781188f											//!< 1 / sqrt(2)
+
+	#define	SQRT3				1.73205080757f											//!< sqrt(3)
+	#define	INVSQRT3			0.577350269189f											//!< 1 / sqrt(3)
+
+	#define null				0														//!< our own NULL pointer
+
+	// Custom types used in ICE
+	typedef signed char			sbyte;		//!< sizeof(sbyte)	must be 1
+	typedef unsigned char		ubyte;		//!< sizeof(ubyte)	must be 1
+	typedef signed short		sword;		//!< sizeof(sword)	must be 2
+	typedef unsigned short		uword;		//!< sizeof(uword)	must be 2
+	typedef signed int			sdword;		//!< sizeof(sdword)	must be 4
+	typedef unsigned int		udword;		//!< sizeof(udword)	must be 4
+	typedef signed __int64		sqword;		//!< sizeof(sqword)	must be 8
+	typedef unsigned __int64	uqword;		//!< sizeof(uqword)	must be 8
+	typedef float				float32;	//!< sizeof(float32)	must be 4
+	typedef double				float64;	//!< sizeof(float64)	must be 4
+
+	ICE_COMPILE_TIME_ASSERT(sizeof(bool)==1);	// ...otherwise things might fail with VC++ 4.2 !
+	ICE_COMPILE_TIME_ASSERT(sizeof(ubyte)==1);
+	ICE_COMPILE_TIME_ASSERT(sizeof(sbyte)==1);
+	ICE_COMPILE_TIME_ASSERT(sizeof(sword)==2);
+	ICE_COMPILE_TIME_ASSERT(sizeof(uword)==2);
+	ICE_COMPILE_TIME_ASSERT(sizeof(udword)==4);
+	ICE_COMPILE_TIME_ASSERT(sizeof(sdword)==4);
+	ICE_COMPILE_TIME_ASSERT(sizeof(uqword)==8);
+	ICE_COMPILE_TIME_ASSERT(sizeof(sqword)==8);
+
+	//! TO BE DOCUMENTED
+	#define DECLARE_ICE_HANDLE(name)	struct name##__ { int unused; }; typedef struct name##__ *name
+
+	typedef udword				DynID;		//!< Dynamic identifier
+#ifdef USE_HANDLE_MANAGER
+	typedef udword				KID;		//!< Kernel ID
+//	DECLARE_ICE_HANDLE(KID);
+#else
+	typedef uword				KID;		//!< Kernel ID
+#endif
+	typedef udword				RTYPE;		//!< Relationship-type (!) between owners and references
+	#define	INVALID_ID			0xffffffff	//!< Invalid dword ID (counterpart of null pointers)
+#ifdef USE_HANDLE_MANAGER
+	#define	INVALID_KID			0xffffffff	//!< Invalid Kernel ID
+#else
+	#define	INVALID_KID			0xffff		//!< Invalid Kernel ID
+#endif
+	#define	INVALID_NUMBER		0xDEADBEEF	//!< Standard junk value
+
+	// Define BOOL if needed
+	#ifndef BOOL
+	typedef int	BOOL;						//!< Another boolean type.
+	#endif
+
+	//! Union of a float and a sdword
+	typedef union {
+		float	f;							//!< The float
+		sdword	d;							//!< The integer
+	}scell;
+
+	//! Union of a float and a udword
+	typedef union {
+		float	f;							//!< The float
+		udword	d;							//!< The integer
+	}ucell;
+
+	// Type ranges
+	#define	MAX_SBYTE				0x7f						//!< max possible sbyte value
+	#define	MIN_SBYTE				0x80						//!< min possible sbyte value
+	#define	MAX_UBYTE				0xff						//!< max possible ubyte value
+	#define	MIN_UBYTE				0x00						//!< min possible ubyte value
+	#define	MAX_SWORD				0x7fff						//!< max possible sword value
+	#define	MIN_SWORD				0x8000						//!< min possible sword value
+	#define	MAX_UWORD				0xffff						//!< max possible uword value
+	#define	MIN_UWORD				0x0000						//!< min possible uword value
+	#define	MAX_SDWORD				0x7fffffff					//!< max possible sdword value
+	#define	MIN_SDWORD				0x80000000					//!< min possible sdword value
+	#define	MAX_UDWORD				0xffffffff					//!< max possible udword value
+	#define	MIN_UDWORD				0x00000000					//!< min possible udword value
+	#define	MAX_FLOAT				FLT_MAX						//!< max possible float value
+	#define	MIN_FLOAT				(-FLT_MAX)					//!< min possible loat value
+	#define IEEE_1_0				0x3f800000					//!< integer representation of 1.0
+	#define IEEE_255_0				0x437f0000					//!< integer representation of 255.0
+	#define IEEE_MAX_FLOAT			0x7f7fffff					//!< integer representation of MAX_FLOAT
+	#define IEEE_MIN_FLOAT			0xff7fffff					//!< integer representation of MIN_FLOAT
+	#define IEEE_UNDERFLOW_LIMIT	0x1a000000
+
+	#define ONE_OVER_RAND_MAX		(1.0f / float(RAND_MAX))	//!< Inverse of the max possible value returned by rand()
+
+	typedef int					(__stdcall* PROC)();			//!< A standard procedure call.
+	typedef bool				(*ENUMERATION)(udword value, udword param, udword context);	//!< ICE standard enumeration call
+	typedef	void**				VTABLE;							//!< A V-Table.
+
+	#undef		MIN
+	#undef		MAX
+	#define		MIN(a, b)       ((a) < (b) ? (a) : (b))			//!< Returns the min value between a and b
+	#define		MAX(a, b)       ((a) > (b) ? (a) : (b))			//!< Returns the max value between a and b
+	#define		MAXMAX(a,b,c)   ((a) > (b) ? MAX (a,c) : MAX (b,c))	//!<	Returns the max value between a, b and c
+
+	template<class T>	inline_ const T&	TMin	(const T& a, const T& b)	{ return b < a ? b : a;	}
+	template<class T>	inline_ const T&	TMax	(const T& a, const T& b)	{ return a < b ? b : a;	}
+	template<class T>	inline_ void		TSetMin	(T& a, const T& b)			{ if(a>b)	a = b;		}
+	template<class T>	inline_ void		TSetMax	(T& a, const T& b)			{ if(a<b)	a = b;		}
+
+	#define		SQR(x)			((x)*(x))						//!< Returns x square
+	#define		CUBE(x)			((x)*(x)*(x))					//!< Returns x cube
+
+	#define		AND		&										//!< ...
+	#define		OR		|										//!< ...
+	#define		XOR		^										//!< ...
+
+	#define		QUADRAT(x)		((x)*(x))						//!< Returns x square
+
+#ifdef _WIN32
+#   define srand48(x) srand((unsigned int) (x))
+#	define srandom(x) srand((unsigned int) (x))
+#	define random()   ((double) rand())
+#   define drand48()  ((double) (((double) rand()) / ((double) RAND_MAX)))
+#endif
+
+#endif // __ICETYPES_H__

src/external/open_dynamics_engine-ef/ode/IceUtils.cpp

@@ -0,0 +1,46 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains misc. useful macros & defines.
+ *	\file		IceUtils.cpp
+ *	\author		Pierre Terdiman (collected from various sources)
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace IceCore;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Returns the alignment of the input address.
+ *	\fn			Alignment()
+ *	\param		address	[in] address to check
+ *	\return		the best alignment (e.g. 1 for odd addresses, etc)
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword IceCore::Alignment(udword address)
+{
+	// Returns 0 for null addresses
+	if(!address) return 0;
+
+	// Test all bits
+	udword Align = 1;
+	for(udword i=1;i<32;i++)
+	{
+		// Returns as soon as the alignment is broken
+		if(address&Align)	return Align;
+		Align<<=1;
+	}
+	// Here all bits are null, except the highest one (else the address would be null)
+	return Align;
+}

src/external/open_dynamics_engine-ef/ode/IceUtils.h

@@ -0,0 +1,257 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains misc. useful macros & defines.
+ *	\file		IceUtils.h
+ *	\author		Pierre Terdiman (collected from various sources)
+ *	\date		April, 4, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __ICEUTILS_H__
+#define __ICEUTILS_H__
+
+	#define START_RUNONCE	{ static bool __RunOnce__ = false;	if(!__RunOnce__){
+	#define END_RUNONCE		__RunOnce__ = true;}}
+
+	//! Reverse all the bits in a 32 bit word (from Steve Baker's Cute Code Collection)
+	//! (each line can be done in any order.
+	inline_ void	ReverseBits(udword& n)
+	{
+		n = ((n >>  1) & 0x55555555) | ((n <<  1) & 0xaaaaaaaa);
+		n = ((n >>  2) & 0x33333333) | ((n <<  2) & 0xcccccccc);
+		n = ((n >>  4) & 0x0f0f0f0f) | ((n <<  4) & 0xf0f0f0f0);
+		n = ((n >>  8) & 0x00ff00ff) | ((n <<  8) & 0xff00ff00);
+		n = ((n >> 16) & 0x0000ffff) | ((n << 16) & 0xffff0000);
+		// Etc for larger intergers (64 bits in Java)
+		// NOTE: the >> operation must be unsigned! (>>> in java)
+	}
+
+	//! Count the number of '1' bits in a 32 bit word (from Steve Baker's Cute Code Collection)
+	inline_ udword	CountBits(udword n)
+	{
+		// This relies of the fact that the count of n bits can NOT overflow 
+		// an n bit interger. EG: 1 bit count takes a 1 bit interger, 2 bit counts
+		// 2 bit interger, 3 bit count requires only a 2 bit interger.
+		// So we add all bit pairs, then each nible, then each byte etc...
+		n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
+		n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
+		n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
+		n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
+		n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
+		// Etc for larger intergers (64 bits in Java)
+		// NOTE: the >> operation must be unsigned! (>>> in java)
+		return n;
+	}
+
+	//! Even faster?
+	inline_ udword	CountBits2(udword bits)
+	{
+		bits = bits - ((bits >> 1) & 0x55555555);
+		bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333);
+		bits = ((bits >> 4) + bits) & 0x0F0F0F0F;
+		return (bits * 0x01010101) >> 24;
+	}
+
+	//! Spread out bits.	EG	00001111  ->   0101010101
+	//! 						00001010  ->   0100010000
+	//! This is used to interleve to intergers to produce a `Morten Key'
+	//! used in Space Filling Curves (See DrDobbs Journal, July 1999)
+	//! Order is important.
+	inline_ void	SpreadBits(udword& n)
+	{
+		n = ( n & 0x0000ffff) | (( n & 0xffff0000) << 16);
+		n = ( n & 0x000000ff) | (( n & 0x0000ff00) <<  8);
+		n = ( n & 0x000f000f) | (( n & 0x00f000f0) <<  4);
+		n = ( n & 0x03030303) | (( n & 0x0c0c0c0c) <<  2);
+		n = ( n & 0x11111111) | (( n & 0x22222222) <<  1);
+	}
+
+	// Next Largest Power of 2
+	// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
+	// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
+	// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
+	// largest power of 2. For a 32-bit value: 
+	inline_ udword	nlpo2(udword x)
+	{
+		x |= (x >> 1);
+		x |= (x >> 2);
+		x |= (x >> 4);
+		x |= (x >> 8);
+		x |= (x >> 16);
+		return x+1;
+	}
+
+	//! Test to see if a number is an exact power of two (from Steve Baker's Cute Code Collection)
+	inline_ bool	IsPowerOfTwo(udword n)				{ return ((n&(n-1))==0);					}
+
+	//! Zero the least significant '1' bit in a word. (from Steve Baker's Cute Code Collection)
+	inline_ void	ZeroLeastSetBit(udword& n)			{ n&=(n-1);									}
+
+	//! Set the least significant N bits in a word. (from Steve Baker's Cute Code Collection)
+	inline_ void	SetLeastNBits(udword& x, udword n)	{ x|=~(~0<<n);								}
+
+	//! Classic XOR swap (from Steve Baker's Cute Code Collection)
+	//! x ^= y;		/* x' = (x^y) */
+	//! y ^= x;		/* y' = (y^(x^y)) = x */
+	//! x ^= y;		/* x' = (x^y)^x = y */
+	inline_ void	Swap(udword& x, udword& y)			{ x ^= y; y ^= x; x ^= y;					}
+
+	//! Little/Big endian (from Steve Baker's Cute Code Collection)
+	//!
+	//! Extra comments by Kenny Hoff:
+	//! Determines the byte-ordering of the current machine (little or big endian)
+	//! by setting an integer value to 1 (so least significant bit is now 1); take
+	//! the address of the int and cast to a byte pointer (treat integer as an
+	//! array of four bytes); check the value of the first byte (must be 0 or 1).
+	//! If the value is 1, then the first byte least significant byte and this
+	//! implies LITTLE endian. If the value is 0, the first byte is the most
+	//! significant byte, BIG endian. Examples:
+	//!      integer 1 on BIG endian: 00000000 00000000 00000000 00000001
+	//!   integer 1 on LITTLE endian: 00000001 00000000 00000000 00000000
+	//!---------------------------------------------------------------------------
+	//! int IsLittleEndian()	{ int x=1;	return ( ((char*)(&x))[0] );	}
+	inline_ char	LittleEndian()						{ int i = 1; return *((char*)&i);			}
+
+	//!< Alternative abs function
+	inline_ udword	abs_(sdword x)					{ sdword y= x >> 31;	return (x^y)-y;		}
+
+	//!< Alternative min function
+	inline_ sdword	min_(sdword a, sdword b)			{ sdword delta = b-a;	return a + (delta&(delta>>31));	}
+
+	// Determine if one of the bytes in a 4 byte word is zero
+	inline_	BOOL	HasNullByte(udword x)			{ return ((x + 0xfefefeff) & (~x) & 0x80808080);		}
+
+	// To find the smallest 1 bit in a word  EG: ~~~~~~10---0    =>    0----010---0
+	inline_	udword	LowestOneBit(udword w)			{ return ((w) & (~(w)+1));					}
+//	inline_	udword	LowestOneBit_(udword w)			{ return ((w) & (-(w)));					}
+
+	// Most Significant 1 Bit
+	// Given a binary integer value x, the most significant 1 bit (highest numbered element of a bit set)
+	// can be computed using a SWAR algorithm that recursively "folds" the upper bits into the lower bits.
+	// This process yields a bit vector with the same most significant 1 as x, but all 1's below it.
+	 // Bitwise AND of the original value with the complement of the "folded" value shifted down by one
+	// yields the most significant bit. For a 32-bit value: 
+	inline_ udword	msb32(udword x)
+	{
+		x |= (x >> 1);
+		x |= (x >> 2);
+		x |= (x >> 4);
+		x |= (x >> 8);
+		x |= (x >> 16);
+		return (x & ~(x >> 1));
+	}
+
+	/*
+	"Just call it repeatedly with various input values and always with the same variable as "memory".
+	The sharpness determines the degree of filtering, where 0 completely filters out the input, and 1
+	does no filtering at all.
+
+	I seem to recall from college that this is called an IIR (Infinite Impulse Response) filter. As opposed
+	to the more typical FIR (Finite Impulse Response).
+
+	Also, I'd say that you can make more intelligent and interesting filters than this, for example filters
+	that remove wrong responses from the mouse because it's being moved too fast. You'd want such a filter
+	to be applied before this one, of course."
+
+	(JCAB on Flipcode)
+	*/
+	inline_ float	FeedbackFilter(float val, float& memory, float sharpness)
+	{
+		ASSERT(sharpness>=0.0f && sharpness<=1.0f && "Invalid sharpness value in feedback filter");
+				if(sharpness<0.0f)	sharpness = 0.0f;
+		else	if(sharpness>1.0f)	sharpness = 1.0f;
+		return memory = val * sharpness + memory * (1.0f - sharpness);
+	}
+
+	//! If you can guarantee that your input domain (i.e. value of x) is slightly
+	//! limited (abs(x) must be < ((1<<31u)-32767)), then you can use the
+	//! following code to clamp the resulting value into [-32768,+32767] range:
+	inline_ int	ClampToInt16(int x)
+	{
+//		ASSERT(abs(x) < (int)((1<<31u)-32767));
+
+		int delta = 32767 - x;
+		x += (delta>>31) & delta;
+		delta = x + 32768;
+		x -= (delta>>31) & delta;
+		return x;
+	}
+
+	// Generic functions
+	template<class Type> inline_ void TSwap(Type& a, Type& b)								{ const Type c = a; a = b; b = c;			}
+	template<class Type> inline_ Type TClamp(const Type& x, const Type& lo, const Type& hi)	{ return ((x<lo) ? lo : (x>hi) ? hi : x);	}
+
+	template<class Type> inline_ void TSort(Type& a, Type& b)
+	{
+		if(a>b)	TSwap(a, b);
+	}
+
+	template<class Type> inline_ void TSort(Type& a, Type& b, Type& c)
+	{
+		if(a>b)	TSwap(a, b);
+		if(b>c)	TSwap(b, c);
+		if(a>b)	TSwap(a, b);
+		if(b>c)	TSwap(b, c);
+	}
+
+	// Prevent nasty user-manipulations (strategy borrowed from Charles Bloom)
+//	#define PREVENT_COPY(curclass)	void operator = (const curclass& object)	{	ASSERT(!"Bad use of operator =");	}
+	// ... actually this is better !
+	#define PREVENT_COPY(cur_class)	private: cur_class(const cur_class& object);	cur_class& operator=(const cur_class& object);
+
+	//! TO BE DOCUMENTED
+	#define OFFSET_OF(Class, Member)	(size_t)&(((Class*)0)->Member)
+	//! TO BE DOCUMENTED
+	#define ARRAYSIZE(p)				(sizeof(p)/sizeof(p[0]))
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	Returns the alignment of the input address.
+	 *	\fn			Alignment(udword address)
+	 *	\param		address	[in] address to check
+	 *	\return		the best alignment (e.g. 1 for odd addresses, etc)
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	FUNCTION ICECORE_API udword Alignment(udword address);
+
+	#define IS_ALIGNED_2(x)		((x&1)==0)
+	#define IS_ALIGNED_4(x)		((x&3)==0)
+	#define IS_ALIGNED_8(x)		((x&7)==0)
+
+    // ericf commented out
+	//inline_ void _prefetch(void const* ptr)		{ (void)*(char const volatile *)ptr;	}
+
+	// Compute implicit coords from an index:
+	// The idea is to get back 2D coords from a 1D index.
+	// For example:
+	//
+	// 0		1		2	...	nbu-1
+	// nbu		nbu+1	i	...
+	//
+	// We have i, we're looking for the equivalent (u=2, v=1) location.
+	//		i = u + v*nbu
+	// <=>	i/nbu = u/nbu + v
+	// Since 0 <= u < nbu, u/nbu = 0 (integer)
+	// Hence: v = i/nbu
+	// Then we simply put it back in the original equation to compute u = i - v*nbu
+	inline_ void Compute2DCoords(udword& u, udword& v, udword i, udword nbu)
+	{
+		v = i / nbu;
+		u = i - (v * nbu);
+	}
+
+	// In 3D:	i = u + v*nbu + w*nbu*nbv
+	// <=>		i/(nbu*nbv) = u/(nbu*nbv) + v/nbv + w
+	// u/(nbu*nbv) is null since u/nbu was null already.
+	// v/nbv is null as well for the same reason.
+	// Hence w = i/(nbu*nbv)
+	// Then we're left with a 2D problem: i' = i - w*nbu*nbv = u + v*nbu
+	inline_ void Compute3DCoords(udword& u, udword& v, udword& w, udword i, udword nbu, udword nbu_nbv)
+	{
+		w = i / (nbu_nbv);
+		Compute2DCoords(u, v, i - (w * nbu_nbv), nbu);
+	}
+
+#endif // __ICEUTILS_H__

src/external/open_dynamics_engine-ef/ode/LICENSE_BSD.TXT

@@ -0,0 +1,34 @@
+
+This is the BSD-style license for the Open Dynamics Engine
+----------------------------------------------------------
+
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/external/open_dynamics_engine-ef/ode/OPC_AABBCollider.cpp

@@ -0,0 +1,706 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for an AABB collider.
+ *	\file		OPC_AABBCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 1st, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains an AABB-vs-tree collider.
+ *
+ *	\class		AABBCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		January, 1st, 2002
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_BoxBoxOverlap.h"
+#include "ode/OPC_TriBoxOverlap.h"
+
+#define SET_CONTACT(prim_index, flag)						\
+	/* Set contact status */								\
+	mFlags |= flag;											\
+	mTouchedPrimitives->Add(prim_index);
+
+//! AABB-triangle test
+#define AABB_PRIM(prim_index, flag)							\
+	/* Request vertices from the app */						\
+	VertexPointers VP;	mIMesh->GetTriangle(VP, prim_index);\
+	mLeafVerts[0] = *VP.Vertex[0];							\
+	mLeafVerts[1] = *VP.Vertex[1];							\
+	mLeafVerts[2] = *VP.Vertex[2];							\
+	/* Perform triangle-box overlap test */					\
+	if(TriBoxOverlap())										\
+	{														\
+		SET_CONTACT(prim_index, flag)						\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBCollider::AABBCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBCollider::~AABBCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic collision query for generic OPCODE models. After the call, access the results:
+ *	- with GetContactStatus()
+ *	- with GetNbTouchedPrimitives()
+ *	- with GetTouchedPrimitives()
+ *
+ *	\param		cache		[in/out] a box cache
+ *	\param		box			[in] collision AABB in world space
+ *	\param		model		[in] Opcode model to collide with
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, Model& model)
+{
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, box))	return true;
+
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a collision query :
+ *	- reset stats & contact status
+ *	- check temporal coherence
+ *
+ *	\param		cache		[in/out] a box cache
+ *	\param		box			[in] AABB in world space
+ *	\return		TRUE if we can return immediately
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL AABBCollider::InitQuery(AABBCache& cache, const CollisionAABB& box)
+{
+	// 1) Call the base method
+	VolumeCollider::InitQuery();
+
+	// 2) Keep track of the query box
+	mBox = box;
+
+	// 3) Setup destination pointer
+	mTouchedPrimitives = &cache.TouchedPrimitives;
+
+	// 4) Special case: 1-triangle meshes [Opcode 1.3]
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		if(!SkipPrimitiveTests())
+		{
+			// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
+			mTouchedPrimitives->Reset();
+
+			// Perform overlap test between the unique triangle and the box (and set contact status if needed)
+			AABB_PRIM(udword(0), OPC_CONTACT)
+
+			// Return immediately regardless of status
+			return TRUE;
+		}
+	}
+
+	// 5) Check temporal coherence :
+	if(TemporalCoherenceEnabled())
+	{
+		// Here we use temporal coherence
+		// => check results from previous frame before performing the collision query
+		if(FirstContactEnabled())
+		{
+			// We're only interested in the first contact found => test the unique previously touched face
+			if(mTouchedPrimitives->GetNbEntries())
+			{
+				// Get index of previously touched face = the first entry in the array
+				udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
+
+				// Then reset the array:
+				// - if the overlap test below is successful, the index we'll get added back anyway
+				// - if it isn't, then the array should be reset anyway for the normal query
+				mTouchedPrimitives->Reset();
+
+				// Perform overlap test between the cached triangle and the box (and set contact status if needed)
+				AABB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
+
+				// Return immediately if possible
+				if(GetContactStatus())	return TRUE;
+			}
+			// else no face has been touched during previous query
+			// => we'll have to perform a normal query
+		}
+		else
+		{
+			// We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious):
+			if(IsCacheValid(cache) && mBox.IsInside(cache.FatBox))
+			{
+				// - if N is included in P, return previous list
+				// => we simply leave the list (mTouchedFaces) unchanged
+
+				// Set contact status if needed
+				if(mTouchedPrimitives->GetNbEntries())	mFlags |= OPC_TEMPORAL_CONTACT;
+
+				// In any case we don't need to do a query
+				return TRUE;
+			}
+			else
+			{
+				// - else do the query using a fat N
+
+				// Reset cache since we'll about to perform a real query
+				mTouchedPrimitives->Reset();
+
+				// Make a fat box so that coherence will work for subsequent frames
+				mBox.mExtents *= cache.FatCoeff;
+
+				// Update cache with query data (signature for cached faces)
+				cache.FatBox = mBox;
+			}
+		}
+	}
+	else
+	{
+		// Here we don't use temporal coherence => do a normal query
+		mTouchedPrimitives->Reset();
+	}
+
+	// 5) Precompute min & max bounds if needed
+	mMin = box.mCenter - box.mExtents;
+	mMax = box.mCenter + box.mExtents;
+
+	return FALSE;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for vanilla AABB trees.
+ *	\param		cache		[in/out] a box cache
+ *	\param		box			[in] collision AABB in world space
+ *	\param		tree		[in] AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const AABBTree* tree)
+{
+	// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
+	// So we don't really have "primitives" to deal with. Hence it doesn't work with
+	// "FirstContact" + "TemporalCoherence".
+	ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
+
+	// Checkings
+	if(!tree)	return false;
+
+	// Init collision query
+	if(InitQuery(cache, box))	return true;
+
+	// Perform collision query
+	_Collide(tree);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the AABB completely contains the box. In which case we can end the query sooner.
+ *	\param		bc	[in] box center
+ *	\param		be	[in] box extents
+ *	\return		true if the AABB contains the whole box
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL AABBCollider::AABBContainsBox(const Point& bc, const Point& be)
+{
+	if(mMin.x > bc.x - be.x)	return FALSE;
+	if(mMin.y > bc.y - be.y)	return FALSE;
+	if(mMin.z > bc.z - be.z)	return FALSE;
+
+	if(mMax.x < bc.x + be.x)	return FALSE;
+	if(mMax.y < bc.y + be.y)	return FALSE;
+	if(mMax.z < bc.z + be.z)	return FALSE;
+
+	return TRUE;
+}
+
+#define TEST_BOX_IN_AABB(center, extents)	\
+	if(AABBContainsBox(center, extents))	\
+	{										\
+		/* Set contact status */			\
+		mFlags |= OPC_CONTACT;				\
+		_Dump(node);						\
+		return;								\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_Collide(const AABBCollisionNode* node)
+{
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		AABB_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
+{
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_Collide(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_AABB(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		AABB_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_AABB(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_Collide(const AABBNoLeafNode* node)
+{
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ AABB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ AABB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
+{
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_Collide(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_AABB(Center, Extents)
+
+	if(node->HasPosLeaf())	{ AABB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ AABB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform AABB-AABB overlap test
+	if(!AABBAABBOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_AABB(Center, Extents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for vanilla AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBCollider::_Collide(const AABBTreeNode* node)
+{
+	// Perform AABB-AABB overlap test
+	Point Center, Extents;
+	node->GetAABB()->GetCenter(Center);
+	node->GetAABB()->GetExtents(Extents);
+	if(!AABBAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf() || AABBContainsBox(Center, Extents))
+	{
+		mFlags |= OPC_CONTACT;
+		mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
+	}
+	else
+	{
+		_Collide(node->GetPos());
+		_Collide(node->GetNeg());
+	}
+}
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridAABBCollider::HybridAABBCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridAABBCollider::~HybridAABBCollider()
+{
+}
+
+//bool HybridAABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const HybridModel& model)
+// ericf change
+bool HybridAABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, HybridModel& model)
+{
+	// We don't want primitive tests here!
+	mFlags |= OPC_NO_PRIMITIVE_TESTS;
+
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, box))	return true;
+
+	// Special case for 1-leaf trees
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
+		udword Nb = mIMesh->GetNbTriangles();
+
+		// Loop through all triangles
+		for(udword i=0;i<Nb;i++)
+		{
+			AABB_PRIM(i, OPC_CONTACT)
+		}
+		return true;
+	}
+
+	// Override destination array since we're only going to get leaf boxes here
+	mTouchedBoxes.Reset();
+	mTouchedPrimitives = &mTouchedBoxes;
+
+	// Now, do the actual query against leaf boxes
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+
+	// We only have a list of boxes so far
+	if(GetContactStatus())
+	{
+		// Reset contact status, since it currently only reflects collisions with leaf boxes
+		Collider::InitQuery();
+
+		// Change dest container so that we can use built-in overlap tests and get collided primitives
+		cache.TouchedPrimitives.Reset();
+		mTouchedPrimitives = &cache.TouchedPrimitives;
+
+		// Read touched leaf boxes
+		udword Nb = mTouchedBoxes.GetNbEntries();
+		const udword* Touched = mTouchedBoxes.GetEntries();
+
+		const LeafTriangles* LT = model.GetLeafTriangles();
+		const udword* Indices = model.GetIndices();
+
+		// Loop through touched leaves
+		while(Nb--)
+		{
+			const LeafTriangles& CurrentLeaf = LT[*Touched++];
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = *T++;
+					AABB_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = BaseIndex++;
+					AABB_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_AABBCollider.h

@@ -0,0 +1,101 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for an AABB collider.
+ *	\file		OPC_AABBCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 1st, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_AABBCOLLIDER_H__
+#define __OPC_AABBCOLLIDER_H__
+
+	struct OPCODE_API AABBCache : VolumeCache
+	{
+						AABBCache() : FatCoeff(1.1f)
+						{
+							FatBox.mCenter.Zero();
+							FatBox.mExtents.Zero();
+						}
+
+		// Cached faces signature
+		CollisionAABB	FatBox;		//!< Box used when performing the query resulting in cached faces
+		// User settings
+		float			FatCoeff;	//!< mRadius2 multiplier used to create a fat sphere
+	};
+
+	class OPCODE_API AABBCollider : public VolumeCollider
+	{
+		public:
+		// Constructor / Destructor
+											AABBCollider();
+		virtual								~AABBCollider();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic collision query for generic OPCODE models. After the call, access the results:
+		 *	- with GetContactStatus()
+		 *	- with GetNbTouchedPrimitives()
+		 *	- with GetTouchedPrimitives()
+		 *
+		 *	\param		cache			[in/out] a box cache
+		 *	\param		box				[in] collision AABB in world space
+		 *	\param		model			[in] Opcode model to collide with
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        //bool			Collide(AABBCache& cache, const CollisionAABB& box, const Model& model);
+        // ericf change
+							bool			Collide(AABBCache& cache, const CollisionAABB& box, Model& model);
+		//
+							bool			Collide(AABBCache& cache, const CollisionAABB& box, const AABBTree* tree);
+		protected:
+							CollisionAABB	mBox;			//!< Query box in (center, extents) form
+							Point			mMin;			//!< Query box min point
+							Point			mMax;			//!< Query box max point
+		// Leaf description
+							Point			mLeafVerts[3];	//!< Triangle vertices
+		// Internal methods
+							void			_Collide(const AABBCollisionNode* node);
+							void			_Collide(const AABBNoLeafNode* node);
+							void			_Collide(const AABBQuantizedNode* node);
+							void			_Collide(const AABBQuantizedNoLeafNode* node);
+							void			_Collide(const AABBTreeNode* node);
+							void			_CollideNoPrimitiveTest(const AABBCollisionNode* node);
+							void			_CollideNoPrimitiveTest(const AABBNoLeafNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
+			// Overlap tests
+		inline_				BOOL			AABBContainsBox(const Point& bc, const Point& be);
+		inline_				BOOL			AABBAABBOverlap(const Point& b, const Point& Pb);
+		inline_				BOOL			TriBoxOverlap();
+			// Init methods
+							BOOL			InitQuery(AABBCache& cache, const CollisionAABB& box);
+	};
+
+	class OPCODE_API HybridAABBCollider : public AABBCollider
+	{
+		public:
+		// Constructor / Destructor
+											HybridAABBCollider();
+		virtual								~HybridAABBCollider();
+
+        //bool			Collide(AABBCache& cache, const CollisionAABB& box, const HybridModel& model);
+        // ericf change
+							bool			Collide(AABBCache& cache, const CollisionAABB& box, HybridModel& model);
+		protected:
+							Container		mTouchedBoxes;
+	};
+
+#endif // __OPC_AABBCOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_AABBTree.cpp

@@ -0,0 +1,581 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a versatile AABB tree.
+ *	\file		OPC_AABBTree.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a generic AABB tree node.
+ *
+ *	\class		AABBTreeNode
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a generic AABB tree.
+ *	This is a vanilla AABB tree, without any particular optimization. It contains anonymous references to
+ *	user-provided primitives, which can theoretically be anything - triangles, boxes, etc. Each primitive
+ *	is surrounded by an AABB, regardless of the primitive's nature. When the primitive is a triangle, the
+ *	resulting tree can be converted into an optimized tree. If the primitive is a box, the resulting tree
+ *	can be used for culling - VFC or occlusion -, assuming you cull on a mesh-by-mesh basis (modern way).
+ *
+ *	\class		AABBTree
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBTreeNode::AABBTreeNode() :
+	mPos			(null),
+#ifndef OPC_NO_NEG_VANILLA_TREE
+	mNeg			(null),
+#endif
+	mNodePrimitives	(null),
+	mNbPrimitives	(0)
+{
+#ifdef OPC_USE_TREE_COHERENCE
+	mBitmask = 0;
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBTreeNode::~AABBTreeNode()
+{
+	// Opcode 1.3:
+	const AABBTreeNode* Pos = GetPos();
+#ifndef OPC_NO_NEG_VANILLA_TREE
+	const AABBTreeNode* Neg = GetNeg();
+	if(!(mPos&1))	DELETESINGLE(Pos);
+	if(!(mNeg&1))	DELETESINGLE(Neg);
+#else
+	if(!(mPos&1))	DELETEARRAY(Pos);
+#endif
+	mNodePrimitives	= null;	// This was just a shortcut to the global list => no release
+	mNbPrimitives	= 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Splits the node along a given axis.
+ *	The list of indices is reorganized according to the split values.
+ *	\param		axis		[in] splitting axis index
+ *	\param		builder		[in] the tree builder
+ *	\return		the number of primitives assigned to the first child
+ *	\warning	this method reorganizes the internal list of primitives
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder)
+{
+	// Get node split value
+	float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis);
+
+	udword NbPos = 0;
+	// Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1].
+	// Those indices map the global list in the tree builder.
+	for(udword i=0;i<mNbPrimitives;i++)
+	{
+		// Get index in global list
+		udword Index = mNodePrimitives[i];
+
+		// Test against the splitting value. The primitive value is tested against the enclosing-box center.
+		// [We only need an approximate partition of the enclosing box here.]
+		float PrimitiveValue = builder->GetSplittingValue(Index, axis);
+
+		// Reorganize the list of indices in this order: positive - negative.
+		if(PrimitiveValue > SplitValue)
+		{
+			// Swap entries
+			udword Tmp = mNodePrimitives[i];
+			mNodePrimitives[i] = mNodePrimitives[NbPos];
+			mNodePrimitives[NbPos] = Tmp;
+			// Count primitives assigned to positive space
+			NbPos++;
+		}
+	}
+	return NbPos;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Subdivides the node.
+ *
+ *	          N
+ *	        /   \
+ *	      /       \
+ *	   N/2         N/2
+ *	  /   \       /   \
+ *	N/4   N/4   N/4   N/4
+ *	(etc)
+ *
+ *	A well-balanced tree should have a O(log n) depth.
+ *	A degenerate tree would have a O(n) depth.
+ *	Note a perfectly-balanced tree is not well-suited to collision detection anyway.
+ *
+ *	\param		builder		[in] the tree builder
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder)
+{
+	// Checkings
+	if(!builder)	return false;
+
+	// Stop subdividing if we reach a leaf node. This is always performed here,
+	// else we could end in trouble if user overrides this.
+	if(mNbPrimitives==1)	return true;
+
+	// Let the user validate the subdivision
+	if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV))	return true;
+
+	bool ValidSplit = true;	// Optimism...
+	udword NbPos = 0;
+	if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS)
+	{
+		// Find the largest axis to split along
+		Point Extents;	mBV.GetExtents(Extents);	// Box extents
+		udword Axis	= Extents.LargestAxis();		// Index of largest axis
+
+		// Split along the axis
+		NbPos = Split(Axis, builder);
+
+		// Check split validity
+		if(!NbPos || NbPos==mNbPrimitives)	ValidSplit = false;
+	}
+	else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS)
+	{
+		// Compute the means
+		Point Means(0.0f, 0.0f, 0.0f);
+		for(udword i=0;i<mNbPrimitives;i++)
+		{
+			udword Index = mNodePrimitives[i];
+			Means.x+=builder->GetSplittingValue(Index, 0);
+			Means.y+=builder->GetSplittingValue(Index, 1);
+			Means.z+=builder->GetSplittingValue(Index, 2);
+		}
+		Means/=float(mNbPrimitives);
+
+		// Compute variances
+		Point Vars(0.0f, 0.0f, 0.0f);
+		for(udword i=0;i<mNbPrimitives;i++)
+		{
+			udword Index = mNodePrimitives[i];
+			float Cx = builder->GetSplittingValue(Index, 0);
+			float Cy = builder->GetSplittingValue(Index, 1);
+			float Cz = builder->GetSplittingValue(Index, 2);
+			Vars.x += (Cx - Means.x)*(Cx - Means.x);
+			Vars.y += (Cy - Means.y)*(Cy - Means.y);
+			Vars.z += (Cz - Means.z)*(Cz - Means.z);
+		}
+		Vars/=float(mNbPrimitives-1);
+
+		// Choose axis with greatest variance
+		udword Axis = Vars.LargestAxis();
+
+		// Split along the axis
+		NbPos = Split(Axis, builder);
+
+		// Check split validity
+		if(!NbPos || NbPos==mNbPrimitives)	ValidSplit = false;
+	}
+	else if(builder->mSettings.mRules & SPLIT_BALANCED)
+	{
+		// Test 3 axis, take the best
+		float Results[3];
+		NbPos = Split(0, builder);	Results[0] = float(NbPos)/float(mNbPrimitives);
+		NbPos = Split(1, builder);	Results[1] = float(NbPos)/float(mNbPrimitives);
+		NbPos = Split(2, builder);	Results[2] = float(NbPos)/float(mNbPrimitives);
+		Results[0]-=0.5f;	Results[0]*=Results[0];
+		Results[1]-=0.5f;	Results[1]*=Results[1];
+		Results[2]-=0.5f;	Results[2]*=Results[2];
+		udword Min=0;
+		if(Results[1]<Results[Min])	Min = 1;
+		if(Results[2]<Results[Min])	Min = 2;
+
+		// Split along the axis
+		NbPos = Split(Min, builder);
+
+		// Check split validity
+		if(!NbPos || NbPos==mNbPrimitives)	ValidSplit = false;
+	}
+	else if(builder->mSettings.mRules & SPLIT_BEST_AXIS)
+	{
+		// Test largest, then middle, then smallest axis...
+
+		// Sort axis
+		Point Extents;	mBV.GetExtents(Extents);	// Box extents
+		udword SortedAxis[] = { 0, 1, 2 };
+		float* Keys = (float*)&Extents.x;
+		for(udword j=0;j<3;j++)
+		{
+			for(udword i=0;i<2;i++)
+			{
+				if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
+				{
+					udword Tmp = SortedAxis[i];
+					SortedAxis[i] = SortedAxis[i+1];
+					SortedAxis[i+1] = Tmp;
+				}
+			}
+		}
+
+		// Find the largest axis to split along
+		udword CurAxis = 0;
+		ValidSplit = false;
+		while(!ValidSplit && CurAxis!=3)
+		{
+			NbPos = Split(SortedAxis[CurAxis], builder);
+			// Check the subdivision has been successful
+			if(!NbPos || NbPos==mNbPrimitives)	CurAxis++;
+			else								ValidSplit = true;
+		}
+	}
+	else if(builder->mSettings.mRules & SPLIT_FIFTY)
+	{
+		// Don't even bother splitting (mainly a performance test)
+		NbPos = mNbPrimitives>>1;
+	}
+	else return false;	// Unknown splitting rules
+
+	// Check the subdivision has been successful
+	if(!ValidSplit)
+	{
+		// Here, all boxes lie in the same sub-space. Two strategies:
+		// - if the tree *must* be complete, make an arbitrary 50-50 split
+		// - else stop subdividing
+//		if(builder->mSettings.mRules&SPLIT_COMPLETE)
+		if(builder->mSettings.mLimit==1)
+		{
+			builder->IncreaseNbInvalidSplits();
+			NbPos = mNbPrimitives>>1;
+		}
+		else return true;
+	}
+
+	// Now create children and assign their pointers.
+	if(builder->mNodeBase)
+	{
+		// We use a pre-allocated linear pool for complete trees [Opcode 1.3]
+		AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase;
+		udword Count = builder->GetCount() - 1;	// Count begins to 1...
+		// Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives
+		ASSERT(!(udword(&Pool[Count+0])&1));
+		ASSERT(!(udword(&Pool[Count+1])&1));
+		mPos = size_t(&Pool[Count+0])|1;
+#ifndef OPC_NO_NEG_VANILLA_TREE
+		mNeg = size_t(&Pool[Count+1])|1;
+#endif
+	}
+	else
+	{
+		// Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly
+#ifndef OPC_NO_NEG_VANILLA_TREE
+		mPos = (size_t)new AABBTreeNode;	CHECKALLOC(mPos);
+		mNeg = (size_t)new AABBTreeNode;	CHECKALLOC(mNeg);
+#else
+		AABBTreeNode* PosNeg = new AABBTreeNode[2];
+		CHECKALLOC(PosNeg);
+		mPos = (size_t)PosNeg;
+#endif
+	}
+
+	// Update stats
+	builder->IncreaseCount(2);
+
+	// Assign children
+	AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
+	AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
+	Pos->mNodePrimitives	= &mNodePrimitives[0];
+	Pos->mNbPrimitives		= NbPos;
+	Neg->mNodePrimitives	= &mNodePrimitives[NbPos];
+	Neg->mNbPrimitives		= mNbPrimitives - NbPos;
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive hierarchy building in a top-down fashion.
+ *	\param		builder		[in] the tree builder
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder)
+{
+	// 1) Compute the global box for current node. The box is stored in mBV.
+	builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
+
+	// 2) Subdivide current node
+	Subdivide(builder);
+
+	// 3) Recurse
+	AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
+	AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
+	if(Pos)	Pos->_BuildHierarchy(builder);
+	if(Neg)	Neg->_BuildHierarchy(builder);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the tree (top-down).
+ *	\param		builder		[in] the tree builder
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeNode::_Refit(AABBTreeBuilder* builder)
+{
+	// 1) Recompute the new global box for current node
+	builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
+
+	// 2) Recurse
+	AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
+	AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
+	if(Pos)	Pos->_Refit(builder);
+	if(Neg)	Neg->_Refit(builder);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBTree::AABBTree() : mIndices(null), mPool(null),mTotalNbNodes(0)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBTree::~AABBTree()
+{
+	Release();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Releases the tree.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTree::Release()
+{
+	DELETEARRAY(mPool);
+	DELETEARRAY(mIndices);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds a generic AABB tree from a tree builder.
+ *	\param		builder		[in] the tree builder
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTree::Build(AABBTreeBuilder* builder)
+{
+	// Checkings
+	if(!builder || !builder->mNbPrimitives)	return false;
+
+	// Release previous tree
+	Release();
+
+	// Init stats
+	builder->SetCount(1);
+	builder->SetNbInvalidSplits(0);
+
+	// Initialize indices. This list will be modified during build.
+	mIndices = new udword[builder->mNbPrimitives];
+	CHECKALLOC(mIndices);
+	// Identity permutation
+	for(udword i=0;i<builder->mNbPrimitives;i++)	mIndices[i] = i;
+
+	// Setup initial node. Here we have a complete permutation of the app's primitives.
+	mNodePrimitives	= mIndices;
+	mNbPrimitives	= builder->mNbPrimitives;
+
+	// Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3]
+//	if(builder->mRules&SPLIT_COMPLETE)
+	if(builder->mSettings.mLimit==1)
+	{
+		// Allocate a pool of nodes
+		mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1];
+
+		builder->mNodeBase = mPool;	// ### ugly !
+	}
+
+	// Build the hierarchy
+	_BuildHierarchy(builder);
+
+	// Get back total number of nodes
+	mTotalNbNodes	= builder->GetCount();
+
+	// For complete trees, check the correct number of nodes has been created [Opcode 1.3]
+	if(mPool)	ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the depth of the tree.
+ *	A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth.
+ *	\return		depth of the tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword AABBTree::ComputeDepth() const
+{
+	return Walk(null, null);	// Use the walking code without callback
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Walks the tree, calling the user back for each node.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword AABBTree::Walk(WalkingCallback callback, void* user_data) const
+{
+	// Call it without callback to compute max depth
+	udword MaxDepth = 0;
+	udword CurrentDepth = 0;
+
+	struct Local
+	{
+		static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data)
+		{
+			// Checkings
+			if(!current_node)	return;
+			// Entering a new node => increase depth
+			current_depth++;
+			// Keep track of max depth
+			if(current_depth>max_depth)	max_depth = current_depth;
+
+			// Callback
+			if(callback && !(callback)(current_node, current_depth, user_data))	return;
+
+			// Recurse
+			if(current_node->GetPos())	{ _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data);	current_depth--;	}
+			if(current_node->GetNeg())	{ _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data);	current_depth--;	}
+		}
+	};
+	Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data);
+	return MaxDepth;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the tree in a top-down way.
+ *	\param		builder		[in] the tree builder
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTree::Refit(AABBTreeBuilder* builder)
+{
+	if(!builder)	return false;
+	_Refit(builder);
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the tree in a bottom-up way.
+ *	\param		builder		[in] the tree builder
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTree::Refit2(AABBTreeBuilder* builder)
+{
+	// Checkings
+	if(!builder)	return false;
+
+	ASSERT(mPool);
+
+	// Bottom-up update
+	Point Min,Max;
+	Point Min_,Max_;
+	udword Index = mTotalNbNodes;
+	while(Index--)
+	{
+		AABBTreeNode& Current = mPool[Index];
+
+		if(Current.IsLeaf())
+		{
+			builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB());
+		}
+		else
+		{
+			Current.GetPos()->GetAABB()->GetMin(Min);
+			Current.GetPos()->GetAABB()->GetMax(Max);
+
+			Current.GetNeg()->GetAABB()->GetMin(Min_);
+			Current.GetNeg()->GetAABB()->GetMax(Max_);
+
+			Min.Min(Min_);
+			Max.Max(Max_);
+
+			((AABB*)Current.GetAABB())->SetMinMax(Min, Max);
+		}
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the number of bytes used by the tree.
+ *	\return		number of bytes used
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword AABBTree::GetUsedBytes() const
+{
+	udword TotalSize = mTotalNbNodes*GetNodeSize();
+	if(mIndices)	TotalSize+=mNbPrimitives*sizeof(udword);
+	return TotalSize;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the tree is a complete tree or not.
+ *	A complete tree is made of 2*N-1 nodes, where N is the number of primitives in the tree.
+ *	\return		true for complete trees
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTree::IsComplete() const
+{
+	return (GetNbNodes()==GetNbPrimitives()*2-1);
+}

src/external/open_dynamics_engine-ef/ode/OPC_AABBTree.h

@@ -0,0 +1,137 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a versatile AABB tree.
+ *	\file		OPC_AABBTree.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_AABBTREE_H__
+#define __OPC_AABBTREE_H__
+
+#ifdef OPC_NO_NEG_VANILLA_TREE
+	//! TO BE DOCUMENTED
+	#define IMPLEMENT_TREE(base_class, volume)																			\
+		public:																											\
+		/* Constructor / Destructor */																					\
+									base_class();																		\
+									~base_class();																		\
+		/* Data access */																								\
+		inline_	const volume*		Get##volume()	const	{ return &mBV;							}					\
+		/* Clear the last bit */																						\
+		inline_	const base_class*	GetPos()		const	{ return (const base_class*)(mPos&~1);	}					\
+		inline_	const base_class*	GetNeg()		const	{ const base_class* P = GetPos(); return P ? P+1 : null;}	\
+																														\
+		/* We don't need to test both nodes since we can't have one without the other	*/								\
+		inline_	bool				IsLeaf()		const	{ return !GetPos();						}					\
+																														\
+		/* Stats */																										\
+		inline_	udword				GetNodeSize()	const	{ return SIZEOFOBJECT;					}					\
+		protected:																										\
+		/* Tree-independent data */																						\
+		/* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/				\
+		/* Whatever happens we need the two children and the enclosing volume.*/										\
+				volume				mBV;		/* Global bounding-volume enclosing all the node-related primitives */	\
+				size_t				mPos;		/* "Positive" & "Negative" children */
+#else
+	//! TO BE DOCUMENTED
+	#define IMPLEMENT_TREE(base_class, volume)																			\
+		public:																											\
+		/* Constructor / Destructor */																					\
+									base_class();																		\
+									~base_class();																		\
+		/* Data access */																								\
+		inline_	const volume*		Get##volume()	const	{ return &mBV;							}					\
+		/* Clear the last bit */																						\
+		inline_	const base_class*	GetPos()		const	{ return (const base_class*)(mPos&~1);	}					\
+		inline_	const base_class*	GetNeg()		const	{ return (const base_class*)(mNeg&~1);	}					\
+																														\
+/*		inline_	bool				IsLeaf()		const	{ return (!GetPos() && !GetNeg());	}	*/					\
+		/* We don't need to test both nodes since we can't have one without the other	*/								\
+		inline_	bool				IsLeaf()		const	{ return !GetPos();						}					\
+																														\
+		/* Stats */																										\
+		inline_	udword				GetNodeSize()	const	{ return SIZEOFOBJECT;					}					\
+		protected:																										\
+		/* Tree-independent data */																						\
+		/* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/				\
+		/* Whatever happens we need the two children and the enclosing volume.*/										\
+				volume				mBV;		/* Global bounding-volume enclosing all the node-related primitives */	\
+				size_t				mPos;		/* "Positive" child */													\
+				size_t				mNeg;		/* "Negative" child */
+#endif
+
+	typedef		void				(*CullingCallback)		(udword nb_primitives, udword* node_primitives, BOOL need_clipping, void* user_data);
+
+	class OPCODE_API AABBTreeNode
+	{
+									IMPLEMENT_TREE(AABBTreeNode, AABB)
+		public:
+		// Data access
+		inline_	const udword*		GetPrimitives()		const	{ return mNodePrimitives;	}
+		inline_	udword				GetNbPrimitives()	const	{ return mNbPrimitives;		}
+
+		protected:
+		// Tree-dependent data
+				udword*				mNodePrimitives;	//!< Node-related primitives (shortcut to a position in mIndices below)
+				udword				mNbPrimitives;		//!< Number of primitives for this node
+		// Internal methods
+				udword				Split(udword axis, AABBTreeBuilder* builder);
+				bool				Subdivide(AABBTreeBuilder* builder);
+				void				_BuildHierarchy(AABBTreeBuilder* builder);
+				void				_Refit(AABBTreeBuilder* builder);
+	};
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	User-callback, called for each node by the walking code.
+	 *	\param		current		[in] current node
+	 *	\param		depth		[in] current node's depth
+	 *	\param		user_data	[in] user-defined data
+	 *	\return		true to recurse through children, else false to bypass them
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	typedef		bool				(*WalkingCallback)	(const AABBTreeNode* current, udword depth, void* user_data);
+
+	class OPCODE_API AABBTree : public AABBTreeNode
+	{
+		public:
+		// Constructor / Destructor
+									AABBTree();
+									~AABBTree();
+		// Build
+				bool				Build(AABBTreeBuilder* builder);
+				void				Release();
+
+		// Data access
+		inline_	const udword*		GetIndices()		const	{ return mIndices;		}	//!< Catch the indices
+		inline_	udword				GetNbNodes()		const	{ return mTotalNbNodes;	}	//!< Catch the number of nodes
+
+		// Infos
+				bool				IsComplete()		const;
+		// Stats
+				udword				ComputeDepth()		const;
+				udword				GetUsedBytes()		const;
+				udword				Walk(WalkingCallback callback, void* user_data) const;
+
+				bool				Refit(AABBTreeBuilder* builder);
+				bool				Refit2(AABBTreeBuilder* builder);
+		private:
+				udword*				mIndices;			//!< Indices in the app list. Indices are reorganized during build (permutation).
+				AABBTreeNode*		mPool;				//!< Linear pool of nodes for complete trees. Null otherwise. [Opcode 1.3]
+		// Stats
+				udword				mTotalNbNodes;		//!< Number of nodes in the tree.
+	};
+
+#endif // __OPC_AABBTREE_H__

src/external/open_dynamics_engine-ef/ode/OPC_BaseModel.cpp

@@ -0,0 +1,146 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains base model interface.
+ *	\file		OPC_BaseModel.cpp
+ *	\author		Pierre Terdiman
+ *	\date		May, 18, 2003
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	The base class for collision models.
+ *
+ *	\class		BaseModel
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		May, 18, 2003
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+OPCODECREATE::OPCODECREATE()
+{
+	mIMesh				= null;
+	mSettings.mRules	= SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
+	mSettings.mLimit	= 1;	// Mandatory for complete trees
+	mNoLeaf				= true;
+	mQuantized			= true;
+#ifdef __MESHMERIZER_H__
+	mCollisionHull		= false;
+#endif // __MESHMERIZER_H__
+	mKeepOriginal		= false;
+	mCanRemap			= false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BaseModel::BaseModel() : mIMesh(null), mModelCode(0), mSource(null), mTree(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BaseModel::~BaseModel()
+{
+	ReleaseBase();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Releases everything.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void BaseModel::ReleaseBase()
+{
+	DELETESINGLE(mSource);
+	DELETESINGLE(mTree);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Creates an optimized tree according to user-settings, and setups mModelCode.
+ *	\param		no_leaf		[in] true for "no leaf" tree
+ *	\param		quantized	[in] true for quantized tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool BaseModel::CreateTree(bool no_leaf, bool quantized)
+{
+	DELETESINGLE(mTree);
+
+	// Setup model code
+	if(no_leaf)		mModelCode |= OPC_NO_LEAF;
+	else			mModelCode &= ~OPC_NO_LEAF;
+
+	if(quantized)	mModelCode |= OPC_QUANTIZED;
+	else			mModelCode &= ~OPC_QUANTIZED;
+
+	// Create the correct class
+	if(mModelCode & OPC_NO_LEAF)
+	{
+		if(mModelCode & OPC_QUANTIZED)	mTree = new AABBQuantizedNoLeafTree;
+		else							mTree = new AABBNoLeafTree;
+	}
+	else
+	{
+		if(mModelCode & OPC_QUANTIZED)	mTree = new AABBQuantizedTree;
+		else							mTree = new AABBCollisionTree;
+	}
+	CHECKALLOC(mTree);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the collision model. This can be used to handle dynamic meshes. Usage is:
+ *	1. modify your mesh vertices (keep the topology constant!)
+ *	2. refit the tree (call this method)
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool BaseModel::Refit()
+{
+	// Refit the optimized tree
+	return mTree->Refit(mIMesh);
+
+// Old code kept for reference : refit the source tree then rebuild !
+//	if(!mSource)	return false;
+//	// Ouch...
+//	mSource->Refit(&mTB);
+//	// Ouch...
+//	return mTree->Build(mSource);
+}

src/external/open_dynamics_engine-ef/ode/OPC_BaseModel.h

@@ -0,0 +1,175 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains base model interface.
+ *	\file		OPC_BaseModel.h
+ *	\author		Pierre Terdiman
+ *	\date		May, 18, 2003
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_BASEMODEL_H__
+#define __OPC_BASEMODEL_H__
+
+	//! Model creation structure
+	struct OPCODE_API OPCODECREATE
+	{
+		//! Constructor
+								OPCODECREATE();
+
+		MeshInterface*			mIMesh;			//!< Mesh interface (access to triangles & vertices) (*)
+		BuildSettings			mSettings;		//!< Builder's settings
+		bool					mNoLeaf;		//!< true => discard leaf nodes (else use a normal tree)
+		bool					mQuantized;		//!< true => quantize the tree (else use a normal tree)
+#ifdef __MESHMERIZER_H__
+		bool					mCollisionHull;	//!< true => use convex hull + GJK
+#endif // __MESHMERIZER_H__
+		bool					mKeepOriginal;	//!< true => keep a copy of the original tree (debug purpose)
+		bool					mCanRemap;		//!< true => allows OPCODE to reorganize client arrays
+
+		// (*) This pointer is saved internally and used by OPCODE until collision structures are released,
+		// so beware of the object's lifetime.
+	};
+
+	enum ModelFlag
+	{
+		OPC_QUANTIZED	= (1<<0),	//!< Compressed/uncompressed tree
+		OPC_NO_LEAF		= (1<<1),	//!< Leaf/NoLeaf tree
+		OPC_SINGLE_NODE	= (1<<2)	//!< Special case for 1-node models
+	};
+
+	class OPCODE_API BaseModel
+	{
+		public:
+		// Constructor/Destructor
+											BaseModel();
+		virtual								~BaseModel();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Builds a collision model.
+		 *	\param		create		[in] model creation structure
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual			bool				Build(const OPCODECREATE& create)	= 0;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the number of bytes used by the tree.
+		 *	\return		amount of bytes used
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual			udword				GetUsedBytes()		const			= 0;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Refits the collision model. This can be used to handle dynamic meshes. Usage is:
+		 *	1. modify your mesh vertices (keep the topology constant!)
+		 *	2. refit the tree (call this method)
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual			bool				Refit();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the source tree.
+		 *	\return		generic tree
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	const	AABBTree*			GetSourceTree()		const	{ return mSource;						}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the tree.
+		 *	\return		the collision tree
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	const	AABBOptimizedTree*	GetTree()			const	{ return mTree;							}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the tree.
+		 *	\return		the collision tree
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			AABBOptimizedTree*	GetTree()					{ return mTree;							}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the number of nodes in the tree.
+		 *	Should be 2*N-1 for normal trees and N-1 for optimized ones.
+		 *	\return		number of nodes
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			udword				GetNbNodes()		const	{ return mTree->GetNbNodes();			}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks whether the tree has leaf nodes or not.
+		 *	\return		true if the tree has leaf nodes (normal tree), else false (optimized tree)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL				HasLeafNodes()		const	{ return !(mModelCode & OPC_NO_LEAF);	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks whether the tree is quantized or not.
+		 *	\return		true if the tree is quantized
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL				IsQuantized()		const	{ return mModelCode & OPC_QUANTIZED;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks whether the model has a single node or not. This special case must be handled separately.
+		 *	\return		true if the model has only 1 node
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			BOOL				HasSingleNode()		const	{ return mModelCode & OPC_SINGLE_NODE;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the model's code.
+		 *	\return		model's code
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			udword				GetModelCode()		const	{ return mModelCode;					}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the mesh interface.
+		 *	\return		mesh interface
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	MeshInterface*		GetMeshInterface()						{ return mIMesh;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Sets the mesh interface.
+		 *	\param		imesh		[in] mesh interface
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_			void				SetMeshInterface(MeshInterface* imesh)	{ mIMesh = imesh;	}
+
+		protected:
+				MeshInterface*		mIMesh;			//!< User-defined mesh interface
+						udword				mModelCode;		//!< Model code = combination of ModelFlag(s)
+						AABBTree*			mSource;		//!< Original source tree
+						AABBOptimizedTree*	mTree;			//!< Optimized tree owned by the model
+		// Internal methods
+						void				ReleaseBase();
+						bool				CreateTree(bool no_leaf, bool quantized);
+	};
+
+#endif //__OPC_BASEMODEL_H__

src/external/open_dynamics_engine-ef/ode/OPC_BoxBoxOverlap.h

@@ -0,0 +1,122 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	OBB-OBB overlap test using the separating axis theorem.
+ *	- original code by Gomez / Gamasutra (similar to Gottschalk's one in RAPID)
+ *	- optimized for AABB trees by computing the rotation matrix once (SOLID-fashion)
+ *	- the fabs matrix is precomputed as well and epsilon-tweaked (RAPID-style, we found this almost mandatory)
+ *	- Class III axes can be disabled... (SOLID & Intel fashion)
+ *	- ...or enabled to perform some profiling
+ *	- CPU comparisons used when appropriate
+ *	- lazy evaluation sometimes saves some work in case of early exits (unlike SOLID)
+ *
+ *	\param		ea	[in] extents from box A
+ *	\param		ca	[in] center from box A
+ *	\param		eb	[in] extents from box B
+ *	\param		cb	[in] center from box B
+ *	\return		true if boxes overlap
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb)
+{
+	// Stats
+	mNbBVBVTests++;
+
+	float t,t2;
+
+	// Class I : A's basis vectors
+	float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x;
+	t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0];
+	if(GREATER(Tx, t))	return FALSE;
+
+	float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y;
+	t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1];
+	if(GREATER(Ty, t))	return FALSE;
+
+	float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z;
+	t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2];
+	if(GREATER(Tz, t))	return FALSE;
+
+	// Class II : B's basis vectors
+	t = Tx*mR1to0.m[0][0] + Ty*mR1to0.m[0][1] + Tz*mR1to0.m[0][2];	t2 = ea.x*mAR.m[0][0] + ea.y*mAR.m[0][1] + ea.z*mAR.m[0][2] + eb.x;
+	if(GREATER(t, t2))	return FALSE;
+
+	t = Tx*mR1to0.m[1][0] + Ty*mR1to0.m[1][1] + Tz*mR1to0.m[1][2];	t2 = ea.x*mAR.m[1][0] + ea.y*mAR.m[1][1] + ea.z*mAR.m[1][2] + eb.y;
+	if(GREATER(t, t2))	return FALSE;
+
+	t = Tx*mR1to0.m[2][0] + Ty*mR1to0.m[2][1] + Tz*mR1to0.m[2][2];	t2 = ea.x*mAR.m[2][0] + ea.y*mAR.m[2][1] + ea.z*mAR.m[2][2] + eb.z;
+	if(GREATER(t, t2))	return FALSE;
+
+	// Class III : 9 cross products
+	// Cool trick: always perform the full test for first level, regardless of settings.
+	// That way pathological cases (such as the pencils scene) are quickly rejected anyway !
+	if(mFullBoxBoxTest || mNbBVBVTests==1)
+	{
+		t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2];	t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0];	if(GREATER(t, t2))	return FALSE;	// L = A0 x B0
+		t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2];	t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0];	if(GREATER(t, t2))	return FALSE;	// L = A0 x B1
+		t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2];	t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0];	if(GREATER(t, t2))	return FALSE;	// L = A0 x B2
+		t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0];	t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1];	if(GREATER(t, t2))	return FALSE;	// L = A1 x B0
+		t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0];	t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1];	if(GREATER(t, t2))	return FALSE;	// L = A1 x B1
+		t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0];	t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1];	if(GREATER(t, t2))	return FALSE;	// L = A1 x B2
+		t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1];	t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2];	if(GREATER(t, t2))	return FALSE;	// L = A2 x B0
+		t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1];	t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2];	if(GREATER(t, t2))	return FALSE;	// L = A2 x B1
+		t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1];	t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2];	if(GREATER(t, t2))	return FALSE;	// L = A2 x B2
+	}
+	return TRUE;
+}
+
+//! A dedicated version when one box is constant
+inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center)
+{
+	// Stats
+	mNbVolumeBVTests++;
+
+	float t,t2;
+
+	// Class I : A's basis vectors
+	float Tx = mTBoxToModel.x - center.x;	t = extents.x + mBBx1;	if(GREATER(Tx, t))	return FALSE;
+	float Ty = mTBoxToModel.y - center.y;	t = extents.y + mBBy1;	if(GREATER(Ty, t))	return FALSE;
+	float Tz = mTBoxToModel.z - center.z;	t = extents.z + mBBz1;	if(GREATER(Tz, t))	return FALSE;
+
+	// Class II : B's basis vectors
+	t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2];
+	t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x;
+	if(GREATER(t, t2))	return FALSE;
+
+	t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2];
+	t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y;
+	if(GREATER(t, t2))	return FALSE;
+
+	t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2];
+	t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z;
+	if(GREATER(t, t2))	return FALSE;
+
+	// Class III : 9 cross products
+	// Cool trick: always perform the full test for first level, regardless of settings.
+	// That way pathological cases (such as the pencils scene) are quickly rejected anyway !
+	if(mFullBoxBoxTest || mNbVolumeBVTests==1)
+	{
+		t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2];	t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1;	if(GREATER(t, t2))	return FALSE;	// L = A0 x B0
+		t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2];	t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2;	if(GREATER(t, t2))	return FALSE;	// L = A0 x B1
+		t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2];	t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3;	if(GREATER(t, t2))	return FALSE;	// L = A0 x B2
+		t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0];	t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4;	if(GREATER(t, t2))	return FALSE;	// L = A1 x B0
+		t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0];	t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5;	if(GREATER(t, t2))	return FALSE;	// L = A1 x B1
+		t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0];	t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6;	if(GREATER(t, t2))	return FALSE;	// L = A1 x B2
+		t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1];	t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7;	if(GREATER(t, t2))	return FALSE;	// L = A2 x B0
+		t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1];	t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8;	if(GREATER(t, t2))	return FALSE;	// L = A2 x B1
+		t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1];	t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9;	if(GREATER(t, t2))	return FALSE;	// L = A2 x B2
+	}
+	return TRUE;
+}
+
+//! A special version for 2 axis-aligned boxes
+inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center)
+{
+	// Stats
+	mNbVolumeBVTests++;
+
+	float tx = mBox.mCenter.x - center.x;	float ex = extents.x + mBox.mExtents.x;	if(GREATER(tx, ex))	return FALSE;
+	float ty = mBox.mCenter.y - center.y;	float ey = extents.y + mBox.mExtents.y;	if(GREATER(ty, ey))	return FALSE;
+	float tz = mBox.mCenter.z - center.z;	float ez = extents.z + mBox.mExtents.z;	if(GREATER(tz, ez))	return FALSE;
+
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_BoxPruning.cpp

@@ -0,0 +1,375 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for box pruning.
+ *	\file		IceBoxPruning.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/*
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	You could use a complex sweep-and-prune as implemented in I-Collide.
+	You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems.
+	You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2.
+
+	Or you could use this.
+	Faster ? I don't know. Probably not. It would be a shame. But who knows ?
+	Easier ? Definitely. Enjoy the sheer simplicity.
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+*/
+
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+	inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max)
+	{
+		int First=index;
+		while(First<=last)
+		{
+			index = (First+last)>>1;
+
+			if(max>array[sorted[index]])	First	= index+1;
+			else							last	= index-1;
+		}
+	}
+// ### could be log(n) !
+// and maybe use cmp integers
+
+// InsertionSort has better coherence, RadixSort is better for one-shot queries.
+#define PRUNING_SORTER	RadixSort
+//#define PRUNING_SORTER	InsertionSort
+
+// Static for coherence
+static PRUNING_SORTER* gCompletePruningSorter = null;
+static PRUNING_SORTER* gBipartitePruningSorter0 = null;
+static PRUNING_SORTER* gBipartitePruningSorter1 = null;
+inline_ PRUNING_SORTER* GetCompletePruningSorter()
+{
+	if(!gCompletePruningSorter)	gCompletePruningSorter = new PRUNING_SORTER;
+	return gCompletePruningSorter;
+}
+inline_ PRUNING_SORTER* GetBipartitePruningSorter0()
+{
+	if(!gBipartitePruningSorter0)	gBipartitePruningSorter0 = new PRUNING_SORTER;
+	return gBipartitePruningSorter0;
+}
+inline_ PRUNING_SORTER* GetBipartitePruningSorter1()
+{
+	if(!gBipartitePruningSorter1)	gBipartitePruningSorter1 = new PRUNING_SORTER;
+	return gBipartitePruningSorter1;
+}
+void ReleasePruningSorters()
+{
+	DELETESINGLE(gBipartitePruningSorter1);
+	DELETESINGLE(gBipartitePruningSorter0);
+	DELETESINGLE(gCompletePruningSorter);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
+ *	\param		nb0		[in] number of boxes in the first set
+ *	\param		array0	[in] array of boxes for the first set
+ *	\param		nb1		[in] number of boxes in the second set
+ *	\param		array1	[in] array of boxes for the second set
+ *	\param		pairs	[out] array of overlapping pairs
+ *	\param		axes	[in] projection order (0,2,1 is often best)
+ *	\return		true if success.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes)
+{
+	// Checkings
+	if(!nb0 || !array0 || !nb1 || !array1)	return false;
+
+	// Catch axes
+	udword Axis0 = axes.mAxis0;
+	udword Axis1 = axes.mAxis1;
+	udword Axis2 = axes.mAxis2;
+
+	// Allocate some temporary data
+	float* MinPosList0 = new float[nb0];
+	float* MinPosList1 = new float[nb1];
+
+	// 1) Build main lists using the primary axis
+	for(udword i=0;i<nb0;i++)	MinPosList0[i] = array0[i]->GetMin(Axis0);
+	for(udword i=0;i<nb1;i++)	MinPosList1[i] = array1[i]->GetMin(Axis0);
+
+	// 2) Sort the lists
+	PRUNING_SORTER* RS0 = GetBipartitePruningSorter0();
+	PRUNING_SORTER* RS1 = GetBipartitePruningSorter1();
+	const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks();
+	const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks();
+
+	// 3) Prune the lists
+	udword Index0, Index1;
+
+	const udword* const LastSorted0 = &Sorted0[nb0];
+	const udword* const LastSorted1 = &Sorted1[nb1];
+	const udword* RunningAddress0 = Sorted0;
+	const udword* RunningAddress1 = Sorted1;
+
+	while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0)
+	{
+		Index0 = *Sorted0++;
+
+		while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0])	RunningAddress1++;
+
+		const udword* RunningAddress2_1 = RunningAddress1;
+
+		while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0]->GetMax(Axis0))
+		{
+			if(array0[Index0]->Intersect(*array1[Index1], Axis1))
+			{
+				if(array0[Index0]->Intersect(*array1[Index1], Axis2))
+				{
+					pairs.AddPair(Index0, Index1);
+				}
+			}
+		}
+	}
+
+	////
+
+	while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1)
+	{
+		Index0 = *Sorted1++;
+
+		while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0])	RunningAddress0++;
+
+		const udword* RunningAddress2_0 = RunningAddress0;
+
+		while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0]->GetMax(Axis0))
+		{
+			if(array0[Index1]->Intersect(*array1[Index0], Axis1))
+			{
+				if(array0[Index1]->Intersect(*array1[Index0], Axis2))
+				{
+					pairs.AddPair(Index1, Index0);
+				}
+			}
+
+		}
+	}
+
+	DELETEARRAY(MinPosList1);
+	DELETEARRAY(MinPosList0);
+
+	return true;
+}
+
+#define ORIGINAL_VERSION
+//#define JOAKIM
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
+ *	\param		nb		[in] number of boxes
+ *	\param		array	[in] array of boxes
+ *	\param		pairs	[out] array of overlapping pairs
+ *	\param		axes	[in] projection order (0,2,1 is often best)
+ *	\return		true if success.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes)
+{
+	// Checkings
+	if(!nb || !array)	return false;
+
+	// Catch axes
+	udword Axis0 = axes.mAxis0;
+	udword Axis1 = axes.mAxis1;
+	udword Axis2 = axes.mAxis2;
+
+#ifdef ORIGINAL_VERSION
+	// Allocate some temporary data
+//	float* PosList = new float[nb];
+	float* PosList = new float[nb+1];
+
+	// 1) Build main list using the primary axis
+	for(udword i=0;i<nb;i++)	PosList[i] = array[i]->GetMin(Axis0);
+PosList[nb++] = MAX_FLOAT;
+
+	// 2) Sort the list
+	PRUNING_SORTER* RS = GetCompletePruningSorter();
+	const udword* Sorted = RS->Sort(PosList, nb).GetRanks();
+
+	// 3) Prune the list
+	const udword* const LastSorted = &Sorted[nb];
+	const udword* RunningAddress = Sorted;
+	udword Index0, Index1;
+	while(RunningAddress<LastSorted && Sorted<LastSorted)
+	{
+		Index0 = *Sorted++;
+
+//		while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
+		while(PosList[*RunningAddress++]<PosList[Index0]){};
+
+		if(RunningAddress<LastSorted)
+		{
+			const udword* RunningAddress2 = RunningAddress;
+
+//			while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
+			while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
+			{
+//				if(Index0!=Index1)
+//				{
+					if(array[Index0]->Intersect(*array[Index1], Axis1))
+					{
+						if(array[Index0]->Intersect(*array[Index1], Axis2))
+						{
+							pairs.AddPair(Index0, Index1);
+						}
+					}
+//				}
+			}
+		}
+	}
+
+	DELETEARRAY(PosList);
+#endif
+
+#ifdef JOAKIM
+	// Allocate some temporary data
+//	float* PosList = new float[nb];
+	float* MinList = new float[nb+1];
+
+	// 1) Build main list using the primary axis
+	for(udword i=0;i<nb;i++)	MinList[i] = array[i]->GetMin(Axis0);
+	MinList[nb] = MAX_FLOAT;
+
+	// 2) Sort the list
+	PRUNING_SORTER* RS = GetCompletePruningSorter();
+	udword* Sorted = RS->Sort(MinList, nb+1).GetRanks();
+
+	// 3) Prune the list
+//	const udword* const LastSorted = &Sorted[nb];
+//	const udword* const LastSorted = &Sorted[nb-1];
+	const udword* RunningAddress = Sorted;
+	udword Index0, Index1;
+
+//	while(RunningAddress<LastSorted && Sorted<LastSorted)
+//	while(RunningAddress<LastSorted)
+	while(RunningAddress<&Sorted[nb])
+//	while(Sorted<LastSorted)
+	{
+//		Index0 = *Sorted++;
+		Index0 = *RunningAddress++;
+
+//		while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
+//		while(PosList[*RunningAddress++]<PosList[Index0]);
+//RunningAddress = Sorted;
+//		if(RunningAddress<LastSorted)
+		{
+			const udword* RunningAddress2 = RunningAddress;
+
+//			while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
+
+//			float CurrentMin = array[Index0]->GetMin(Axis0);
+			float CurrentMax = array[Index0]->GetMax(Axis0);
+
+			while(MinList[Index1 = *RunningAddress2] <= CurrentMax)
+//			while(PosList[Index1 = *RunningAddress] <= CurrentMax)
+			{
+//				if(Index0!=Index1)
+//				{
+					if(array[Index0]->Intersect(*array[Index1], Axis1))
+					{
+						if(array[Index0]->Intersect(*array[Index1], Axis2))
+						{
+							pairs.AddPair(Index0, Index1);
+						}
+					}
+//				}
+
+				RunningAddress2++;
+//				RunningAddress++;
+			}
+		}
+	}
+
+	DELETEARRAY(MinList);
+#endif
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Brute-force versions are kept:
+// - to check the optimized versions return the correct list of intersections
+// - to check the speed of the optimized code against the brute-force one
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
+ *	\param		nb0		[in] number of boxes in the first set
+ *	\param		array0	[in] array of boxes for the first set
+ *	\param		nb1		[in] number of boxes in the second set
+ *	\param		array1	[in] array of boxes for the second set
+ *	\param		pairs	[out] array of overlapping pairs
+ *	\return		true if success.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs)
+{
+	// Checkings
+	if(!nb0 || !array0 || !nb1 || !array1)	return false;
+
+	// Brute-force nb0*nb1 overlap tests
+	for(udword i=0;i<nb0;i++)
+	{
+		for(udword j=0;j<nb1;j++)
+		{
+			if(array0[i]->Intersect(*array1[j]))	pairs.AddPair(i, j);
+		}
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
+ *	\param		nb		[in] number of boxes
+ *	\param		array	[in] array of boxes
+ *	\param		pairs	[out] array of overlapping pairs
+ *	\return		true if success.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs)
+{
+	// Checkings
+	if(!nb || !array)	return false;
+
+	// Brute-force n(n-1)/2 overlap tests
+	for(udword i=0;i<nb;i++)
+	{
+		for(udword j=i+1;j<nb;j++)
+		{
+			if(array[i]->Intersect(*array[j]))	pairs.AddPair(i, j);
+		}
+	}
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_BoxPruning.h

@@ -0,0 +1,31 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for box pruning.
+ *	\file		OPC_BoxPruning.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_BOXPRUNING_H__
+#define __OPC_BOXPRUNING_H__
+
+	// Optimized versions
+	FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes);
+	FUNCTION OPCODE_API bool BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes);
+
+	// Brute-force versions
+	FUNCTION OPCODE_API bool BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs);
+	FUNCTION OPCODE_API bool BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs);
+
+#endif //__OPC_BOXPRUNING_H__

src/external/open_dynamics_engine-ef/ode/OPC_Collider.cpp

@@ -0,0 +1,62 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains base collider class.
+ *	\file		OPC_Collider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains the abstract class for colliders.
+ *
+ *	\class		Collider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		June, 2, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Collider::Collider() :
+	mFlags			(0),
+	mCurrentModel	(null),
+	mIMesh			(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Collider::~Collider()
+{
+}

src/external/open_dynamics_engine-ef/ode/OPC_Collider.h

@@ -0,0 +1,180 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains base collider class.
+ *	\file		OPC_Collider.h
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_COLLIDER_H__
+#define __OPC_COLLIDER_H__
+
+	enum CollisionFlag
+	{
+		OPC_FIRST_CONTACT		= (1<<0),		//!< Report all contacts (false) or only first one (true)
+		OPC_TEMPORAL_COHERENCE	= (1<<1),		//!< Use temporal coherence or not
+		OPC_CONTACT				= (1<<2),		//!< Final contact status after a collision query
+		OPC_TEMPORAL_HIT		= (1<<3),		//!< There has been an early exit due to temporal coherence
+		OPC_NO_PRIMITIVE_TESTS	= (1<<4),		//!< Keep or discard primitive-bv tests in leaf nodes (volume-mesh queries)
+
+		OPC_CONTACT_FOUND		= OPC_FIRST_CONTACT | OPC_CONTACT,
+		OPC_TEMPORAL_CONTACT	= OPC_TEMPORAL_HIT | OPC_CONTACT,
+
+		OPC_FORCE_DWORD			= 0x7fffffff
+	};
+
+	class OPCODE_API Collider
+	{
+		public:
+		// Constructor / Destructor
+											Collider();
+		virtual								~Collider();
+
+		// Collision report
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the last collision status after a collision query.
+		 *	\return		true if a collision occured
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				BOOL			GetContactStatus()			const	{ return mFlags & OPC_CONTACT;							}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the "first contact" mode.
+		 *	\return		true if "first contact" mode is on
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				BOOL			FirstContactEnabled()		const	{ return mFlags & OPC_FIRST_CONTACT;					}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the temporal coherence mode.
+		 *	\return		true if temporal coherence is on
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				BOOL			TemporalCoherenceEnabled()	const	{ return mFlags & OPC_TEMPORAL_COHERENCE;				}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks a first contact has already been found.
+		 *	\return		true if a first contact has been found and we can stop a query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				BOOL			ContactFound()				const	{ return (mFlags&OPC_CONTACT_FOUND)==OPC_CONTACT_FOUND;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks there's been an early exit due to temporal coherence;
+		 *	\return		true if a temporal hit has occured
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				BOOL			TemporalHit()				const	{ return mFlags & OPC_TEMPORAL_HIT;						}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks primitive tests are enabled;
+		 *	\return		true if primitive tests must be skipped
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				BOOL			SkipPrimitiveTests()		const	{ return mFlags & OPC_NO_PRIMITIVE_TESTS;				}
+
+		// Settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Reports all contacts (false) or first contact only (true)
+		 *	\param		flag		[in] true for first contact, false for all contacts
+		 *	\see		SetTemporalCoherence(bool flag)
+		 *	\see		ValidateSettings()
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetFirstContact(bool flag)
+											{
+												if(flag)	mFlags |= OPC_FIRST_CONTACT;
+												else		mFlags &= ~OPC_FIRST_CONTACT;
+											}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Enable/disable temporal coherence.
+		 *	\param		flag		[in] true to enable temporal coherence, false to discard it
+		 *	\see		SetFirstContact(bool flag)
+		 *	\see		ValidateSettings()
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetTemporalCoherence(bool flag)
+											{
+												if(flag)	mFlags |= OPC_TEMPORAL_COHERENCE;
+												else		mFlags &= ~OPC_TEMPORAL_COHERENCE;
+											}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Enable/disable primitive tests.
+		 *	\param		flag		[in] true to enable primitive tests, false to discard them
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetPrimitiveTests(bool flag)
+											{
+												if(!flag)	mFlags |= OPC_NO_PRIMITIVE_TESTS;
+												else		mFlags &= ~OPC_NO_PRIMITIVE_TESTS;
+											}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider.
+		 *	\return		null if everything is ok, else a string describing the problem
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual				const char*		ValidateSettings()	= 0;
+
+		protected:
+							udword			mFlags;			//!< Bit flags
+					const	BaseModel*		mCurrentModel;	//!< Current model for collision query (owner of touched faces)
+		// User mesh interface
+					//const	MeshInterface*	mIMesh;			//!< User-defined mesh interface
+                    // ericf change
+					MeshInterface*	mIMesh;			//!< User-defined mesh interface
+
+		// Internal methods
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups current collision model
+		 *	\param		model	[in] current collision model
+		 *	\return		TRUE if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//inline_				BOOL			Setup(const BaseModel* model)
+                    // ericf change
+		inline_				BOOL			Setup(BaseModel* model)
+											{
+												// Keep track of current model
+												mCurrentModel = model;
+												if(!mCurrentModel)	return FALSE;
+
+												mIMesh = model->GetMeshInterface();
+												return mIMesh!=null;
+											}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Initializes a query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual inline_		void			InitQuery()		{ mFlags &= ~OPC_TEMPORAL_CONTACT;	}
+	};
+
+#endif // __OPC_COLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_Common.cpp

@@ -0,0 +1,52 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains common classes & defs used in OPCODE.
+ *	\file		OPC_Common.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	An AABB dedicated to collision detection.
+ *	We don't use the generic AABB class included in ICE, since it can be a Min/Max or a Center/Extents one (depends
+ *	on compilation flags). Since the Center/Extents model is more efficient in collision detection, it was worth
+ *	using an extra special class.
+ *
+ *	\class		CollisionAABB
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A quantized AABB.
+ *	Center/Extent model, using 16-bits integers.
+ *
+ *	\class		QuantizedAABB
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;

src/external/open_dynamics_engine-ef/ode/OPC_Common.h

@@ -0,0 +1,101 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains common classes & defs used in OPCODE.
+ *	\file		OPC_Common.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_COMMON_H__
+#define __OPC_COMMON_H__
+
+// [GOTTFRIED]: Just a small change for readability.
+#ifdef OPC_CPU_COMPARE
+	#define GREATER(x, y)	AIR(x) > IR(y)
+#else
+	#define GREATER(x, y)	fabsf(x) > (y)
+#endif
+
+	class OPCODE_API CollisionAABB
+	{
+		public:
+		//! Constructor
+		inline_				CollisionAABB()						{}
+		//! Constructor
+		inline_				CollisionAABB(const AABB& b)		{ b.GetCenter(mCenter);	b.GetExtents(mExtents);	}
+		//! Destructor
+		inline_				~CollisionAABB()					{}
+
+		//! Get min point of the box
+		inline_	void		GetMin(Point& min)		const		{ min = mCenter - mExtents;					}
+		//! Get max point of the box
+		inline_	void		GetMax(Point& max)		const		{ max = mCenter + mExtents;					}
+
+		//! Get component of the box's min point along a given axis
+		inline_	float		GetMin(udword axis)		const		{ return mCenter[axis] - mExtents[axis];	}
+		//! Get component of the box's max point along a given axis
+		inline_	float		GetMax(udword axis)		const		{ return mCenter[axis] + mExtents[axis];	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Setups an AABB from min & max vectors.
+		 *	\param		min			[in] the min point
+		 *	\param		max			[in] the max point
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	void		SetMinMax(const Point& min, const Point& max)		{ mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f;		}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks a box is inside another box.
+		 *	\param		box		[in] the other box
+		 *	\return		true if current box is inside input box
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	BOOL		IsInside(const CollisionAABB& box) const
+							{
+								if(box.GetMin(0)>GetMin(0))	return FALSE;
+								if(box.GetMin(1)>GetMin(1))	return FALSE;
+								if(box.GetMin(2)>GetMin(2))	return FALSE;
+								if(box.GetMax(0)<GetMax(0))	return FALSE;
+								if(box.GetMax(1)<GetMax(1))	return FALSE;
+								if(box.GetMax(2)<GetMax(2))	return FALSE;
+								return TRUE;
+							}
+
+				Point		mCenter;				//!< Box center
+				Point		mExtents;				//!< Box extents
+	};
+
+	class OPCODE_API QuantizedAABB
+	{
+		public:
+		//! Constructor
+		inline_				QuantizedAABB()			{}
+		//! Destructor
+		inline_				~QuantizedAABB()		{}
+
+				sword		mCenter[3];				//!< Quantized center
+				uword		mExtents[3];			//!< Quantized extents
+	};
+
+	//! Quickly rotates & translates a vector
+	inline_ void TransformPoint(Point& dest, const Point& source, const Matrix3x3& rot, const Point& trans)
+	{
+		dest.x = trans.x + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
+		dest.y = trans.y + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
+		dest.z = trans.z + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
+	}
+
+#endif //__OPC_COMMON_H__

src/external/open_dynamics_engine-ef/ode/OPC_HybridModel.cpp

@@ -0,0 +1,471 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for hybrid models.
+ *	\file		OPC_HybridModel.cpp
+ *	\author		Pierre Terdiman
+ *	\date		May, 18, 2003
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	An hybrid collision model.
+ *
+ *	The problem :
+ *
+ *	Opcode really shines for mesh-mesh collision, especially when meshes are deeply overlapping
+ *	(it typically outperforms RAPID in those cases).
+ *
+ *	Unfortunately this is not the typical scenario in games.
+ *
+ *	For close-proximity cases, especially for volume-mesh queries, it's relatively easy to run faster
+ *	than Opcode, that suffers from a relatively high setup time.
+ *
+ *	In particular, Opcode's "vanilla" trees in those cases -can- run faster. They can also use -less-
+ *	memory than the optimized ones, when you let the system stop at ~10 triangles / leaf for example
+ *	(i.e. when you don't use "complete" trees). However, those trees tend to fragment memory quite a
+ *	lot, increasing cache misses : since they're not "complete", we can't predict the final number of
+ *	nodes and we have to allocate nodes on-the-fly. For the same reasons we can't use Opcode's "optimized"
+ *	trees here, since they rely on a known layout to perform the "optimization".
+ *
+ *	Hybrid trees :
+ *
+ *	Hybrid trees try to combine best of both worlds :
+ *
+ *	- they use a maximum limit of 16 triangles/leaf. "16" is used so that we'll be able to save the
+ *	number of triangles using 4 bits only.
+ *
+ *	- they're still "complete" trees thanks to a two-passes building phase. First we create a "vanilla"
+ *	AABB-tree with Opcode, limited to 16 triangles/leaf. Then we create a *second* vanilla tree, this
+ *	time using the leaves of the first one. The trick is : this second tree is now "complete"... so we
+ *	can further transform it into an Opcode's optimized tree.
+ *
+ *	- then we run the collision queries on that standard Opcode tree. The only difference is that leaf
+ *	nodes contain indices to leaf nodes of another tree. Also, we have to skip all primitive tests in
+ *	Opcode optimized trees, since our leaves don't contain triangles anymore.
+ *
+ *	- finally, for each collided leaf, we simply loop through 16 triangles max, and collide them with
+ *	the bounding volume used in the query (we only support volume-vs-mesh queries here, not mesh-vs-mesh)
+ *
+ *	All of that is wrapped in this "hybrid model" that contains the minimal data required for this to work.
+ *	It's a mix between old "vanilla" trees, and old "optimized" trees.
+ *
+ *	Extra advantages:
+ *
+ *	- If we use them for dynamic models, we're left with a very small number of leaf nodes to refit. It
+ *	might be a bit faster since we have less nodes to write back.
+ *
+ *	- In rigid body simulation, using temporal coherence and sleeping objects greatly reduce the actual
+ *	influence of one tree over another (i.e. the speed difference is often invisible). So memory is really
+ *	the key element to consider, and in this regard hybrid trees are just better.
+ *
+ *	Information to take home:
+ *	- they use less ram
+ *	- they're not slower (they're faster or slower depending on cases, overall there's no significant
+ *	difference *as long as objects don't interpenetrate too much* - in which case Opcode's optimized trees
+ *	are still notably faster)
+ *
+ *	\class		HybridModel
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		May, 18, 2003
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridModel::HybridModel() :
+	mNbLeaves		(0),
+	mTriangles		(null),
+	mNbPrimitives	(0),
+	mIndices		(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridModel::~HybridModel()
+{
+	Release();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Releases everything.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void HybridModel::Release()
+{
+	ReleaseBase();
+	DELETEARRAY(mIndices);
+	DELETEARRAY(mTriangles);
+	mNbLeaves		= 0;
+	mNbPrimitives	= 0;
+}
+
+	struct Internal
+	{
+		Internal()
+		{
+			mNbLeaves	= 0;
+			mLeaves		= null;
+			mTriangles	= null;
+			mBase		= null;
+		}
+		~Internal()
+		{
+			DELETEARRAY(mLeaves);
+		}
+
+		udword			mNbLeaves;
+		AABB*			mLeaves;
+		LeafTriangles*	mTriangles;
+		const udword*	mBase;
+	};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds a collision model.
+ *	\param		create		[in] model creation structure
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool HybridModel::Build(const OPCODECREATE& create)
+{
+	// 1) Checkings
+	if(!create.mIMesh || !create.mIMesh->IsValid())	return false;
+
+	// Look for degenerate faces.
+	udword NbDegenerate = create.mIMesh->CheckTopology();
+	if(NbDegenerate)
+		Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate);
+	// We continue nonetheless....
+
+	Release();	// Make sure previous tree has been discarded
+
+	// 1-1) Setup mesh interface automatically
+	SetMeshInterface(create.mIMesh);
+
+	bool Status = false;
+	AABBTree* LeafTree = null;
+	Internal Data;
+
+	// 2) Build a generic AABB Tree.
+	mSource = new AABBTree;
+	CHECKALLOC(mSource);
+
+	// 2-1) Setup a builder. Our primitives here are triangles from input mesh,
+	// so we use an AABBTreeOfTrianglesBuilder.....
+	{
+		AABBTreeOfTrianglesBuilder TB;
+		TB.mIMesh			= create.mIMesh;
+		TB.mNbPrimitives	= create.mIMesh->GetNbTriangles();
+		TB.mSettings		= create.mSettings;
+		TB.mSettings.mLimit	= 16;	// ### Hardcoded, but maybe we could let the user choose 8 / 16 / 32 ...
+		if(!mSource->Build(&TB))	goto FreeAndExit;
+	}
+
+	// 2-2) Here's the trick : create *another* AABB tree using the leaves of the first one (which are boxes, this time)
+	struct Local
+	{
+		// A callback to count leaf nodes
+		static bool CountLeaves(const AABBTreeNode* current, udword depth, void* user_data)
+		{
+			if(current->IsLeaf())
+			{
+				Internal* Data = (Internal*)user_data;
+				Data->mNbLeaves++;
+			}
+			return true;
+		}
+
+		// A callback to setup leaf nodes in our internal structures
+		static bool SetupLeafData(const AABBTreeNode* current, udword depth, void* user_data)
+		{
+			if(current->IsLeaf())
+			{
+				Internal* Data = (Internal*)user_data;
+
+				// Get current leaf's box
+				Data->mLeaves[Data->mNbLeaves] = *current->GetAABB();
+
+				// Setup leaf data
+				udword Index = (size_t(current->GetPrimitives()) - size_t(Data->mBase))/sizeof(size_t);
+				Data->mTriangles[Data->mNbLeaves].SetData(current->GetNbPrimitives(), Index);
+
+				Data->mNbLeaves++;
+			}
+			return true;
+		}
+	};
+
+	// Walk the tree & count number of leaves
+	Data.mNbLeaves = 0;
+	mSource->Walk(Local::CountLeaves, &Data);
+	mNbLeaves = Data.mNbLeaves;	// Keep track of it
+
+	// Special case for 1-leaf meshes
+	if(mNbLeaves==1)
+	{
+		mModelCode |= OPC_SINGLE_NODE;
+		Status = true;
+		goto FreeAndExit;
+	}
+
+	// Allocate our structures
+	Data.mLeaves = new AABB[Data.mNbLeaves];		CHECKALLOC(Data.mLeaves);
+	mTriangles = new LeafTriangles[Data.mNbLeaves];	CHECKALLOC(mTriangles);
+
+	// Walk the tree again & setup leaf data
+	Data.mTriangles	= mTriangles;
+	Data.mBase		= mSource->GetIndices();
+	Data.mNbLeaves	= 0;	// Reset for incoming walk
+	mSource->Walk(Local::SetupLeafData, &Data);
+
+	// Handle source indices
+	{
+		bool MustKeepIndices = true;
+		if(create.mCanRemap)
+		{
+			// We try to get rid of source indices (saving more ram!) by reorganizing triangle arrays...
+			// Remap can fail when we use callbacks => keep track of indices in that case (it still
+			// works, only using more memory)
+			if(create.mIMesh->RemapClient(mSource->GetNbPrimitives(), mSource->GetIndices()))
+			{
+				MustKeepIndices = false;
+			}
+		}
+
+		if(MustKeepIndices)
+		{
+			// Keep track of source indices (from vanilla tree)
+			mNbPrimitives = mSource->GetNbPrimitives();
+			mIndices = new udword[mNbPrimitives];
+			CopyMemory(mIndices, mSource->GetIndices(), mNbPrimitives*sizeof(udword));
+		}
+	}
+
+	// Now, create our optimized tree using previous leaf nodes
+	LeafTree = new AABBTree;
+	CHECKALLOC(LeafTree);
+	{
+		AABBTreeOfAABBsBuilder TB;	// Now using boxes !
+		TB.mSettings		= create.mSettings;
+		TB.mSettings.mLimit	= 1;	// We now want a complete tree so that we can "optimize" it
+		TB.mNbPrimitives	= Data.mNbLeaves;
+		TB.mAABBArray		= Data.mLeaves;
+		if(!LeafTree->Build(&TB))	goto FreeAndExit;
+	}
+
+	// 3) Create an optimized tree according to user-settings
+	if(!CreateTree(create.mNoLeaf, create.mQuantized))	goto FreeAndExit;
+
+	// 3-2) Create optimized tree
+	if(!mTree->Build(LeafTree))	goto FreeAndExit;
+
+	// Finally ok...
+	Status = true;
+
+FreeAndExit:	// Allow me this one...
+	DELETESINGLE(LeafTree);
+
+	// 3-3) Delete generic tree if needed
+	if(!create.mKeepOriginal)	DELETESINGLE(mSource);
+
+	return Status;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the number of bytes used by the tree.
+ *	\return		amount of bytes used
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword HybridModel::GetUsedBytes() const
+{
+	udword UsedBytes = 0;
+	if(mTree)		UsedBytes += mTree->GetUsedBytes();
+	if(mIndices)	UsedBytes += mNbPrimitives * sizeof(udword);	// mIndices
+	if(mTriangles)	UsedBytes += mNbLeaves * sizeof(LeafTriangles);	// mTriangles
+	return UsedBytes;
+}
+
+inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
+{
+	// Compute triangle's AABB = a leaf box
+#ifdef OPC_USE_FCOMI	// a 15% speedup on my machine, not much
+	min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
+	max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
+
+	min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
+	max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
+
+	min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
+	max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
+#else
+	min = *vp.Vertex[0];
+	max = *vp.Vertex[0];
+	min.Min(*vp.Vertex[1]);
+	max.Max(*vp.Vertex[1]);
+	min.Min(*vp.Vertex[2]);
+	max.Max(*vp.Vertex[2]);
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the collision model. This can be used to handle dynamic meshes. Usage is:
+ *	1. modify your mesh vertices (keep the topology constant!)
+ *	2. refit the tree (call this method)
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool HybridModel::Refit()
+{
+	if(!mIMesh)	return false;
+	if(!mTree)	return false;
+
+	if(IsQuantized())	return false;
+	if(HasLeafNodes())	return false;
+
+	const LeafTriangles* LT = GetLeafTriangles();
+	const udword* Indices = GetIndices();
+
+	// Bottom-up update
+	VertexPointers VP;
+	Point Min,Max;
+	Point Min_,Max_;
+	udword Index = mTree->GetNbNodes();
+	AABBNoLeafNode* Nodes = (AABBNoLeafNode*)((AABBNoLeafTree*)mTree)->GetNodes();
+	while(Index--)
+	{
+		AABBNoLeafNode& Current = Nodes[Index];
+
+		if(Current.HasPosLeaf())
+		{
+			const LeafTriangles& CurrentLeaf = LT[Current.GetPosPrimitive()];
+
+			Min.SetPlusInfinity();
+			Max.SetMinusInfinity();
+
+			Point TmpMin, TmpMax;
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					mIMesh->GetTriangle(VP, *T++);
+					ComputeMinMax(TmpMin, TmpMax, VP);
+					Min.Min(TmpMin);
+					Max.Max(TmpMax);
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					mIMesh->GetTriangle(VP, BaseIndex++);
+					ComputeMinMax(TmpMin, TmpMax, VP);
+					Min.Min(TmpMin);
+					Max.Max(TmpMax);
+				}
+			}
+		}
+		else
+		{
+			const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
+			CurrentBox.GetMin(Min);
+			CurrentBox.GetMax(Max);
+		}
+
+		if(Current.HasNegLeaf())
+		{
+			const LeafTriangles& CurrentLeaf = LT[Current.GetNegPrimitive()];
+
+			Min_.SetPlusInfinity();
+			Max_.SetMinusInfinity();
+
+			Point TmpMin, TmpMax;
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					mIMesh->GetTriangle(VP, *T++);
+					ComputeMinMax(TmpMin, TmpMax, VP);
+					Min_.Min(TmpMin);
+					Max_.Max(TmpMax);
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					mIMesh->GetTriangle(VP, BaseIndex++);
+					ComputeMinMax(TmpMin, TmpMax, VP);
+					Min_.Min(TmpMin);
+					Max_.Max(TmpMax);
+				}
+			}
+		}
+		else
+		{
+			const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
+			CurrentBox.GetMin(Min_);
+			CurrentBox.GetMax(Max_);
+		}
+#ifdef OPC_USE_FCOMI
+		Min.x = FCMin2(Min.x, Min_.x);
+		Max.x = FCMax2(Max.x, Max_.x);
+		Min.y = FCMin2(Min.y, Min_.y);
+		Max.y = FCMax2(Max.y, Max_.y);
+		Min.z = FCMin2(Min.z, Min_.z);
+		Max.z = FCMax2(Max.z, Max_.z);
+#else
+		Min.Min(Min_);
+		Max.Max(Max_);
+#endif
+		Current.mAABB.SetMinMax(Min, Max);
+	}
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_HybridModel.h

@@ -0,0 +1,106 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for hybrid models.
+ *	\file		OPC_HybridModel.h
+ *	\author		Pierre Terdiman
+ *	\date		May, 18, 2003
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_HYBRIDMODEL_H__
+#define __OPC_HYBRIDMODEL_H__
+
+	//! Leaf descriptor
+	struct LeafTriangles
+	{
+		udword			Data;		//!< Packed data
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets number of triangles in the leaf.
+		 *	\return		number of triangles N, with 0 < N <= 16
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	udword	GetNbTriangles()				const	{ return (Data & 15)+1;											}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets triangle index for this leaf. Indexed model's array of indices retrieved with HybridModel::GetIndices()
+		 *	\return		triangle index
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	udword	GetTriangleIndex()				const	{ return Data>>4;												}
+		inline_	void	SetData(udword nb, udword index)		{ ASSERT(nb>0 && nb<=16);	nb--;	Data = (index<<4)|(nb&15);	}
+	};
+
+	class OPCODE_API HybridModel : public BaseModel
+	{
+		public:
+		// Constructor/Destructor
+													HybridModel();
+		virtual										~HybridModel();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Builds a collision model.
+		 *	\param		create		[in] model creation structure
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(BaseModel)	bool					Build(const OPCODECREATE& create);
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the number of bytes used by the tree.
+		 *	\return		amount of bytes used
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(BaseModel)	udword					GetUsedBytes()		const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Refits the collision model. This can be used to handle dynamic meshes. Usage is:
+		 *	1. modify your mesh vertices (keep the topology constant!)
+		 *	2. refit the tree (call this method)
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(BaseModel)	bool					Refit();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets array of triangles.
+		 *	\return		array of triangles
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				const LeafTriangles*	GetLeafTriangles()	const	{ return mTriangles;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets array of indices.
+		 *	\return		array of indices
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				const udword*			GetIndices()		const	{ return mIndices;		}
+
+		private:
+							udword					mNbLeaves;		//!< Number of leaf nodes in the model
+							LeafTriangles*			mTriangles;		//!< Array of mNbLeaves leaf descriptors
+							udword					mNbPrimitives;	//!< Number of primitives in the model
+							udword*					mIndices;		//!< Array of primitive indices
+
+		// Internal methods
+							void					Release();
+	};
+
+#endif // __OPC_HYBRIDMODEL_H__

src/external/open_dynamics_engine-ef/ode/OPC_IceHook.h

@@ -0,0 +1,70 @@
+
+// Should be included by Opcode.h if needed
+
+	#define ICE_DONT_CHECK_COMPILER_OPTIONS
+
+	// From Windows...
+	typedef int                 BOOL;
+	#ifndef FALSE
+	#define FALSE               0
+	#endif
+
+	#ifndef TRUE
+	#define TRUE                1
+	#endif
+
+	#include <stdio.h>
+	#include <stdlib.h>
+	#include <assert.h>
+	#include <string.h>
+	#include <float.h>
+	#include <math.h>
+
+	#ifndef ASSERT
+		#define	ASSERT(exp)	{}
+	#endif
+	#define ICE_COMPILE_TIME_ASSERT(exp)	extern char ICE_Dummy[ (exp) ? 1 : -1 ]
+
+	#define	Log				printf
+	#define	SetIceError(a,b)	false
+	#define	EC_OUTOFMEMORY	"Out of memory"
+
+	#include "ode/IcePreprocessor.h"
+
+	#undef ICECORE_API
+	#define ICECORE_API	OPCODE_API
+
+	#include "ode/IceTypes.h"
+	#include "ode/IceFPU.h"
+	#include "ode/IceMemoryMacros.h"
+
+	namespace IceCore
+	{
+		#include "ode/IceUtils.h"
+		#include "ode/IceContainer.h"
+		#include "ode/IcePairs.h"
+		#include "ode/IceRevisitedRadix.h"
+		#include "ode/IceRandom.h"
+	}
+	using namespace IceCore;
+
+	#define ICEMATHS_API	OPCODE_API
+	namespace IceMaths
+	{
+		#include "ode/IceAxes.h"
+		#include "ode/IcePoint.h"
+		#include "ode/IceHPoint.h"
+		#include "ode/IceMatrix3x3.h"
+		#include "ode/IceMatrix4x4.h"
+		#include "ode/IcePlane.h"
+		#include "ode/IceRay.h"
+		#include "ode/IceIndexedTriangle.h"
+		#include "ode/IceTriangle.h"
+		#include "ode/IceTriList.h"
+		#include "ode/IceAABB.h"
+		#include "ode/IceOBB.h"
+		#include "ode/IceBoundingSphere.h"
+		#include "ode/IceSegment.h"
+		#include "ode/IceLSS.h"
+	}
+	using namespace IceMaths;

src/external/open_dynamics_engine-ef/ode/OPC_LSSAABBOverlap.h

@@ -0,0 +1,523 @@
+
+// Following code from Magic-Software (http://www.magic-software.com/)
+// A bit modified for Opcode
+
+inline_ float OPC_PointAABBSqrDist(const Point& point, const Point& center, const Point& extents)
+{
+	// Compute coordinates of point in box coordinate system
+	Point Closest = point - center;
+
+	float SqrDistance = 0.0f;
+
+	if(Closest.x < -extents.x)
+	{
+		float Delta = Closest.x + extents.x;
+		SqrDistance += Delta*Delta;
+	}
+	else if(Closest.x > extents.x)
+	{
+		float Delta = Closest.x - extents.x;
+		SqrDistance += Delta*Delta;
+	}
+
+	if(Closest.y < -extents.y)
+	{
+		float Delta = Closest.y + extents.y;
+		SqrDistance += Delta*Delta;
+	}
+	else if(Closest.y > extents.y)
+	{
+		float Delta = Closest.y - extents.y;
+		SqrDistance += Delta*Delta;
+	}
+
+	if(Closest.z < -extents.z)
+	{
+		float Delta = Closest.z + extents.z;
+		SqrDistance += Delta*Delta;
+	}
+	else if(Closest.z > extents.z)
+	{
+		float Delta = Closest.z - extents.z;
+		SqrDistance += Delta*Delta;
+	}
+	return SqrDistance;
+}
+
+static void Face(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, const Point& rkPmE, float* pfLParam, float& rfSqrDistance)
+{
+    Point kPpE;
+    float fLSqr, fInv, fTmp, fParam, fT, fDelta;
+
+    kPpE[i1] = rkPnt[i1] + extents[i1];
+    kPpE[i2] = rkPnt[i2] + extents[i2];
+    if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0])
+    {
+        if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0])
+        {
+            // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
+            if(pfLParam)
+            {
+                rkPnt[i0] = extents[i0];
+                fInv = 1.0f/rkDir[i0];
+                rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
+                rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
+                *pfLParam = -rkPmE[i0]*fInv;
+            }
+        }
+        else
+        {
+            // v[i1] >= -e[i1], v[i2] < -e[i2]
+            fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
+            fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
+            if(fTmp <= 2.0f*fLSqr*extents[i1])
+            {
+                fT = fTmp/fLSqr;
+                fLSqr += rkDir[i1]*rkDir[i1];
+                fTmp = kPpE[i1] - fT;
+                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
+                fParam = -fDelta/fLSqr;
+                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+                if(pfLParam)
+                {
+                    *pfLParam = fParam;
+                    rkPnt[i0] = extents[i0];
+                    rkPnt[i1] = fT - extents[i1];
+                    rkPnt[i2] = -extents[i2];
+                }
+            }
+            else
+            {
+                fLSqr += rkDir[i1]*rkDir[i1];
+                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
+                fParam = -fDelta/fLSqr;
+                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+                if(pfLParam)
+                {
+                    *pfLParam = fParam;
+                    rkPnt[i0] = extents[i0];
+                    rkPnt[i1] = extents[i1];
+                    rkPnt[i2] = -extents[i2];
+                }
+            }
+        }
+    }
+    else
+    {
+        if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
+        {
+            // v[i1] < -e[i1], v[i2] >= -e[i2]
+            fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
+            fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
+            if(fTmp <= 2.0f*fLSqr*extents[i2])
+            {
+                fT = fTmp/fLSqr;
+                fLSqr += rkDir[i2]*rkDir[i2];
+                fTmp = kPpE[i2] - fT;
+                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
+                fParam = -fDelta/fLSqr;
+                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
+
+                if(pfLParam)
+                {
+                    *pfLParam = fParam;
+                    rkPnt[i0] = extents[i0];
+                    rkPnt[i1] = -extents[i1];
+                    rkPnt[i2] = fT - extents[i2];
+                }
+            }
+            else
+            {
+                fLSqr += rkDir[i2]*rkDir[i2];
+                fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
+                fParam = -fDelta/fLSqr;
+                rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
+
+                if(pfLParam)
+                {
+                    *pfLParam = fParam;
+                    rkPnt[i0] = extents[i0];
+                    rkPnt[i1] = -extents[i1];
+                    rkPnt[i2] = extents[i2];
+                }
+            }
+        }
+        else
+        {
+            // v[i1] < -e[i1], v[i2] < -e[i2]
+            fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
+            fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
+            if(fTmp >= 0.0f)
+            {
+                // v[i1]-edge is closest
+                if ( fTmp <= 2.0f*fLSqr*extents[i1] )
+                {
+                    fT = fTmp/fLSqr;
+                    fLSqr += rkDir[i1]*rkDir[i1];
+                    fTmp = kPpE[i1] - fT;
+                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
+                    fParam = -fDelta/fLSqr;
+                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+                    if(pfLParam)
+                    {
+                        *pfLParam = fParam;
+                        rkPnt[i0] = extents[i0];
+                        rkPnt[i1] = fT - extents[i1];
+                        rkPnt[i2] = -extents[i2];
+                    }
+                }
+                else
+                {
+                    fLSqr += rkDir[i1]*rkDir[i1];
+                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
+                    fParam = -fDelta/fLSqr;
+                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+                    if(pfLParam)
+                    {
+                        *pfLParam = fParam;
+                        rkPnt[i0] = extents[i0];
+                        rkPnt[i1] = extents[i1];
+                        rkPnt[i2] = -extents[i2];
+                    }
+                }
+                return;
+            }
+
+            fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
+            fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
+            if(fTmp >= 0.0f)
+            {
+                // v[i2]-edge is closest
+                if(fTmp <= 2.0f*fLSqr*extents[i2])
+                {
+                    fT = fTmp/fLSqr;
+                    fLSqr += rkDir[i2]*rkDir[i2];
+                    fTmp = kPpE[i2] - fT;
+                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
+                    fParam = -fDelta/fLSqr;
+                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
+
+                    if(pfLParam)
+                    {
+                        *pfLParam = fParam;
+                        rkPnt[i0] = extents[i0];
+                        rkPnt[i1] = -extents[i1];
+                        rkPnt[i2] = fT - extents[i2];
+                    }
+                }
+                else
+                {
+                    fLSqr += rkDir[i2]*rkDir[i2];
+                    fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
+                    fParam = -fDelta/fLSqr;
+                    rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
+
+                    if(pfLParam)
+                    {
+                        *pfLParam = fParam;
+                        rkPnt[i0] = extents[i0];
+                        rkPnt[i1] = -extents[i1];
+                        rkPnt[i2] = extents[i2];
+                    }
+                }
+                return;
+            }
+
+            // (v[i1],v[i2])-corner is closest
+            fLSqr += rkDir[i2]*rkDir[i2];
+            fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2];
+            fParam = -fDelta/fLSqr;
+            rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
+
+            if(pfLParam)
+            {
+                *pfLParam = fParam;
+                rkPnt[i0] = extents[i0];
+                rkPnt[i1] = -extents[i1];
+                rkPnt[i2] = -extents[i2];
+            }
+        }
+    }
+}
+
+static void CaseNoZeros(Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
+{
+    Point kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z);
+
+    float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
+
+    fProdDxPy = rkDir.x*kPmE.y;
+    fProdDyPx = rkDir.y*kPmE.x;
+    if(fProdDyPx >= fProdDxPy)
+    {
+        fProdDzPx = rkDir.z*kPmE.x;
+        fProdDxPz = rkDir.x*kPmE.z;
+        if(fProdDzPx >= fProdDxPz)
+        {
+            // line intersects x = e0
+            Face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+        }
+        else
+        {
+            // line intersects z = e2
+            Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+        }
+    }
+    else
+    {
+        fProdDzPy = rkDir.z*kPmE.y;
+        fProdDyPz = rkDir.y*kPmE.z;
+        if(fProdDzPy >= fProdDyPz)
+        {
+            // line intersects y = e1
+            Face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+        }
+        else
+        {
+            // line intersects z = e2
+            Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
+        }
+    }
+}
+
+static void Case0(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
+{
+    float fPmE0 = rkPnt[i0] - extents[i0];
+    float fPmE1 = rkPnt[i1] - extents[i1];
+    float fProd0 = rkDir[i1]*fPmE0;
+    float fProd1 = rkDir[i0]*fPmE1;
+    float fDelta, fInvLSqr, fInv;
+
+    if(fProd0 >= fProd1)
+    {
+        // line intersects P[i0] = e[i0]
+        rkPnt[i0] = extents[i0];
+
+        float fPpE1 = rkPnt[i1] + extents[i1];
+        fDelta = fProd0 - rkDir[i0]*fPpE1;
+        if(fDelta >= 0.0f)
+        {
+            fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
+            rfSqrDistance += fDelta*fDelta*fInvLSqr;
+            if(pfLParam)
+            {
+                rkPnt[i1] = -extents[i1];
+                *pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
+            }
+        }
+        else
+        {
+            if(pfLParam)
+            {
+                fInv = 1.0f/rkDir[i0];
+                rkPnt[i1] -= fProd0*fInv;
+                *pfLParam = -fPmE0*fInv;
+            }
+        }
+    }
+    else
+    {
+        // line intersects P[i1] = e[i1]
+        rkPnt[i1] = extents[i1];
+
+        float fPpE0 = rkPnt[i0] + extents[i0];
+        fDelta = fProd1 - rkDir[i1]*fPpE0;
+        if(fDelta >= 0.0f)
+        {
+            fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
+            rfSqrDistance += fDelta*fDelta*fInvLSqr;
+            if(pfLParam)
+            {
+                rkPnt[i0] = -extents[i0];
+                *pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
+            }
+        }
+        else
+        {
+            if(pfLParam)
+            {
+                fInv = 1.0f/rkDir[i1];
+                rkPnt[i0] -= fProd1*fInv;
+                *pfLParam = -fPmE1*fInv;
+            }
+        }
+    }
+
+    if(rkPnt[i2] < -extents[i2])
+    {
+        fDelta = rkPnt[i2] + extents[i2];
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt[i2] = -extents[i2];
+    }
+    else if ( rkPnt[i2] > extents[i2] )
+    {
+        fDelta = rkPnt[i2] - extents[i2];
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt[i2] = extents[i2];
+    }
+}
+
+static void Case00(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
+{
+    float fDelta;
+
+    if(pfLParam)
+        *pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0];
+
+    rkPnt[i0] = extents[i0];
+
+    if(rkPnt[i1] < -extents[i1])
+    {
+        fDelta = rkPnt[i1] + extents[i1];
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt[i1] = -extents[i1];
+    }
+    else if(rkPnt[i1] > extents[i1])
+    {
+        fDelta = rkPnt[i1] - extents[i1];
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt[i1] = extents[i1];
+    }
+
+    if(rkPnt[i2] < -extents[i2])
+    {
+        fDelta = rkPnt[i2] + extents[i2];
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt[i1] = -extents[i2];
+    }
+    else if(rkPnt[i2] > extents[i2])
+    {
+        fDelta = rkPnt[i2] - extents[i2];
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt[i2] = extents[i2];
+    }
+}
+
+static void Case000(Point& rkPnt, const Point& extents, float& rfSqrDistance)
+{
+    float fDelta;
+
+    if(rkPnt.x < -extents.x)
+    {
+        fDelta = rkPnt.x + extents.x;
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt.x = -extents.x;
+    }
+    else if(rkPnt.x > extents.x)
+    {
+        fDelta = rkPnt.x - extents.x;
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt.x = extents.x;
+    }
+
+    if(rkPnt.y < -extents.y)
+    {
+        fDelta = rkPnt.y + extents.y;
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt.y = -extents.y;
+    }
+    else if(rkPnt.y > extents.y)
+    {
+        fDelta = rkPnt.y - extents.y;
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt.y = extents.y;
+    }
+
+    if(rkPnt.z < -extents.z)
+    {
+        fDelta = rkPnt.z + extents.z;
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt.z = -extents.z;
+    }
+    else if(rkPnt.z > extents.z)
+    {
+        fDelta = rkPnt.z - extents.z;
+        rfSqrDistance += fDelta*fDelta;
+        rkPnt.z = extents.z;
+    }
+}
+
+static float SqrDistance(const Ray& rkLine, const Point& center, const Point& extents, float* pfLParam)
+{
+    // compute coordinates of line in box coordinate system
+    Point kDiff = rkLine.mOrig - center;
+    Point kPnt = kDiff;
+    Point kDir = rkLine.mDir;
+
+    // Apply reflections so that direction vector has nonnegative components.
+    bool bReflect[3];
+    for(int i=0;i<3;i++)
+    {
+        if(kDir[i]<0.0f)
+        {
+            kPnt[i] = -kPnt[i];
+            kDir[i] = -kDir[i];
+            bReflect[i] = true;
+        }
+        else
+        {
+            bReflect[i] = false;
+        }
+    }
+
+    float fSqrDistance = 0.0f;
+
+    if(kDir.x>0.0f)
+    {
+        if(kDir.y>0.0f)
+        {
+            if(kDir.z>0.0f)	CaseNoZeros(kPnt, kDir, extents, pfLParam, fSqrDistance);		// (+,+,+)
+            else			Case0(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance);	// (+,+,0)
+        }
+        else
+        {
+            if(kDir.z>0.0f)	Case0(0, 2, 1, kPnt, kDir, extents, pfLParam, fSqrDistance);	// (+,0,+)
+            else			Case00(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance);	// (+,0,0)
+        }
+    }
+    else
+    {
+        if(kDir.y>0.0f)
+        {
+            if(kDir.z>0.0f)	Case0(1, 2, 0, kPnt, kDir, extents, pfLParam, fSqrDistance);	// (0,+,+)
+            else			Case00(1, 0, 2, kPnt, kDir, extents, pfLParam, fSqrDistance);	// (0,+,0)
+        }
+        else
+        {
+            if(kDir.z>0.0f)	Case00(2, 0, 1, kPnt, kDir, extents, pfLParam, fSqrDistance);	// (0,0,+)
+            else
+            {
+							Case000(kPnt, extents, fSqrDistance);							// (0,0,0)
+							if(pfLParam)	*pfLParam = 0.0f;
+            }
+        }
+    }
+    return fSqrDistance;
+}
+
+inline_ float OPC_SegmentOBBSqrDist(const Segment& segment, const Point& c0, const Point& e0)
+{
+	float fLP;
+	float fSqrDistance = SqrDistance(Ray(segment.GetOrigin(), segment.ComputeDirection()), c0, e0, &fLP);
+	if(fLP>=0.0f)
+	{
+		if(fLP<=1.0f)	return fSqrDistance;
+		else			return OPC_PointAABBSqrDist(segment.mP1, c0, e0);
+	}
+	else				return OPC_PointAABBSqrDist(segment.mP0, c0, e0);
+}
+
+inline_ BOOL LSSCollider::LSSAABBOverlap(const Point& center, const Point& extents)
+{
+	// Stats
+	mNbVolumeBVTests++;
+
+	float s2 = OPC_SegmentOBBSqrDist(mSeg, center, extents);
+	if(s2<mRadius2)	return TRUE;
+
+	return FALSE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_LSSCollider.cpp

@@ -0,0 +1,737 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for an LSS collider.
+ *	\file		OPC_LSSCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		December, 28, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a lss-vs-tree collider.
+ *
+ *	\class		LSSCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		December, 28, 2002
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_LSSAABBOverlap.h"
+#include "ode/OPC_LSSTriOverlap.h"
+
+#define SET_CONTACT(prim_index, flag)									\
+	/* Set contact status */											\
+	mFlags |= flag;														\
+	mTouchedPrimitives->Add(prim_index);
+
+//! LSS-triangle overlap test
+#define LSS_PRIM(prim_index, flag)										\
+	/* Request vertices from the app */									\
+	VertexPointers VP;	mIMesh->GetTriangle(VP, prim_index);			\
+																		\
+	/* Perform LSS-tri overlap test */									\
+	if(LSSTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))		\
+	{																	\
+		SET_CONTACT(prim_index, flag)									\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+LSSCollider::LSSCollider()
+{
+//	mCenter.Zero();
+//	mRadius2 = 0.0f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+LSSCollider::~LSSCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic collision query for generic OPCODE models. After the call, access the results:
+ *	- with GetContactStatus()
+ *	- with GetNbTouchedPrimitives()
+ *	- with GetTouchedPrimitives()
+ *
+ *	\param		cache			[in/out] an lss cache
+ *	\param		lss				[in] collision lss in local space
+ *	\param		model			[in] Opcode model to collide with
+ *	\param		worldl			[in] lss world matrix, or null
+ *	\param		worldm			[in] model's world matrix, or null
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const Model& model, const Matrix4x4* worldl, const Matrix4x4* worldm)
+// ericf change
+bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, Model& model, const Matrix4x4* worldl, const Matrix4x4* worldm)
+{
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, lss, worldl, worldm))	return true;
+
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a collision query :
+ *	- reset stats & contact status
+ *	- setup matrices
+ *	- check temporal coherence
+ *
+ *	\param		cache		[in/out] an lss cache
+ *	\param		lss			[in] lss in local space
+ *	\param		worldl		[in] lss world matrix, or null
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		TRUE if we can return immediately
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL LSSCollider::InitQuery(LSSCache& cache, const LSS& lss, const Matrix4x4* worldl, const Matrix4x4* worldm)
+{
+	// 1) Call the base method
+	VolumeCollider::InitQuery();
+
+	// 2) Compute LSS in model space:
+	// - Precompute R^2
+	mRadius2 = lss.mRadius * lss.mRadius;
+	// - Compute segment
+	mSeg.mP0 = lss.mP0;
+	mSeg.mP1 = lss.mP1;
+	// -> to world space
+	if(worldl)
+	{
+		mSeg.mP0 *= *worldl;
+		mSeg.mP1 *= *worldl;
+	}
+	// -> to model space
+	if(worldm)
+	{
+		// Invert model matrix
+		Matrix4x4 InvWorldM;
+		InvertPRMatrix(InvWorldM, *worldm);
+
+		mSeg.mP0 *= InvWorldM;
+		mSeg.mP1 *= InvWorldM;
+	}
+
+	// 3) Setup destination pointer
+	mTouchedPrimitives = &cache.TouchedPrimitives;
+
+	// 4) Special case: 1-triangle meshes [Opcode 1.3]
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		if(!SkipPrimitiveTests())
+		{
+			// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
+			mTouchedPrimitives->Reset();
+
+			// Perform overlap test between the unique triangle and the LSS (and set contact status if needed)
+			LSS_PRIM(udword(0), OPC_CONTACT)
+
+			// Return immediately regardless of status
+			return TRUE;
+		}
+	}
+
+	// 5) Check temporal coherence :
+	if(TemporalCoherenceEnabled())
+	{
+		// Here we use temporal coherence
+		// => check results from previous frame before performing the collision query
+		if(FirstContactEnabled())
+		{
+			// We're only interested in the first contact found => test the unique previously touched face
+			if(mTouchedPrimitives->GetNbEntries())
+			{
+				// Get index of previously touched face = the first entry in the array
+				udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
+
+				// Then reset the array:
+				// - if the overlap test below is successful, the index we'll get added back anyway
+				// - if it isn't, then the array should be reset anyway for the normal query
+				mTouchedPrimitives->Reset();
+
+				// Perform overlap test between the cached triangle and the LSS (and set contact status if needed)
+				LSS_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
+
+				// Return immediately if possible
+				if(GetContactStatus())	return TRUE;
+			}
+			// else no face has been touched during previous query
+			// => we'll have to perform a normal query
+		}
+		else
+		{
+			// We're interested in all contacts =>test the new real LSS N(ew) against the previous fat LSS P(revious):
+
+			// ### rewrite this
+
+			LSS Test(mSeg, lss.mRadius);	// in model space
+			LSS Previous(cache.Previous, sqrtf(cache.Previous.mRadius));
+
+//			if(cache.Previous.Contains(Test))
+			if(IsCacheValid(cache) && Previous.Contains(Test))
+			{
+				// - if N is included in P, return previous list
+				// => we simply leave the list (mTouchedFaces) unchanged
+
+				// Set contact status if needed
+				if(mTouchedPrimitives->GetNbEntries())	mFlags |= OPC_TEMPORAL_CONTACT;
+
+				// In any case we don't need to do a query
+				return TRUE;
+			}
+			else
+			{
+				// - else do the query using a fat N
+
+				// Reset cache since we'll about to perform a real query
+				mTouchedPrimitives->Reset();
+
+				// Make a fat sphere so that coherence will work for subsequent frames
+				mRadius2 *= cache.FatCoeff;
+//				mRadius2 = (lss.mRadius * cache.FatCoeff)*(lss.mRadius * cache.FatCoeff);
+
+
+				// Update cache with query data (signature for cached faces)
+				cache.Previous.mP0 = mSeg.mP0;
+				cache.Previous.mP1 = mSeg.mP1;
+				cache.Previous.mRadius = mRadius2;
+			}
+		}
+	}
+	else
+	{
+		// Here we don't use temporal coherence => do a normal query
+		mTouchedPrimitives->Reset();
+	}
+
+	return FALSE;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for vanilla AABB trees.
+ *	\param		cache		[in/out] an lss cache
+ *	\param		lss			[in] collision lss in world space
+ *	\param		tree		[in] AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const AABBTree* tree)
+{
+	// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
+	// So we don't really have "primitives" to deal with. Hence it doesn't work with
+	// "FirstContact" + "TemporalCoherence".
+	ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
+
+	// Checkings
+	if(!tree)	return false;
+
+	// Init collision query
+	if(InitQuery(cache, lss))	return true;
+
+	// Perform collision query
+	_Collide(tree);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the LSS completely contains the box. In which case we can end the query sooner.
+ *	\param		bc	[in] box center
+ *	\param		be	[in] box extents
+ *	\return		true if the LSS contains the whole box
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL LSSCollider::LSSContainsBox(const Point& bc, const Point& be)
+{
+	// Not implemented
+	return FALSE;
+}
+
+#define TEST_BOX_IN_LSS(center, extents)	\
+	if(LSSContainsBox(center, extents))		\
+	{										\
+		/* Set contact status */			\
+		mFlags |= OPC_CONTACT;				\
+		_Dump(node);						\
+		return;								\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_Collide(const AABBCollisionNode* node)
+{
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		LSS_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
+{
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_Collide(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_LSS(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		LSS_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_LSS(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_Collide(const AABBNoLeafNode* node)
+{
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ LSS_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ LSS_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
+{
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_Collide(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_LSS(Center, Extents)
+
+	if(node->HasPosLeaf())	{ LSS_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ LSS_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform LSS-AABB overlap test
+	if(!LSSAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_LSS(Center, Extents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for vanilla AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void LSSCollider::_Collide(const AABBTreeNode* node)
+{
+	// Perform LSS-AABB overlap test
+	Point Center, Extents;
+	node->GetAABB()->GetCenter(Center);
+	node->GetAABB()->GetExtents(Extents);
+	if(!LSSAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf() || LSSContainsBox(Center, Extents))
+	{
+		mFlags |= OPC_CONTACT;
+		mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
+	}
+	else
+	{
+		_Collide(node->GetPos());
+		_Collide(node->GetNeg());
+	}
+}
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridLSSCollider::HybridLSSCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridLSSCollider::~HybridLSSCollider()
+{
+}
+
+//bool HybridLSSCollider::Collide(LSSCache& cache, const LSS& lss, const HybridModel& model, const Matrix4x4* worldl, const Matrix4x4* worldm)
+// ericf change
+bool HybridLSSCollider::Collide(LSSCache& cache, const LSS& lss, HybridModel& model, const Matrix4x4* worldl, const Matrix4x4* worldm)
+{
+	// We don't want primitive tests here!
+	mFlags |= OPC_NO_PRIMITIVE_TESTS;
+
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, lss, worldl, worldm))	return true;
+
+	// Special case for 1-leaf trees
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
+		udword Nb = mIMesh->GetNbTriangles();
+
+		// Loop through all triangles
+		for(udword i=0;i<Nb;i++)
+		{
+			LSS_PRIM(i, OPC_CONTACT)
+		}
+		return true;
+	}
+
+	// Override destination array since we're only going to get leaf boxes here
+	mTouchedBoxes.Reset();
+	mTouchedPrimitives = &mTouchedBoxes;
+
+	// Now, do the actual query against leaf boxes
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+
+	// We only have a list of boxes so far
+	if(GetContactStatus())
+	{
+		// Reset contact status, since it currently only reflects collisions with leaf boxes
+		Collider::InitQuery();
+
+		// Change dest container so that we can use built-in overlap tests and get collided primitives
+		cache.TouchedPrimitives.Reset();
+		mTouchedPrimitives = &cache.TouchedPrimitives;
+
+		// Read touched leaf boxes
+		udword Nb = mTouchedBoxes.GetNbEntries();
+		const udword* Touched = mTouchedBoxes.GetEntries();
+
+		const LeafTriangles* LT = model.GetLeafTriangles();
+		const udword* Indices = model.GetIndices();
+
+		// Loop through touched leaves
+		while(Nb--)
+		{
+			const LeafTriangles& CurrentLeaf = LT[*Touched++];
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = *T++;
+					LSS_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = BaseIndex++;
+					LSS_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_LSSCollider.h

@@ -0,0 +1,103 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for an LSS collider.
+ *	\file		OPC_LSSCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		December, 28, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_LSSCOLLIDER_H__
+#define __OPC_LSSCOLLIDER_H__
+
+	struct OPCODE_API LSSCache : VolumeCache
+	{
+					LSSCache()
+					{
+						Previous.mP0 = Point(0.0f, 0.0f, 0.0f);
+						Previous.mP1 = Point(0.0f, 0.0f, 0.0f);
+						Previous.mRadius = 0.0f;
+						FatCoeff = 1.1f;
+					}
+
+		// Cached faces signature
+		LSS			Previous;	//!< LSS used when performing the query resulting in cached faces
+		// User settings
+		float		FatCoeff;	//!< mRadius2 multiplier used to create a fat LSS
+	};
+
+	class OPCODE_API LSSCollider : public VolumeCollider
+	{
+		public:
+		// Constructor / Destructor
+											LSSCollider();
+		virtual								~LSSCollider();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic collision query for generic OPCODE models. After the call, access the results:
+		 *	- with GetContactStatus()
+		 *	- with GetNbTouchedPrimitives()
+		 *	- with GetTouchedPrimitives()
+		 *
+		 *	\param		cache			[in/out] an lss cache
+		 *	\param		lss				[in] collision lss in local space
+		 *	\param		model			[in] Opcode model to collide with
+		 *	\param		worldl			[in] lss world matrix, or null
+		 *	\param		worldm			[in] model's world matrix, or null
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        //bool			Collide(LSSCache& cache, const LSS& lss, const Model& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(LSSCache& cache, const LSS& lss, Model& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
+		// 
+							bool			Collide(LSSCache& cache, const LSS& lss, const AABBTree* tree);
+		protected:
+		// LSS in model space
+							Segment			mSeg;			//!< Segment
+							float			mRadius2;		//!< LSS radius squared
+		// Internal methods
+							void			_Collide(const AABBCollisionNode* node);
+							void			_Collide(const AABBNoLeafNode* node);
+							void			_Collide(const AABBQuantizedNode* node);
+							void			_Collide(const AABBQuantizedNoLeafNode* node);
+							void			_Collide(const AABBTreeNode* node);
+							void			_CollideNoPrimitiveTest(const AABBCollisionNode* node);
+							void			_CollideNoPrimitiveTest(const AABBNoLeafNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
+			// Overlap tests
+		inline_				BOOL			LSSContainsBox(const Point& bc, const Point& be);
+		inline_				BOOL			LSSAABBOverlap(const Point& center, const Point& extents);
+		inline_				BOOL			LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2);
+			// Init methods
+							BOOL			InitQuery(LSSCache& cache, const LSS& lss, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
+	};
+
+	class OPCODE_API HybridLSSCollider : public LSSCollider
+	{
+		public:
+		// Constructor / Destructor
+											HybridLSSCollider();
+		virtual								~HybridLSSCollider();
+
+        //bool			Collide(LSSCache& cache, const LSS& lss, const HybridModel& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(LSSCache& cache, const LSS& lss, HybridModel& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
+		protected:
+							Container		mTouchedBoxes;
+	};
+
+#endif // __OPC_LSSCOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_LSSTriOverlap.h

@@ -0,0 +1,679 @@
+// Following code from Magic-Software (http://www.magic-software.com/)
+// A bit modified for Opcode
+
+static const float gs_fTolerance = 1e-05f;
+
+static float OPC_PointTriangleSqrDist(const Point& point, const Point& p0, const Point& p1, const Point& p2)
+{
+	// Hook
+	Point TriEdge0 = p1 - p0;
+	Point TriEdge1 = p2 - p0;
+
+	Point kDiff	= p0 - point;
+	float fA00	= TriEdge0.SquareMagnitude();
+	float fA01	= TriEdge0 | TriEdge1;
+	float fA11	= TriEdge1.SquareMagnitude();
+	float fB0	= kDiff | TriEdge0;
+	float fB1	= kDiff | TriEdge1;
+	float fC	= kDiff.SquareMagnitude();
+	float fDet	= fabsf(fA00*fA11 - fA01*fA01);
+	float fS	= fA01*fB1-fA11*fB0;
+	float fT	= fA01*fB0-fA00*fB1;
+	float fSqrDist;
+
+	if(fS + fT <= fDet)
+	{
+		if(fS < 0.0f)
+		{
+			if(fT < 0.0f)  // region 4
+			{
+				if(fB0 < 0.0f)
+				{
+					if(-fB0 >= fA00)		fSqrDist = fA00+2.0f*fB0+fC;
+					else					fSqrDist = fB0*(-fB0/fA00)+fC;
+				}
+				else
+				{
+					if(fB1 >= 0.0f)			fSqrDist = fC;
+					else if(-fB1 >= fA11)	fSqrDist = fA11+2.0f*fB1+fC;
+					else					fSqrDist = fB1*(-fB1/fA11)+fC;
+				}
+			}
+			else  // region 3
+			{
+				if(fB1 >= 0.0f)				fSqrDist = fC;
+				else if(-fB1 >= fA11)		fSqrDist = fA11+2.0f*fB1+fC;
+				else						fSqrDist = fB1*(-fB1/fA11)+fC;
+			}
+		}
+		else if(fT < 0.0f)  // region 5
+		{
+			if(fB0 >= 0.0f)					fSqrDist = fC;
+			else if(-fB0 >= fA00)			fSqrDist = fA00+2.0f*fB0+fC;
+			else							fSqrDist = fB0*(-fB0/fA00)+fC;
+		}
+		else  // region 0
+		{
+			// minimum at interior point
+			if(fDet==0.0f)
+			{
+				fSqrDist = MAX_FLOAT;
+			}
+			else
+			{
+				float fInvDet = 1.0f/fDet;
+				fS *= fInvDet;
+				fT *= fInvDet;
+				fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
+			}
+		}
+	}
+	else
+	{
+		float fTmp0, fTmp1, fNumer, fDenom;
+
+		if(fS < 0.0f)  // region 2
+		{
+			fTmp0 = fA01 + fB0;
+			fTmp1 = fA11 + fB1;
+			if(fTmp1 > fTmp0)
+			{
+				fNumer = fTmp1 - fTmp0;
+				fDenom = fA00-2.0f*fA01+fA11;
+				if(fNumer >= fDenom)
+				{
+					fSqrDist = fA00+2.0f*fB0+fC;
+				}
+				else
+				{
+					fS = fNumer/fDenom;
+					fT = 1.0f - fS;
+					fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
+				}
+			}
+			else
+			{
+				if(fTmp1 <= 0.0f)		fSqrDist = fA11+2.0f*fB1+fC;
+				else if(fB1 >= 0.0f)	fSqrDist = fC;
+				else					fSqrDist = fB1*(-fB1/fA11)+fC;
+			}
+		}
+		else if(fT < 0.0f)  // region 6
+		{
+			fTmp0 = fA01 + fB1;
+			fTmp1 = fA00 + fB0;
+			if(fTmp1 > fTmp0)
+			{
+				fNumer = fTmp1 - fTmp0;
+				fDenom = fA00-2.0f*fA01+fA11;
+				if(fNumer >= fDenom)
+				{
+					fSqrDist = fA11+2.0f*fB1+fC;
+				}
+				else
+				{
+					fT = fNumer/fDenom;
+					fS = 1.0f - fT;
+					fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
+				}
+			}
+			else
+			{
+				if(fTmp1 <= 0.0f)		fSqrDist = fA00+2.0f*fB0+fC;
+				else if(fB0 >= 0.0f)	fSqrDist = fC;
+				else					fSqrDist = fB0*(-fB0/fA00)+fC;
+			}
+		}
+		else  // region 1
+		{
+			fNumer = fA11 + fB1 - fA01 - fB0;
+			if(fNumer <= 0.0f)
+			{
+				fSqrDist = fA11+2.0f*fB1+fC;
+			}
+			else
+			{
+				fDenom = fA00-2.0f*fA01+fA11;
+				if(fNumer >= fDenom)
+				{
+					fSqrDist = fA00+2.0f*fB0+fC;
+				}
+				else
+				{
+					fS = fNumer/fDenom;
+					fT = 1.0f - fS;
+					fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
+				}
+			}
+		}
+	}
+	return fabsf(fSqrDist);
+}
+
+static float OPC_SegmentSegmentSqrDist(const Segment& rkSeg0, const Segment& rkSeg1)
+{
+	// Hook
+	Point rkSeg0Direction	= rkSeg0.ComputeDirection();
+	Point rkSeg1Direction	= rkSeg1.ComputeDirection();
+
+	Point kDiff	= rkSeg0.mP0 - rkSeg1.mP0;
+	float fA00	= rkSeg0Direction.SquareMagnitude();
+	float fA01	= -rkSeg0Direction.Dot(rkSeg1Direction);
+	float fA11	= rkSeg1Direction.SquareMagnitude();
+	float fB0	= kDiff.Dot(rkSeg0Direction);
+	float fC	= kDiff.SquareMagnitude();
+	float fDet	= fabsf(fA00*fA11-fA01*fA01);
+
+	float fB1, fS, fT, fSqrDist, fTmp;
+
+	if(fDet>=gs_fTolerance)
+	{
+		// line segments are not parallel
+		fB1 = -kDiff.Dot(rkSeg1Direction);
+		fS = fA01*fB1-fA11*fB0;
+		fT = fA01*fB0-fA00*fB1;
+
+		if(fS >= 0.0f)
+		{
+			if(fS <= fDet)
+			{
+				if(fT >= 0.0f)
+				{
+					if(fT <= fDet)  // region 0 (interior)
+					{
+						// minimum at two interior points of 3D lines
+						float fInvDet = 1.0f/fDet;
+						fS *= fInvDet;
+						fT *= fInvDet;
+						fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
+					}
+					else  // region 3 (side)
+					{
+						fTmp = fA01+fB0;
+						if(fTmp>=0.0f)			fSqrDist = fA11+2.0f*fB1+fC;
+						else if(-fTmp>=fA00)	fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
+						else					fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
+					}
+				}
+				else  // region 7 (side)
+				{
+					if(fB0>=0.0f)				fSqrDist = fC;
+					else if(-fB0>=fA00)			fSqrDist = fA00+2.0f*fB0+fC;
+					else						fSqrDist = fB0*(-fB0/fA00)+fC;
+				}
+			}
+			else
+			{
+				if ( fT >= 0.0 )
+				{
+					if ( fT <= fDet )  // region 1 (side)
+					{
+						fTmp = fA01+fB1;
+						if(fTmp>=0.0f)			fSqrDist = fA00+2.0f*fB0+fC;
+						else if(-fTmp>=fA11)	fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
+						else					fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
+					}
+					else  // region 2 (corner)
+					{
+						fTmp = fA01+fB0;
+						if ( -fTmp <= fA00 )
+						{
+							if(fTmp>=0.0f)		fSqrDist = fA11+2.0f*fB1+fC;
+							else				fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
+						}
+						else
+						{
+							fTmp = fA01+fB1;
+							if(fTmp>=0.0f)			fSqrDist = fA00+2.0f*fB0+fC;
+							else if(-fTmp>=fA11)	fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
+							else					fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
+						}
+					}
+				}
+				else  // region 8 (corner)
+				{
+					if ( -fB0 < fA00 )
+					{
+						if(fB0>=0.0f)	fSqrDist = fC;
+						else			fSqrDist = fB0*(-fB0/fA00)+fC;
+					}
+					else
+					{
+						fTmp = fA01+fB1;
+						if(fTmp>=0.0f)			fSqrDist = fA00+2.0f*fB0+fC;
+						else if(-fTmp>=fA11)	fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
+						else					fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
+					}
+				}
+			}
+		}
+		else 
+		{
+			if ( fT >= 0.0f )
+			{
+				if ( fT <= fDet )  // region 5 (side)
+				{
+					if(fB1>=0.0f)		fSqrDist = fC;
+					else if(-fB1>=fA11)	fSqrDist = fA11+2.0f*fB1+fC;
+					else				fSqrDist = fB1*(-fB1/fA11)+fC;
+				}
+				else  // region 4 (corner)
+				{
+					fTmp = fA01+fB0;
+					if ( fTmp < 0.0f )
+					{
+						if(-fTmp>=fA00)	fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
+						else			fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
+					}
+					else
+					{
+						if(fB1>=0.0f)		fSqrDist = fC;
+						else if(-fB1>=fA11)	fSqrDist = fA11+2.0f*fB1+fC;
+						else				fSqrDist = fB1*(-fB1/fA11)+fC;
+					}
+				}
+			}
+			else   // region 6 (corner)
+			{
+				if ( fB0 < 0.0f )
+				{
+					if(-fB0>=fA00)	fSqrDist = fA00+2.0f*fB0+fC;
+					else			fSqrDist = fB0*(-fB0/fA00)+fC;
+				}
+				else
+				{
+					if(fB1>=0.0f)		fSqrDist = fC;
+					else if(-fB1>=fA11)	fSqrDist = fA11+2.0f*fB1+fC;
+					else				fSqrDist = fB1*(-fB1/fA11)+fC;
+				}
+			}
+		}
+	}
+	else
+	{
+		// line segments are parallel
+		if ( fA01 > 0.0f )
+		{
+			// direction vectors form an obtuse angle
+			if ( fB0 >= 0.0f )
+			{
+				fSqrDist = fC;
+			}
+			else if ( -fB0 <= fA00 )
+			{
+				fSqrDist = fB0*(-fB0/fA00)+fC;
+			}
+			else
+			{
+				fB1 = -kDiff.Dot(rkSeg1Direction);
+				fTmp = fA00+fB0;
+				if ( -fTmp >= fA01 )
+				{
+					fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
+				}
+				else
+				{
+					fT = -fTmp/fA01;
+					fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
+				}
+			}
+		}
+		else
+		{
+			// direction vectors form an acute angle
+			if ( -fB0 >= fA00 )
+			{
+				fSqrDist = fA00+2.0f*fB0+fC;
+			}
+			else if ( fB0 <= 0.0f )
+			{
+				fSqrDist = fB0*(-fB0/fA00)+fC;
+			}
+			else
+			{
+				fB1 = -kDiff.Dot(rkSeg1Direction);
+				if ( fB0 >= -fA01 )
+				{
+					fSqrDist = fA11+2.0f*fB1+fC;
+				}
+				else
+				{
+					fT = -fB0/fA01;
+					fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);
+				}
+			}
+		}
+	}
+	return fabsf(fSqrDist);
+}
+
+inline_ float OPC_SegmentRaySqrDist(const Segment& rkSeg0, const Ray& rkSeg1)
+{
+	return OPC_SegmentSegmentSqrDist(rkSeg0, Segment(rkSeg1.mOrig, rkSeg1.mOrig + rkSeg1.mDir));
+}
+
+static float OPC_SegmentTriangleSqrDist(const Segment& segment, const Point& p0, const Point& p1, const Point& p2)
+{
+	// Hook
+	const Point TriEdge0 = p1 - p0;
+	const Point TriEdge1 = p2 - p0;
+
+	const Point& rkSegOrigin	= segment.GetOrigin();
+	Point rkSegDirection		= segment.ComputeDirection();
+
+	Point kDiff = p0 - rkSegOrigin;
+	float fA00 = rkSegDirection.SquareMagnitude();
+	float fA01 = -rkSegDirection.Dot(TriEdge0);
+	float fA02 = -rkSegDirection.Dot(TriEdge1);
+	float fA11 = TriEdge0.SquareMagnitude();
+	float fA12 = TriEdge0.Dot(TriEdge1);
+	float fA22 = TriEdge1.Dot(TriEdge1);
+	float fB0  = -kDiff.Dot(rkSegDirection);
+	float fB1  = kDiff.Dot(TriEdge0);
+	float fB2  = kDiff.Dot(TriEdge1);
+	float fCof00 = fA11*fA22-fA12*fA12;
+	float fCof01 = fA02*fA12-fA01*fA22;
+	float fCof02 = fA01*fA12-fA02*fA11;
+	float fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
+
+	Ray kTriSeg;
+	Point kPt;
+	float fSqrDist, fSqrDist0;
+
+	if(fabsf(fDet)>=gs_fTolerance)
+	{
+		float fCof11 = fA00*fA22-fA02*fA02;
+		float fCof12 = fA02*fA01-fA00*fA12;
+		float fCof22 = fA00*fA11-fA01*fA01;
+		float fInvDet = 1.0f/fDet;
+		float fRhs0 = -fB0*fInvDet;
+		float fRhs1 = -fB1*fInvDet;
+		float fRhs2 = -fB2*fInvDet;
+
+		float fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
+		float fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
+		float fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
+
+		if ( fR < 0.0f )
+		{
+			if ( fS+fT <= 1.0f )
+			{
+				if ( fS < 0.0f )
+				{
+					if ( fT < 0.0f )  // region 4m
+					{
+						// min on face s=0 or t=0 or r=0
+						kTriSeg.mOrig = p0;
+						kTriSeg.mDir = TriEdge1;
+						fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+						kTriSeg.mOrig = p0;
+						kTriSeg.mDir = TriEdge0;
+						fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+						if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+						fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+						if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    }
+                    else  // region 3m
+                    {
+                        // min on face s=0 or r=0
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge1;
+                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                        fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+                        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    }
+                }
+                else if ( fT < 0.0f )  // region 5m
+                {
+                    // min on face t=0 or r=0
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else  // region 0m
+                {
+                    // min on face r=0
+                    fSqrDist = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+                }
+            }
+            else
+            {
+                if ( fS < 0.0f )  // region 2m
+                {
+                    // min on face s=0 or s+t=1 or r=0
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge1;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else if ( fT < 0.0f )  // region 6m
+                {
+                    // min on face t=0 or s+t=1 or r=0
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else  // region 1m
+                {
+                    // min on face s+t=1 or r=0
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+            }
+        }
+        else if ( fR <= 1.0f )
+        {
+            if ( fS+fT <= 1.0f )
+            {
+                if ( fS < 0.0f )
+                {
+                    if ( fT < 0.0f )  // region 4
+                    {
+                        // min on face s=0 or t=0
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge1;
+                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge0;
+                        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    }
+                    else  // region 3
+                    {
+                        // min on face s=0
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge1;
+                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    }
+                }
+                else if ( fT < 0.0f )  // region 5
+                {
+                    // min on face t=0
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                }
+                else  // region 0
+                {
+                    // global minimum is interior, done
+                    fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0f*fB0)
+                          +fS*(fA01*fR+fA11*fS+fA12*fT+2.0f*fB1)
+                          +fT*(fA02*fR+fA12*fS+fA22*fT+2.0f*fB2)
+                          +kDiff.SquareMagnitude();
+                }
+            }
+            else
+            {
+                if ( fS < 0.0f )  // region 2
+                {
+                    // min on face s=0 or s+t=1
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge1;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else if ( fT < 0.0f )  // region 6
+                {
+                    // min on face t=0 or s+t=1
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else  // region 1
+                {
+                    // min on face s+t=1
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                }
+            }
+        }
+        else  // fR > 1
+        {
+            if ( fS+fT <= 1.0f )
+            {
+                if ( fS < 0.0f )
+                {
+                    if ( fT < 0.0f )  // region 4p
+                    {
+                        // min on face s=0 or t=0 or r=1
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge1;
+                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge0;
+                        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                        kPt = rkSegOrigin+rkSegDirection;
+                        fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    }
+                    else  // region 3p
+                    {
+                        // min on face s=0 or r=1
+                        kTriSeg.mOrig = p0;
+                        kTriSeg.mDir = TriEdge1;
+                        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                        kPt = rkSegOrigin+rkSegDirection;
+                        fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    }
+                }
+                else if ( fT < 0.0f )  // region 5p
+                {
+                    // min on face t=0 or r=1
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kPt = rkSegOrigin+rkSegDirection;
+                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else  // region 0p
+                {
+                    // min face on r=1
+                    kPt = rkSegOrigin+rkSegDirection;
+                    fSqrDist = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                }
+            }
+            else
+            {
+                if ( fS < 0.0f )  // region 2p
+                {
+                    // min on face s=0 or s+t=1 or r=1
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge1;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    kPt = rkSegOrigin+rkSegDirection;
+                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else if ( fT < 0.0f )  // region 6p
+                {
+                    // min on face t=0 or s+t=1 or r=1
+                    kTriSeg.mOrig = p0;
+                    kTriSeg.mDir = TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                    kPt = rkSegOrigin+rkSegDirection;
+                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+                else  // region 1p
+                {
+                    // min on face s+t=1 or r=1
+                    kTriSeg.mOrig = p1;
+                    kTriSeg.mDir = TriEdge1-TriEdge0;
+                    fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+                    kPt = rkSegOrigin+rkSegDirection;
+                    fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+                    if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+                }
+            }
+        }
+    }
+    else
+    {
+        // segment and triangle are parallel
+        kTriSeg.mOrig = p0;
+        kTriSeg.mDir = TriEdge0;
+        fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
+
+        kTriSeg.mDir = TriEdge1;
+        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+
+        kTriSeg.mOrig = p1;
+        kTriSeg.mDir = TriEdge1 - TriEdge0;
+        fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
+        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+
+        fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
+        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+
+        kPt = rkSegOrigin+rkSegDirection;
+        fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
+        if(fSqrDist0<fSqrDist)	fSqrDist = fSqrDist0;
+    }
+    return fabsf(fSqrDist);
+}
+
+inline_ BOOL LSSCollider::LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
+{
+	// Stats
+	mNbVolumePrimTests++;
+
+	float s2 = OPC_SegmentTriangleSqrDist(mSeg, vert0, vert1, vert2);
+	if(s2<mRadius2)	return TRUE;
+	return FALSE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_MeshInterface.cpp

@@ -0,0 +1,316 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a mesh interface.
+ *	\file		OPC_MeshInterface.cpp
+ *	\author		Pierre Terdiman
+ *	\date		November, 27, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	This structure holds 3 vertex-pointers. It's mainly used by collision callbacks so that the app doesn't have
+ *	to return 3 vertices to OPCODE (36 bytes) but only 3 pointers (12 bytes). It seems better but I never profiled
+ *	the alternative.
+ *
+ *	\class		VertexPointers
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	This class is an interface between us and user-defined meshes. Meshes can be defined in a lot of ways, and here we
+ *	try to support most of them.
+ *
+ *	Basically you have two options:
+ *	- callbacks, if OPC_USE_CALLBACKS is defined in OPC_Settings.h.
+ *	- else pointers.
+ *
+ *	If using pointers, you can also use strides or not. Strides are used when OPC_USE_STRIDE is defined.
+ *
+ *
+ *	CALLBACKS:
+ *
+ *	Using callbacks is the most generic way to feed OPCODE with your meshes. Indeed, you just have to give
+ *	access to three vertices at the end of the day. It's up to you to fetch them from your database, using
+ *	whatever method you want. Hence your meshes can lie in system memory or AGP, be indexed or not, use 16
+ *	or 32-bits indices, you can decompress them on-the-fly if needed, etc. On the other hand, a callback is
+ *	called each time OPCODE needs access to a particular triangle, so there might be a slight overhead.
+ *
+ *	To make things clear: geometry & topology are NOT stored in the collision system,
+ *	in order to save some ram. So, when the system needs them to perform accurate intersection
+ *	tests, you're requested to provide the triangle-vertices corresponding to a given face index.
+ *
+ *	Ex:
+ *
+ *	\code
+ *		static void ColCallback(udword triangle_index, VertexPointers& triangle, udword user_data)
+ *		{
+ *			// Get back Mesh0 or Mesh1 (you also can use 2 different callbacks)
+ *			Mesh* MyMesh = (Mesh*)user_data;
+ *			// Get correct triangle in the app-controlled database
+ *			const Triangle* Tri = MyMesh->GetTriangle(triangle_index);
+ *			// Setup pointers to vertices for the collision system
+ *			triangle.Vertex[0] = MyMesh->GetVertex(Tri->mVRef[0]);
+ *			triangle.Vertex[1] = MyMesh->GetVertex(Tri->mVRef[1]);
+ *			triangle.Vertex[2] = MyMesh->GetVertex(Tri->mVRef[2]);
+ *		}
+ *
+ *		// Setup callbacks
+ *		MeshInterface0->SetCallback(ColCallback, udword(Mesh0));
+ *		MeshInterface1->SetCallback(ColCallback, udword(Mesh1));
+ *	\endcode
+ *
+ *	Of course, you should make this callback as fast as possible. And you're also not supposed
+ *	to modify the geometry *after* the collision trees have been built. The alternative was to
+ *	store the geometry & topology in the collision system as well (as in RAPID) but we have found
+ *	this approach to waste a lot of ram in many cases.
+ *
+ *
+ *	POINTERS:
+ *
+ *	If you're internally using the following canonical structures:
+ *	- a vertex made of three 32-bits floating point values
+ *	- a triangle made of three 32-bits integer vertex references
+ *	...then you may want to use pointers instead of callbacks. This is the same, except OPCODE will directly
+ *	use provided pointers to access the topology and geometry, without using a callback. It might be faster,
+ *	but probably not as safe. Pointers have been introduced in OPCODE 1.2.
+ *
+ *	Ex:
+ *
+ *	\code
+ *		// Setup pointers
+ *		MeshInterface0->SetPointers(Mesh0->GetFaces(), Mesh0->GetVerts());
+ *		MeshInterface1->SetPointers(Mesh1->GetFaces(), Mesh1->GetVerts());
+ *	\endcode
+ *
+ *
+ *	STRIDES:
+ *
+ *	If your vertices are D3D-like entities interleaving a position, a normal and/or texture coordinates
+ *	(i.e. if your vertices are FVFs), you might want to use a vertex stride to skip extra data OPCODE
+ *	doesn't need. Using a stride shouldn't be notably slower than not using it, but it might increase
+ *	cache misses. Please also note that you *shouldn't* read from AGP or video-memory buffers !
+ *
+ *
+ *	In any case, compilation flags are here to select callbacks/pointers/strides at compile time, so
+ *	choose what's best for your application. All of this has been wrapped into this MeshInterface.
+ *
+ *	\class		MeshInterface
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		November, 27, 2002
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+// ericf change
+// Point MeshInterface::VertexCache[3];
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+MeshInterface::MeshInterface() :
+#ifdef OPC_USE_CALLBACKS
+	mUserData		(null),
+	mObjCallback	(null),
+	mNbTris			(0),
+	mNbVerts		(0),
+#else
+	mNbTris			(0),
+	mNbVerts		(0),
+	mTris			(null),
+	mVerts			(null),
+# ifdef OPC_USE_STRIDE
+	mTriStride		(sizeof(IndexedTriangle)),
+	mVertexStride	(sizeof(Point)),
+# endif
+#endif
+
+	Single(true)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+MeshInterface::~MeshInterface()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the mesh interface is valid, i.e. things have been setup correctly.
+ *	\return		true if valid
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool MeshInterface::IsValid() const
+{
+	if(!mNbTris || !mNbVerts)	return false;
+#ifdef OPC_USE_CALLBACKS
+	if(!mObjCallback)			return false;
+#else
+	if(!mTris || !mVerts)		return false;
+#endif
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the mesh itself is valid.
+ *	Currently we only look for degenerate faces.
+ *	\return		number of degenerate faces
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//udword MeshInterface::CheckTopology()	const
+// ericf change
+udword MeshInterface::CheckTopology()
+{
+	// Check topology. If the model contains degenerate faces, collision report can be wrong in some cases.
+	// e.g. it happens with the standard MAX teapot. So clean your meshes first... If you don't have a mesh cleaner
+	// you can try this: www.codercorner.com/Consolidation.zip
+
+	udword NbDegenerate = 0;
+
+	VertexPointers VP;
+
+	// Using callbacks, we don't have access to vertex indices. Nevertheless we still can check for
+	// redundant vertex pointers, which cover all possibilities (callbacks/pointers/strides).
+	for(udword i=0;i<mNbTris;i++)
+	{
+		GetTriangle(VP, i);
+
+		if(		(VP.Vertex[0]==VP.Vertex[1])
+			||	(VP.Vertex[1]==VP.Vertex[2])
+			||	(VP.Vertex[2]==VP.Vertex[0]))	NbDegenerate++;
+	}
+
+	return NbDegenerate;
+}
+
+#ifdef OPC_USE_CALLBACKS
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
+ *	\param		callback	[in] user-defined callback
+ *	\param		user_data	[in] user-defined data
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool MeshInterface::SetCallback(RequestCallback callback, void* user_data)
+{
+	if(!callback)	return SetIceError("MeshInterface::SetCallback: callback pointer is null");
+
+	mObjCallback	= callback;
+	mUserData		= user_data;
+	return true;
+}
+#else
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Pointers control: setups object pointers. Must provide access to faces and vertices for a given object.
+ *	\param		tris	[in] pointer to triangles
+ *	\param		verts	[in] pointer to vertices
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool MeshInterface::SetPointers(const IndexedTriangle* tris, const Point* verts)
+{
+	if(!tris || !verts)	return SetIceError("MeshInterface::SetPointers: pointer is null", null);
+
+	mTris	= tris;
+	mVerts	= verts;
+	return true;
+}
+#ifdef OPC_USE_STRIDE
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Strides control
+ *	\param		tri_stride		[in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices.
+ *	\param		vertex_stride	[in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position.
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool MeshInterface::SetStrides(udword tri_stride, udword vertex_stride)
+{
+	if(tri_stride<sizeof(IndexedTriangle))	return SetIceError("MeshInterface::SetStrides: invalid triangle stride", null);
+	if(vertex_stride<sizeof(Point))			return SetIceError("MeshInterface::SetStrides: invalid vertex stride", null);
+
+	mTriStride		= tri_stride;
+	mVertexStride	= vertex_stride;
+	return true;
+}
+#endif
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Remaps client's mesh according to a permutation.
+ *	\param		nb_indices	[in] number of indices in the permutation (will be checked against number of triangles)
+ *	\param		permutation	[in] list of triangle indices
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool MeshInterface::RemapClient(udword nb_indices, const udword* permutation) const
+{
+	// Checkings
+	if(!nb_indices || !permutation)	return false;
+	if(nb_indices!=mNbTris)			return false;
+
+#ifdef OPC_USE_CALLBACKS
+	// We can't really do that using callbacks
+	return false;
+#else
+	IndexedTriangle* Tmp = new IndexedTriangle[mNbTris];
+	CHECKALLOC(Tmp);
+
+	#ifdef OPC_USE_STRIDE
+	udword Stride = mTriStride;
+	#else
+	udword Stride = sizeof(IndexedTriangle);
+	#endif
+
+	for(udword i=0;i<mNbTris;i++)
+	{
+		const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + i * Stride);
+		Tmp[i] = *T;
+	}
+
+	for(udword i=0;i<mNbTris;i++)
+	{
+		IndexedTriangle* T = (IndexedTriangle*)(((ubyte*)mTris) + i * Stride);
+		*T = Tmp[permutation[i]];
+	}
+
+	DELETEARRAY(Tmp);
+#endif
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_MeshInterface.h

@@ -0,0 +1,205 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a mesh interface.
+ *	\file		OPC_MeshInterface.h
+ *	\author		Pierre Terdiman
+ *	\date		November, 27, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_MESHINTERFACE_H__
+#define __OPC_MESHINTERFACE_H__
+
+	struct VertexPointers
+	{
+		const Point*	Vertex[3];
+
+		bool BackfaceCulling(const Point& source)
+		{
+			const Point& p0 = *Vertex[0];
+			const Point& p1 = *Vertex[1];
+			const Point& p2 = *Vertex[2];
+
+			// Compute normal direction
+			Point Normal = (p2 - p1)^(p0 - p1);
+
+			// Backface culling
+			return (Normal | (source - p0)) >= 0.0f;
+		}
+	};
+
+#ifdef OPC_USE_CALLBACKS
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	User-callback, called by OPCODE to request vertices from the app.
+	 *	\param		triangle_index	[in] face index for which the system is requesting the vertices
+	 *	\param		triangle		[out] triangle's vertices (must be provided by the user)
+	 *	\param		user_data		[in] user-defined data from SetCallback()
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	typedef void	(*RequestCallback)	(udword triangle_index, VertexPointers& triangle, void* user_data);
+#endif
+
+	class OPCODE_API MeshInterface
+	{
+		public:
+		// Constructor / Destructor
+											MeshInterface();
+											~MeshInterface();
+		// Common settings
+		inline_			udword				GetNbTriangles()	const	{ return mNbTris;	}
+		inline_			udword				GetNbVertices()		const	{ return mNbVerts;	}
+		inline_			void				SetNbTriangles(udword nb)	{ mNbTris = nb;		}
+		inline_			void				SetNbVertices(udword nb)	{ mNbVerts = nb;	}
+
+#ifdef OPC_USE_CALLBACKS
+		// Callback settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
+		 *	\param		callback	[in] user-defined callback
+		 *	\param		user_data	[in] user-defined data
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						bool				SetCallback(RequestCallback callback, void* user_data);
+		inline_			void*				GetUserData()		const	{ return mUserData;		}
+		inline_			RequestCallback		GetCallback()		const	{ return mObjCallback;	}
+#else
+		// Pointers settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Pointers control: setups object pointers. Must provide access to faces and vertices for a given object.
+		 *	\param		tris	[in] pointer to triangles
+		 *	\param		verts	[in] pointer to vertices
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						bool				SetPointers(const IndexedTriangle* tris, const Point* verts);
+		inline_	const	IndexedTriangle*	GetTris()			const	{ return mTris;			}
+		inline_	const	Point*				GetVerts()			const	{ return mVerts;		}
+
+	#ifdef OPC_USE_STRIDE
+		// Strides settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Strides control
+		 *	\param		tri_stride		[in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices.
+		 *	\param		vertex_stride	[in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position.
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						bool				SetStrides(udword tri_stride=sizeof(IndexedTriangle), udword vertex_stride=sizeof(Point));
+		inline_			udword				GetTriStride()		const	{ return mTriStride;	}
+		inline_			udword				GetVertexStride()	const	{ return mVertexStride;	}
+	#endif
+#endif
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Fetches a triangle given a triangle index.
+		 *	\param		vp		[out] required triangle's vertex pointers
+		 *	\param		index	[in] triangle index
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//inline_			void				GetTriangle(VertexPointers& vp, udword index)	const
+		inline_			void				GetTriangle(VertexPointers& vp, udword index)
+											{
+#ifdef OPC_USE_CALLBACKS
+												(mObjCallback)(index, vp, mUserData);
+#else
+	#ifdef OPC_USE_STRIDE
+												const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride);
+
+												if (Single){
+													vp.Vertex[0] = (const Point*)(((ubyte*)mVerts) + T->mVRef[0] * mVertexStride);
+													vp.Vertex[1] = (const Point*)(((ubyte*)mVerts) + T->mVRef[1] * mVertexStride);
+													vp.Vertex[2] = (const Point*)(((ubyte*)mVerts) + T->mVRef[2] * mVertexStride);
+												}
+												else{
+													for (int i = 0; i < 3; i++){
+														const double* v = (const double*)(((ubyte*)mVerts) + T->mVRef[i] * mVertexStride);
+
+														VertexCache[i].x = (float)v[0];
+														VertexCache[i].y = (float)v[1];
+														VertexCache[i].z = (float)v[2];
+														vp.Vertex[i] = &VertexCache[i];
+													}
+												}
+	#else
+												const IndexedTriangle* T = &mTris[index];
+												vp.Vertex[0] = &mVerts[T->mVRef[0]];
+												vp.Vertex[1] = &mVerts[T->mVRef[1]];
+												vp.Vertex[2] = &mVerts[T->mVRef[2]];
+	#endif
+#endif
+											}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Remaps client's mesh according to a permutation.
+		 *	\param		nb_indices	[in] number of indices in the permutation (will be checked against number of triangles)
+		 *	\param		permutation	[in] list of triangle indices
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		bool				RemapClient(udword nb_indices, const udword* permutation)	const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the mesh interface is valid, i.e. things have been setup correctly.
+		 *	\return		true if valid
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+						bool				IsValid()		const;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Checks the mesh itself is valid.
+		 *	Currently we only look for degenerate faces.
+		 *	\return		number of degenerate faces
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+                        // ericf change
+						udword				CheckTopology();
+						//udword				CheckTopology()	const;
+		private:
+
+						udword				mNbTris;			//!< Number of triangles in the input model
+						udword				mNbVerts;			//!< Number of vertices in the input model
+#ifdef OPC_USE_CALLBACKS
+		// User callback
+						void*				mUserData;			//!< User-defined data sent to callback
+						RequestCallback		mObjCallback;		//!< Object callback
+#else
+		// User pointers
+				const	IndexedTriangle*	mTris;				//!< Array of indexed triangles
+				const	Point*				mVerts;				//!< Array of vertices
+#   ifdef OPC_USE_STRIDE
+						udword				mTriStride;			//!< Possible triangle stride in bytes [Opcode 1.3]
+						udword				mVertexStride;		//!< Possible vertex stride in bytes [Opcode 1.3]
+#   endif
+		public:
+						bool Single;							//!< Use single or double precision vertices
+		private:
+						//static Point VertexCache[3];
+
+                        // ericf change for multithreading
+						Point VertexCache[3];
+#endif
+	};
+
+#endif //__OPC_MESHINTERFACE_H__

src/external/open_dynamics_engine-ef/ode/OPC_Model.cpp

@@ -0,0 +1,230 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for OPCODE models.
+ *	\file		OPC_Model.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	The main collision wrapper, for all trees. Supported trees are:
+ *	- Normal trees (2*N-1 nodes, full size)
+ *	- No-leaf trees (N-1 nodes, full size)
+ *	- Quantized trees (2*N-1 nodes, half size)
+ *	- Quantized no-leaf trees (N-1 nodes, half size)
+ *
+ *	Usage:
+ *
+ *	1) Create a static mesh interface using callbacks or pointers. (see OPC_MeshInterface.cpp).
+ *	Keep it around in your app, since a pointer to this interface is saved internally and
+ *	used until you release the collision structures.
+ *
+ *	2) Build a Model using a creation structure:
+ *
+ *	\code
+ *		Model Sample;
+ *
+ *		OPCODECREATE OPCC;
+ *		OPCC.IMesh			= ...;
+ *		OPCC.Rules			= ...;
+ *		OPCC.NoLeaf			= ...;
+ *		OPCC.Quantized		= ...;
+ *		OPCC.KeepOriginal	= ...;
+ *		bool Status = Sample.Build(OPCC);
+ *	\endcode
+ *
+ *	3) Create a tree collider and set it up:
+ *
+ *	\code
+ *		AABBTreeCollider TC;
+ *		TC.SetFirstContact(...);
+ *		TC.SetFullBoxBoxTest(...);
+ *		TC.SetFullPrimBoxTest(...);
+ *		TC.SetTemporalCoherence(...);
+ *	\endcode
+ *
+ *	4) Perform a collision query
+ *
+ *	\code
+ *		// Setup cache
+ *		static BVTCache ColCache;
+ *		ColCache.Model0 = &Model0;
+ *		ColCache.Model1 = &Model1;
+ *
+ *		// Collision query
+ *		bool IsOk = TC.Collide(ColCache, World0, World1);
+ *
+ *		// Get collision status => if true, objects overlap
+ *		BOOL Status = TC.GetContactStatus();
+ *
+ *		// Number of colliding pairs and list of pairs
+ *		udword NbPairs = TC.GetNbPairs();
+ *		const Pair* p = TC.GetPairs()
+ *	\endcode
+ *
+ *	5) Stats
+ *
+ *	\code
+ *		Model0.GetUsedBytes()	= number of bytes used for this collision tree
+ *		TC.GetNbBVBVTests()		= number of BV-BV overlap tests performed during last query
+ *		TC.GetNbPrimPrimTests()	= number of Triangle-Triangle overlap tests performed during last query
+ *		TC.GetNbBVPrimTests()	= number of Triangle-BV overlap tests performed during last query
+ *	\endcode
+ *
+ *	\class		Model
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Model::Model()
+{
+#ifdef __MESHMERIZER_H__	// Collision hulls only supported within ICE !
+	mHull	= null;
+#endif // __MESHMERIZER_H__
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Model::~Model()
+{
+	Release();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Releases the model.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void Model::Release()
+{
+	ReleaseBase();
+#ifdef __MESHMERIZER_H__	// Collision hulls only supported within ICE !
+	DELETESINGLE(mHull);
+#endif // __MESHMERIZER_H__
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds a collision model.
+ *	\param		create		[in] model creation structure
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool Model::Build(const OPCODECREATE& create)
+{
+	// 1) Checkings
+	if(!create.mIMesh || !create.mIMesh->IsValid())	return false;
+
+	// For this model, we only support complete trees
+	if(create.mSettings.mLimit!=1)	return SetIceError("OPCODE WARNING: supports complete trees only! Use mLimit = 1.\n", null);
+
+	// Look for degenerate faces.
+	udword NbDegenerate = create.mIMesh->CheckTopology();
+	if(NbDegenerate)	Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate);
+	// We continue nonetheless....
+
+	Release();	// Make sure previous tree has been discarded [Opcode 1.3, thanks Adam]
+
+	// 1-1) Setup mesh interface automatically [Opcode 1.3]
+	SetMeshInterface(create.mIMesh);
+
+	// Special case for 1-triangle meshes [Opcode 1.3]
+	udword NbTris = create.mIMesh->GetNbTriangles();
+	if(NbTris==1)
+	{
+		// We don't need to actually create a tree here, since we'll only have a single triangle to deal with anyway.
+		// It's a waste to use a "model" for this but at least it will work.
+		mModelCode |= OPC_SINGLE_NODE;
+		return true;
+	}
+
+	// 2) Build a generic AABB Tree.
+	mSource = new AABBTree;
+	CHECKALLOC(mSource);
+
+	// 2-1) Setup a builder. Our primitives here are triangles from input mesh,
+	// so we use an AABBTreeOfTrianglesBuilder.....
+	{
+		AABBTreeOfTrianglesBuilder TB;
+		TB.mIMesh			= create.mIMesh;
+		TB.mSettings		= create.mSettings;
+		TB.mNbPrimitives	= NbTris;
+		if(!mSource->Build(&TB))	return false;
+	}
+
+	// 3) Create an optimized tree according to user-settings
+	if(!CreateTree(create.mNoLeaf, create.mQuantized))	return false;
+
+	// 3-2) Create optimized tree
+	if(!mTree->Build(mSource))	return false;
+
+	// 3-3) Delete generic tree if needed
+	if(!create.mKeepOriginal)	DELETESINGLE(mSource);
+
+#ifdef __MESHMERIZER_H__
+	// 4) Convex hull
+	if(create.mCollisionHull)
+	{
+		// Create hull
+		mHull = new CollisionHull;
+		CHECKALLOC(mHull);
+
+		CONVEXHULLCREATE CHC;
+		// ### doesn't work with strides
+		CHC.NbVerts			= create.mIMesh->GetNbVertices();
+		CHC.Vertices		= create.mIMesh->GetVerts();
+		CHC.UnifyNormals	= true;
+		CHC.ReduceVertices	= true;
+		CHC.WordFaces		= false;
+		mHull->Compute(CHC);
+	}
+#endif // __MESHMERIZER_H__
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets the number of bytes used by the tree.
+ *	\return		amount of bytes used
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword Model::GetUsedBytes() const
+{
+	if(!mTree)	return 0;
+	return mTree->GetUsedBytes();
+}

src/external/open_dynamics_engine-ef/ode/OPC_Model.h

@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for OPCODE models.
+ *	\file		OPC_Model.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_MODEL_H__
+#define __OPC_MODEL_H__
+
+	class OPCODE_API Model : public BaseModel
+	{
+		public:
+		// Constructor/Destructor
+												Model();
+		virtual									~Model();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Builds a collision model.
+		 *	\param		create		[in] model creation structure
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(BaseModel)	bool				Build(const OPCODECREATE& create);
+
+#ifdef __MESHMERIZER_H__
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the collision hull.
+		 *	\return		the collision hull if it exists
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_	const		CollisionHull*		GetHull()		const	{ return mHull;		}
+#endif // __MESHMERIZER_H__
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the number of bytes used by the tree.
+		 *	\return		amount of bytes used
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(BaseModel)	udword				GetUsedBytes()	const;
+
+		private:
+#ifdef __MESHMERIZER_H__
+							CollisionHull*		mHull;			//!< Possible convex hull
+#endif // __MESHMERIZER_H__
+		// Internal methods
+							void				Release();
+	};
+
+#endif //__OPC_MODEL_H__

src/external/open_dynamics_engine-ef/ode/OPC_OBBCollider.cpp

@@ -0,0 +1,779 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for an OBB collider.
+ *	\file		OPC_OBBCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 1st, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains an OBB-vs-tree collider.
+ *
+ *	\class		OBBCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		January, 1st, 2002
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_BoxBoxOverlap.h"
+#include "ode/OPC_TriBoxOverlap.h"
+
+#define SET_CONTACT(prim_index, flag)											\
+	/* Set contact status */													\
+	mFlags |= flag;																\
+	mTouchedPrimitives->Add(prim_index);
+
+//! OBB-triangle test
+#define OBB_PRIM(prim_index, flag)												\
+	/* Request vertices from the app */											\
+	VertexPointers VP;	mIMesh->GetTriangle(VP, prim_index);					\
+	/* Transform them in a common space */										\
+	TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox);	\
+	TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox);	\
+	TransformPoint(mLeafVerts[2], *VP.Vertex[2], mRModelToBox, mTModelToBox);	\
+	/* Perform triangle-box overlap test */										\
+	if(TriBoxOverlap())															\
+	{																			\
+		SET_CONTACT(prim_index, flag)											\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+OBBCollider::OBBCollider() : mFullBoxBoxTest(true)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+OBBCollider::~OBBCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Validates current settings. You should call this method after all the settings and callbacks have been defined.
+ *	\return		null if everything is ok, else a string describing the problem
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const char* OBBCollider::ValidateSettings()
+{
+	if(TemporalCoherenceEnabled() && !FirstContactEnabled())	return "Temporal coherence only works with ""First contact"" mode!";
+
+	return VolumeCollider::ValidateSettings();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic collision query for generic OPCODE models. After the call, access the results:
+ *	- with GetContactStatus()
+ *	- with GetNbTouchedPrimitives()
+ *	- with GetTouchedPrimitives()
+ *
+ *	\param		cache		[in/out] a box cache
+ *	\param		box			[in] collision OBB in local space
+ *	\param		model		[in] Opcode model to collide with
+ *	\param		worldb		[in] OBB's world matrix, or null
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
+// ericf change
+bool OBBCollider::Collide(OBBCache& cache, const OBB& box, Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
+{
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, box, worldb, worldm))	return true;
+
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a collision query :
+ *	- reset stats & contact status
+ *	- setup matrices
+ *	- check temporal coherence
+ *
+ *	\param		cache		[in/out] a box cache
+ *	\param		box			[in] obb in local space
+ *	\param		worldb		[in] obb's world matrix, or null
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		TRUE if we can return immediately
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL OBBCollider::InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb, const Matrix4x4* worldm)
+{
+	// 1) Call the base method
+	VolumeCollider::InitQuery();
+
+	// 2) Compute obb in world space
+	mBoxExtents = box.mExtents;
+
+	Matrix4x4 WorldB;
+
+	if(worldb)
+	{
+		WorldB = Matrix4x4( box.mRot * Matrix3x3(*worldb) );
+		WorldB.SetTrans(box.mCenter * *worldb);
+	}
+	else
+	{
+		WorldB = box.mRot;
+		WorldB.SetTrans(box.mCenter);
+	}
+
+	// Setup matrices
+	Matrix4x4 InvWorldB;
+	InvertPRMatrix(InvWorldB, WorldB);
+
+	if(worldm)
+	{
+		Matrix4x4 InvWorldM;
+		InvertPRMatrix(InvWorldM, *worldm);
+
+		Matrix4x4 WorldBtoM = WorldB * InvWorldM;
+		Matrix4x4 WorldMtoB = *worldm * InvWorldB;
+
+		mRModelToBox = WorldMtoB;		WorldMtoB.GetTrans(mTModelToBox);
+		mRBoxToModel = WorldBtoM;		WorldBtoM.GetTrans(mTBoxToModel);
+	}
+	else
+	{
+		mRModelToBox = InvWorldB;	InvWorldB.GetTrans(mTModelToBox);
+		mRBoxToModel = WorldB;		WorldB.GetTrans(mTBoxToModel);
+	}
+
+	// 3) Setup destination pointer
+	mTouchedPrimitives = &cache.TouchedPrimitives;
+
+	// 4) Special case: 1-triangle meshes [Opcode 1.3]
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		if(!SkipPrimitiveTests())
+		{
+			// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
+			mTouchedPrimitives->Reset();
+
+			// Perform overlap test between the unique triangle and the box (and set contact status if needed)
+			OBB_PRIM(udword(0), OPC_CONTACT)
+
+			// Return immediately regardless of status
+			return TRUE;
+		}
+	}
+
+	// 5) Check temporal coherence:
+	if(TemporalCoherenceEnabled())
+	{
+		// Here we use temporal coherence
+		// => check results from previous frame before performing the collision query
+		if(FirstContactEnabled())
+		{
+			// We're only interested in the first contact found => test the unique previously touched face
+			if(mTouchedPrimitives->GetNbEntries())
+			{
+				// Get index of previously touched face = the first entry in the array
+				udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
+
+				// Then reset the array:
+				// - if the overlap test below is successful, the index we'll get added back anyway
+				// - if it isn't, then the array should be reset anyway for the normal query
+				mTouchedPrimitives->Reset();
+
+				// Perform overlap test between the cached triangle and the box (and set contact status if needed)
+				OBB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
+
+				// Return immediately if possible
+				if(GetContactStatus())	return TRUE;
+			}
+			// else no face has been touched during previous query
+			// => we'll have to perform a normal query
+		}
+		else
+		{
+			// ### rewrite this
+			OBB TestBox(mTBoxToModel, mBoxExtents, mRBoxToModel);
+
+			// We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious):
+			if(IsCacheValid(cache) && TestBox.IsInside(cache.FatBox))
+			{
+				// - if N is included in P, return previous list
+				// => we simply leave the list (mTouchedFaces) unchanged
+
+				// Set contact status if needed
+				if(mTouchedPrimitives->GetNbEntries())	mFlags |= OPC_TEMPORAL_CONTACT;
+
+				// In any case we don't need to do a query
+				return TRUE;
+			}
+			else
+			{
+				// - else do the query using a fat N
+
+				// Reset cache since we'll about to perform a real query
+				mTouchedPrimitives->Reset();
+
+				// Make a fat box so that coherence will work for subsequent frames
+				TestBox.mExtents *= cache.FatCoeff;
+				mBoxExtents *= cache.FatCoeff;
+
+				// Update cache with query data (signature for cached faces)
+				cache.FatBox = TestBox;
+			}
+		}
+	}
+	else
+	{
+		// Here we don't use temporal coherence => do a normal query
+		mTouchedPrimitives->Reset();
+	}
+
+	// Now we can precompute box-box data
+
+	// Precompute absolute box-to-model rotation matrix
+	for(udword i=0;i<3;i++)
+	{
+		for(udword j=0;j<3;j++)
+		{
+			// Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
+			mAR.m[i][j] = 1e-6f + fabsf(mRBoxToModel.m[i][j]);
+		}
+	}
+
+	// Precompute bounds for box-in-box test
+	mB0 = mBoxExtents - mTModelToBox;
+	mB1 = - mBoxExtents - mTModelToBox;
+
+	// Precompute box-box data - Courtesy of Erwin de Vries
+	mBBx1 = mBoxExtents.x*mAR.m[0][0] + mBoxExtents.y*mAR.m[1][0] + mBoxExtents.z*mAR.m[2][0];
+	mBBy1 = mBoxExtents.x*mAR.m[0][1] + mBoxExtents.y*mAR.m[1][1] + mBoxExtents.z*mAR.m[2][1];
+	mBBz1 = mBoxExtents.x*mAR.m[0][2] + mBoxExtents.y*mAR.m[1][2] + mBoxExtents.z*mAR.m[2][2];
+
+	mBB_1 = mBoxExtents.y*mAR.m[2][0] + mBoxExtents.z*mAR.m[1][0];
+	mBB_2 = mBoxExtents.x*mAR.m[2][0] + mBoxExtents.z*mAR.m[0][0];
+	mBB_3 = mBoxExtents.x*mAR.m[1][0] + mBoxExtents.y*mAR.m[0][0];
+	mBB_4 = mBoxExtents.y*mAR.m[2][1] + mBoxExtents.z*mAR.m[1][1];
+	mBB_5 = mBoxExtents.x*mAR.m[2][1] + mBoxExtents.z*mAR.m[0][1];
+	mBB_6 = mBoxExtents.x*mAR.m[1][1] + mBoxExtents.y*mAR.m[0][1];
+	mBB_7 = mBoxExtents.y*mAR.m[2][2] + mBoxExtents.z*mAR.m[1][2];
+	mBB_8 = mBoxExtents.x*mAR.m[2][2] + mBoxExtents.z*mAR.m[0][2];
+	mBB_9 = mBoxExtents.x*mAR.m[1][2] + mBoxExtents.y*mAR.m[0][2];
+
+	return FALSE;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the OBB completely contains the box. In which case we can end the query sooner.
+ *	\param		bc	[in] box center
+ *	\param		be	[in] box extents
+ *	\return		true if the OBB contains the whole box
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL OBBCollider::OBBContainsBox(const Point& bc, const Point& be)
+{
+	// I assume if all 8 box vertices are inside the OBB, so does the whole box.
+	// Sounds ok but maybe there's a better way?
+/*
+#define TEST_PT(a,b,c)																												\
+	p.x=a;	p.y=b;	p.z=c;		p+=bc;																								\
+	f = p.x * mRModelToBox.m[0][0] + p.y * mRModelToBox.m[1][0] + p.z * mRModelToBox.m[2][0];	if(f>mB0.x || f<mB1.x) return FALSE;\
+	f = p.x * mRModelToBox.m[0][1] + p.y * mRModelToBox.m[1][1] + p.z * mRModelToBox.m[2][1];	if(f>mB0.y || f<mB1.y) return FALSE;\
+	f = p.x * mRModelToBox.m[0][2] + p.y * mRModelToBox.m[1][2] + p.z * mRModelToBox.m[2][2];	if(f>mB0.z || f<mB1.z) return FALSE;
+
+	Point p;
+	float f;
+
+	TEST_PT(be.x, be.y, be.z)
+	TEST_PT(-be.x, be.y, be.z)
+	TEST_PT(be.x, -be.y, be.z)
+	TEST_PT(-be.x, -be.y, be.z)
+	TEST_PT(be.x, be.y, -be.z)
+	TEST_PT(-be.x, be.y, -be.z)
+	TEST_PT(be.x, -be.y, -be.z)
+	TEST_PT(-be.x, -be.y, -be.z)
+
+	return TRUE;
+*/
+
+	// Yes there is:
+	// - compute model-box's AABB in OBB space
+	// - test AABB-in-AABB
+	float NCx = bc.x * mRModelToBox.m[0][0] + bc.y * mRModelToBox.m[1][0] + bc.z * mRModelToBox.m[2][0];
+	float NEx = fabsf(mRModelToBox.m[0][0] * be.x) + fabsf(mRModelToBox.m[1][0] * be.y) + fabsf(mRModelToBox.m[2][0] * be.z);
+
+	if(mB0.x < NCx+NEx)	return FALSE;
+	if(mB1.x > NCx-NEx)	return FALSE;
+
+	float NCy = bc.x * mRModelToBox.m[0][1] + bc.y * mRModelToBox.m[1][1] + bc.z * mRModelToBox.m[2][1];
+	float NEy = fabsf(mRModelToBox.m[0][1] * be.x) + fabsf(mRModelToBox.m[1][1] * be.y) + fabsf(mRModelToBox.m[2][1] * be.z);
+
+	if(mB0.y < NCy+NEy)	return FALSE;
+	if(mB1.y > NCy-NEy)	return FALSE;
+
+	float NCz = bc.x * mRModelToBox.m[0][2] + bc.y * mRModelToBox.m[1][2] + bc.z * mRModelToBox.m[2][2];
+	float NEz = fabsf(mRModelToBox.m[0][2] * be.x) + fabsf(mRModelToBox.m[1][2] * be.y) + fabsf(mRModelToBox.m[2][2] * be.z);
+
+	if(mB0.z < NCz+NEz)	return FALSE;
+	if(mB1.z > NCz-NEz)	return FALSE;
+
+	return TRUE;
+}
+
+#define TEST_BOX_IN_OBB(center, extents)	\
+	if(OBBContainsBox(center, extents))		\
+	{										\
+		/* Set contact status */			\
+		mFlags |= OPC_CONTACT;				\
+		_Dump(node);						\
+		return;								\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_Collide(const AABBCollisionNode* node)
+{
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
+{
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_Collide(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_OBB(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_OBB(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_Collide(const AABBNoLeafNode* node)
+{
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
+{
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))	return;
+
+	TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_Collide(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_OBB(Center, Extents)
+
+	if(node->HasPosLeaf())	{ OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform OBB-AABB overlap test
+	if(!BoxBoxOverlap(Extents, Center))	return;
+
+	TEST_BOX_IN_OBB(Center, Extents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridOBBCollider::HybridOBBCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridOBBCollider::~HybridOBBCollider()
+{
+}
+
+//bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
+// ericf change
+bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
+{
+	// We don't want primitive tests here!
+	mFlags |= OPC_NO_PRIMITIVE_TESTS;
+
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, box, worldb, worldm))	return true;
+
+	// Special case for 1-leaf trees
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
+		udword Nb = mIMesh->GetNbTriangles();
+
+		// Loop through all triangles
+		for(udword i=0;i<Nb;i++)
+		{
+			OBB_PRIM(i, OPC_CONTACT)
+		}
+		return true;
+	}
+
+	// Override destination array since we're only going to get leaf boxes here
+	mTouchedBoxes.Reset();
+	mTouchedPrimitives = &mTouchedBoxes;
+
+	// Now, do the actual query against leaf boxes
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+
+	// We only have a list of boxes so far
+	if(GetContactStatus())
+	{
+		// Reset contact status, since it currently only reflects collisions with leaf boxes
+		Collider::InitQuery();
+
+		// Change dest container so that we can use built-in overlap tests and get collided primitives
+		cache.TouchedPrimitives.Reset();
+		mTouchedPrimitives = &cache.TouchedPrimitives;
+
+		// Read touched leaf boxes
+		udword Nb = mTouchedBoxes.GetNbEntries();
+		const udword* Touched = mTouchedBoxes.GetEntries();
+
+		const LeafTriangles* LT = model.GetLeafTriangles();
+		const udword* Indices = model.GetIndices();
+
+		// Loop through touched leaves
+		while(Nb--)
+		{
+			const LeafTriangles& CurrentLeaf = LT[*Touched++];
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = *T++;
+					OBB_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = BaseIndex++;
+					OBB_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_OBBCollider.h

@@ -0,0 +1,146 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for an OBB collider.
+ *	\file		OPC_OBBCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 1st, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_OBBCOLLIDER_H__
+#define __OPC_OBBCOLLIDER_H__
+
+	struct OPCODE_API OBBCache : VolumeCache
+	{
+					OBBCache() : FatCoeff(1.1f)
+					{
+						FatBox.mCenter.Zero();
+						FatBox.mExtents.Zero();
+						FatBox.mRot.Identity();
+					}
+
+		// Cached faces signature
+		OBB				FatBox;		//!< Box used when performing the query resulting in cached faces
+		// User settings
+		float			FatCoeff;	//!< extents multiplier used to create a fat box
+	};
+
+	class OPCODE_API OBBCollider : public VolumeCollider
+	{
+		public:
+		// Constructor / Destructor
+											OBBCollider();
+		virtual								~OBBCollider();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic collision query for generic OPCODE models. After the call, access the results:
+		 *	- with GetContactStatus()
+		 *	- with GetNbTouchedPrimitives()
+		 *	- with GetTouchedPrimitives()
+		 *
+		 *	\param		cache			[in/out] a box cache
+		 *	\param		box				[in] collision OBB in local space
+		 *	\param		model			[in] Opcode model to collide with
+		 *	\param		worldb			[in] OBB's world matrix, or null
+		 *	\param		worldm			[in] model's world matrix, or null
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        //bool			Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(OBBCache& cache, const OBB& box, Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
+
+		// Settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: select between full box-box tests or "SAT-lite" tests (where Class III axes are discarded)
+		 *	\param		flag		[in] true for full tests, false for coarse tests
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetFullBoxBoxTest(bool flag)	{ mFullBoxBoxTest = flag;	}
+
+		// Settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
+		 *	\return		null if everything is ok, else a string describing the problem
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(Collider)	const char*		ValidateSettings();
+
+		protected:
+		// Precomputed data
+							Matrix3x3		mAR;				//!< Absolute rotation matrix
+							Matrix3x3		mRModelToBox;		//!< Rotation from model space to obb space
+							Matrix3x3		mRBoxToModel;		//!< Rotation from obb space to model space
+							Point			mTModelToBox;		//!< Translation from model space to obb space
+							Point			mTBoxToModel;		//!< Translation from obb space to model space
+
+							Point			mBoxExtents;
+							Point			mB0;				//!< - mTModelToBox + mBoxExtents
+							Point			mB1;				//!< - mTModelToBox - mBoxExtents
+
+							float			mBBx1;
+							float			mBBy1;
+							float			mBBz1;
+
+							float			mBB_1;
+							float			mBB_2;
+							float			mBB_3;
+							float			mBB_4;
+							float			mBB_5;
+							float			mBB_6;
+							float			mBB_7;
+							float			mBB_8;
+							float			mBB_9;
+
+		// Leaf description
+							Point			mLeafVerts[3];		//!< Triangle vertices
+		// Settings
+							bool			mFullBoxBoxTest;	//!< Perform full BV-BV tests (true) or SAT-lite tests (false)
+		// Internal methods
+							void			_Collide(const AABBCollisionNode* node);
+							void			_Collide(const AABBNoLeafNode* node);
+							void			_Collide(const AABBQuantizedNode* node);
+							void			_Collide(const AABBQuantizedNoLeafNode* node);
+							void			_CollideNoPrimitiveTest(const AABBCollisionNode* node);
+							void			_CollideNoPrimitiveTest(const AABBNoLeafNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
+			// Overlap tests
+		inline_				BOOL			OBBContainsBox(const Point& bc, const Point& be);
+		inline_				BOOL			BoxBoxOverlap(const Point& extents, const Point& center);
+		inline_				BOOL			TriBoxOverlap();
+			// Init methods
+							BOOL			InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
+	};
+
+	class OPCODE_API HybridOBBCollider : public OBBCollider
+	{
+		public:
+		// Constructor / Destructor
+											HybridOBBCollider();
+		virtual								~HybridOBBCollider();
+
+        //bool			Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(OBBCache& cache, const OBB& box, HybridModel& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
+		protected:
+							Container		mTouchedBoxes;
+	};
+
+#endif // __OPC_OBBCOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_OptimizedTree.cpp

@@ -0,0 +1,798 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for optimized trees. Implements 4 trees:
+ *	- normal
+ *	- no leaf
+ *	- quantized
+ *	- no leaf / quantized
+ *
+ *	\file		OPC_OptimizedTree.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A standard AABB tree.
+ *
+ *	\class		AABBCollisionTree
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A no-leaf AABB tree.
+ *
+ *	\class		AABBNoLeafTree
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A quantized AABB tree.
+ *
+ *	\class		AABBQuantizedTree
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A quantized no-leaf AABB tree.
+ *
+ *	\class		AABBQuantizedNoLeafTree
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+//! Compilation flag:
+//! - true to fix quantized boxes (i.e. make sure they enclose the original ones)
+//! - false to see the effects of quantization errors (faster, but wrong results in some cases)
+static bool gFixQuantized = true;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds an implicit tree from a standard one. An implicit tree is a complete tree (2*N-1 nodes) whose negative
+ *	box pointers and primitive pointers have been made implicit, hence packing 3 pointers in one.
+ *
+ *	Layout for implicit trees:
+ *	Node:
+ *			- box
+ *			- data (32-bits value)
+ *
+ *	if data's LSB = 1 =>	remaining bits are a primitive pointer
+ *	else					remaining bits are a P-node pointer, and N = P + 1
+ *
+ *	\relates	AABBCollisionNode
+ *	\fn			_BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
+ *	\param		linear			[in] base address of destination nodes
+ *	\param		box_id			[in] index of destination node
+ *	\param		current_id		[in] current running index
+ *	\param		current_node	[in] current node from input tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
+{
+	// Current node from input tree is "current_node". Must be flattened into "linear[boxid]".
+
+	// Store the AABB
+	current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
+	current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
+	// Store remaining info
+	if(current_node->IsLeaf())
+	{
+		// The input tree must be complete => i.e. one primitive/leaf
+		ASSERT(current_node->GetNbPrimitives()==1);
+		// Get the primitive index from the input tree
+		udword PrimitiveIndex = current_node->GetPrimitives()[0];
+		// Setup box data as the primitive index, marked as leaf
+		linear[box_id].mData = (PrimitiveIndex<<1)|1;
+	}
+	else
+	{
+		// To make the negative one implicit, we must store P and N in successive order
+		udword PosID = current_id++;	// Get a new id for positive child
+		udword NegID = current_id++;	// Get a new id for negative child
+		// Setup box data as the forthcoming new P pointer
+		linear[box_id].mData = (size_t)&linear[PosID];
+		// Make sure it's not marked as leaf
+		ASSERT(!(linear[box_id].mData&1));
+		// Recurse with new IDs
+		_BuildCollisionTree(linear, PosID, current_id, current_node->GetPos());
+		_BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds a "no-leaf" tree from a standard one. This is a tree whose leaf nodes have been removed.
+ *
+ *	Layout for no-leaf trees:
+ *
+ *	Node:
+ *			- box
+ *			- P pointer => a node (LSB=0) or a primitive (LSB=1)
+ *			- N pointer => a node (LSB=0) or a primitive (LSB=1)
+ *
+ *	\relates	AABBNoLeafNode
+ *	\fn			_BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
+ *	\param		linear			[in] base address of destination nodes
+ *	\param		box_id			[in] index of destination node
+ *	\param		current_id		[in] current running index
+ *	\param		current_node	[in] current node from input tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
+{
+	const AABBTreeNode* P = current_node->GetPos();
+	const AABBTreeNode* N = current_node->GetNeg();
+	// Leaf nodes here?!
+	ASSERT(P);
+	ASSERT(N);
+	// Internal node => keep the box
+	current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
+	current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
+
+	if(P->IsLeaf())
+	{
+		// The input tree must be complete => i.e. one primitive/leaf
+		ASSERT(P->GetNbPrimitives()==1);
+		// Get the primitive index from the input tree
+		udword PrimitiveIndex = P->GetPrimitives()[0];
+		// Setup prev box data as the primitive index, marked as leaf
+		linear[box_id].mPosData = (PrimitiveIndex<<1)|1;
+	}
+	else
+	{
+		// Get a new id for positive child
+		udword PosID = current_id++;
+		// Setup box data
+		linear[box_id].mPosData = (size_t)&linear[PosID];
+		// Make sure it's not marked as leaf
+		ASSERT(!(linear[box_id].mPosData&1));
+		// Recurse
+		_BuildNoLeafTree(linear, PosID, current_id, P);
+	}
+
+	if(N->IsLeaf())
+	{
+		// The input tree must be complete => i.e. one primitive/leaf
+		ASSERT(N->GetNbPrimitives()==1);
+		// Get the primitive index from the input tree
+		udword PrimitiveIndex = N->GetPrimitives()[0];
+		// Setup prev box data as the primitive index, marked as leaf
+		linear[box_id].mNegData = (PrimitiveIndex<<1)|1;
+	}
+	else
+	{
+		// Get a new id for negative child
+		udword NegID = current_id++;
+		// Setup box data
+		linear[box_id].mNegData = (size_t)&linear[NegID];
+		// Make sure it's not marked as leaf
+		ASSERT(!(linear[box_id].mNegData&1));
+		// Recurse
+		_BuildNoLeafTree(linear, NegID, current_id, N);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBCollisionTree::AABBCollisionTree() : mNodes(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBCollisionTree::~AABBCollisionTree()
+{
+	DELETEARRAY(mNodes);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds the collision tree from a generic AABB tree.
+ *	\param		tree			[in] generic AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBCollisionTree::Build(AABBTree* tree)
+{
+	// Checkings
+	if(!tree)	return false;
+	// Check the input tree is complete
+	udword NbTriangles	= tree->GetNbPrimitives();
+	udword NbNodes		= tree->GetNbNodes();
+	if(NbNodes!=NbTriangles*2-1)	return false;
+
+	// Get nodes
+	if(mNbNodes!=NbNodes)	// Same number of nodes => keep moving
+	{
+		mNbNodes = NbNodes;
+		DELETEARRAY(mNodes);
+		mNodes = new AABBCollisionNode[mNbNodes];
+		CHECKALLOC(mNodes);
+	}
+
+	// Build the tree
+	udword CurID = 1;
+	_BuildCollisionTree(mNodes, 0, CurID, tree);
+	ASSERT(CurID==mNbNodes);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the collision tree after vertices have been modified.
+ *	\param		mesh_interface	[in] mesh interface for current model
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface)
+// ericf change
+bool AABBCollisionTree::Refit(MeshInterface* mesh_interface)
+{
+	ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!");
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Walks the tree and call the user back for each node.
+ *	\param		callback	[in] walking callback
+ *	\param		user_data	[in] callback's user data
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const
+{
+	if(!callback)	return false;
+
+	struct Local
+	{
+		static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data)
+		{
+			if(!current_node || !(callback)(current_node, user_data))	return;
+
+			if(!current_node->IsLeaf())
+			{
+				_Walk(current_node->GetPos(), callback, user_data);
+				_Walk(current_node->GetNeg(), callback, user_data);
+			}
+		}
+	};
+	Local::_Walk(mNodes, callback, user_data);
+	return true;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBNoLeafTree::AABBNoLeafTree() : mNodes(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBNoLeafTree::~AABBNoLeafTree()
+{
+	DELETEARRAY(mNodes);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds the collision tree from a generic AABB tree.
+ *	\param		tree			[in] generic AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBNoLeafTree::Build(AABBTree* tree)
+{
+	// Checkings
+	if(!tree)	return false;
+	// Check the input tree is complete
+	udword NbTriangles	= tree->GetNbPrimitives();
+	udword NbNodes		= tree->GetNbNodes();
+	if(NbNodes!=NbTriangles*2-1)	return false;
+
+	// Get nodes
+	if(mNbNodes!=NbTriangles-1)	// Same number of nodes => keep moving
+	{
+		mNbNodes = NbTriangles-1;
+		DELETEARRAY(mNodes);
+		mNodes = new AABBNoLeafNode[mNbNodes];
+		CHECKALLOC(mNodes);
+	}
+
+	// Build the tree
+	udword CurID = 1;
+	_BuildNoLeafTree(mNodes, 0, CurID, tree);
+	ASSERT(CurID==mNbNodes);
+
+	return true;
+}
+
+inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
+{
+	// Compute triangle's AABB = a leaf box
+#ifdef OPC_USE_FCOMI	// a 15% speedup on my machine, not much
+	min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
+	max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
+
+	min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
+	max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
+
+	min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
+	max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
+#else
+	min = *vp.Vertex[0];
+	max = *vp.Vertex[0];
+	min.Min(*vp.Vertex[1]);
+	max.Max(*vp.Vertex[1]);
+	min.Min(*vp.Vertex[2]);
+	max.Max(*vp.Vertex[2]);
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the collision tree after vertices have been modified.
+ *	\param		mesh_interface	[in] mesh interface for current model
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface)
+// ericf change
+bool AABBNoLeafTree::Refit(MeshInterface* mesh_interface)
+{
+	// Checkings
+	if(!mesh_interface)	return false;
+
+	// Bottom-up update
+	VertexPointers VP;
+	Point Min,Max;
+	Point Min_,Max_;
+	udword Index = mNbNodes;
+	while(Index--)
+	{
+		AABBNoLeafNode& Current = mNodes[Index];
+
+		if(Current.HasPosLeaf())
+		{
+			mesh_interface->GetTriangle(VP, Current.GetPosPrimitive());
+			ComputeMinMax(Min, Max, VP);
+		}
+		else
+		{
+			const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
+			CurrentBox.GetMin(Min);
+			CurrentBox.GetMax(Max);
+		}
+
+		if(Current.HasNegLeaf())
+		{
+			mesh_interface->GetTriangle(VP, Current.GetNegPrimitive());
+			ComputeMinMax(Min_, Max_, VP);
+		}
+		else
+		{
+			const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
+			CurrentBox.GetMin(Min_);
+			CurrentBox.GetMax(Max_);
+		}
+#ifdef OPC_USE_FCOMI
+		Min.x = FCMin2(Min.x, Min_.x);
+		Max.x = FCMax2(Max.x, Max_.x);
+		Min.y = FCMin2(Min.y, Min_.y);
+		Max.y = FCMax2(Max.y, Max_.y);
+		Min.z = FCMin2(Min.z, Min_.z);
+		Max.z = FCMax2(Max.z, Max_.z);
+#else
+		Min.Min(Min_);
+		Max.Max(Max_);
+#endif
+		Current.mAABB.SetMinMax(Min, Max);
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Walks the tree and call the user back for each node.
+ *	\param		callback	[in] walking callback
+ *	\param		user_data	[in] callback's user data
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
+{
+	if(!callback)	return false;
+
+	struct Local
+	{
+		static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
+		{
+			if(!current_node || !(callback)(current_node, user_data))	return;
+
+			if(!current_node->HasPosLeaf())	_Walk(current_node->GetPos(), callback, user_data);
+			if(!current_node->HasNegLeaf())	_Walk(current_node->GetNeg(), callback, user_data);
+		}
+	};
+	Local::_Walk(mNodes, callback, user_data);
+	return true;
+}
+
+// Quantization notes:
+// - We could use the highest bits of mData to store some more quantized bits. Dequantization code
+//   would be slightly more complex, but number of overlap tests would be reduced (and anyhow those
+//   bits are currently wasted). Of course it's not possible if we move to 16 bits mData.
+// - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion.
+// - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some
+//   lazy-dequantization which may save some work in case of early exits). At the very least some
+//   muls could be saved by precomputing several more matrices. But maybe not worth the pain.
+// - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has
+//   been scaled, for example.
+// - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed
+//   number of quantization bits. Even better, could probably be best delta-encoded.
+
+
+// Find max values. Some people asked why I wasn't simply using the first node. Well, I can't.
+// I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal
+// centers/extents in order to quantize them. The first node would only give a single center and
+// a single extents. While extents would be the biggest, the center wouldn't.
+#define FIND_MAX_VALUES																			\
+	/* Get max values */																		\
+	Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);												\
+	Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);												\
+	for(udword i=0;i<mNbNodes;i++)																\
+	{																							\
+		if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.x)	CMax.x = fabsf(Nodes[i].mAABB.mCenter.x);	\
+		if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y)	CMax.y = fabsf(Nodes[i].mAABB.mCenter.y);	\
+		if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z)	CMax.z = fabsf(Nodes[i].mAABB.mCenter.z);	\
+		if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x)	EMax.x = fabsf(Nodes[i].mAABB.mExtents.x);	\
+		if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y)	EMax.y = fabsf(Nodes[i].mAABB.mExtents.y);	\
+		if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z)	EMax.z = fabsf(Nodes[i].mAABB.mExtents.z);	\
+	}
+
+#define INIT_QUANTIZATION													\
+	udword nbc=15;	/* Keep one bit for sign */								\
+	udword nbe=15;	/* Keep one bit for fix */								\
+	if(!gFixQuantized) nbe++;												\
+																			\
+	/* Compute quantization coeffs */										\
+	Point CQuantCoeff, EQuantCoeff;											\
+	CQuantCoeff.x = CMax.x!=0.0f ? float((1<<nbc)-1)/CMax.x : 0.0f;			\
+	CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f;			\
+	CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f;			\
+	EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f;			\
+	EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f;			\
+	EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f;			\
+	/* Compute and save dequantization coeffs */							\
+	mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f;		\
+	mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f;		\
+	mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f;		\
+	mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f;	\
+	mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f;	\
+	mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f;	\
+
+#define PERFORM_QUANTIZATION														\
+	/* Quantize */																	\
+	mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x);	\
+	mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y);	\
+	mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z);	\
+	mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x);	\
+	mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y);	\
+	mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z);	\
+	/* Fix quantized boxes */														\
+	if(gFixQuantized)																\
+	{																				\
+		/* Make sure the quantized box is still valid */							\
+		Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents;				\
+		Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents;				\
+		/* For each axis */															\
+		for(udword j=0;j<3;j++)														\
+		{	/* Dequantize the box center */											\
+			float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j];			\
+			bool FixMe=true;														\
+			do																		\
+			{	/* Dequantize the box extent */										\
+				float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j];	\
+				/* Compare real & dequantized values */								\
+				if(qc+qe<Max[j] || qc-qe>Min[j])	mNodes[i].mAABB.mExtents[j]++;	\
+				else								FixMe=false;					\
+				/* Prevent wrapping */												\
+				if(!mNodes[i].mAABB.mExtents[j])									\
+				{																	\
+					mNodes[i].mAABB.mExtents[j]=0xffff;								\
+					FixMe=false;													\
+				}																	\
+			}while(FixMe);															\
+		}																			\
+	}
+
+#define REMAP_DATA(member)											\
+	/* Fix data */													\
+	Data = Nodes[i].member;											\
+	if(!(Data&1))													\
+	{																\
+		/* Compute box number */									\
+		udword Nb = (Data - size_t(Nodes))/Nodes[i].GetNodeSize();	\
+		Data = (size_t) &mNodes[Nb]		;									\
+	}																\
+	/* ...remapped */												\
+	mNodes[i].member = Data;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBQuantizedTree::AABBQuantizedTree() : mNodes(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBQuantizedTree::~AABBQuantizedTree()
+{
+	DELETEARRAY(mNodes);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds the collision tree from a generic AABB tree.
+ *	\param		tree			[in] generic AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBQuantizedTree::Build(AABBTree* tree)
+{
+	// Checkings
+	if(!tree)	return false;
+	// Check the input tree is complete
+	udword NbTriangles	= tree->GetNbPrimitives();
+	udword NbNodes		= tree->GetNbNodes();
+	if(NbNodes!=NbTriangles*2-1)	return false;
+
+	// Get nodes
+	mNbNodes = NbNodes;
+	DELETEARRAY(mNodes);
+	AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes];
+	CHECKALLOC(Nodes);
+
+	// Build the tree
+	udword CurID = 1;
+	_BuildCollisionTree(Nodes, 0, CurID, tree);
+
+	// Quantize
+	{
+		mNodes = new AABBQuantizedNode[mNbNodes];
+		CHECKALLOC(mNodes);
+
+		// Get max values
+		FIND_MAX_VALUES
+
+		// Quantization
+		INIT_QUANTIZATION
+
+		// Quantize
+		size_t Data;
+		for(udword i=0;i<mNbNodes;i++)
+		{
+			PERFORM_QUANTIZATION
+			REMAP_DATA(mData)
+		}
+
+		DELETEARRAY(Nodes);
+	}
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the collision tree after vertices have been modified.
+ *	\param		mesh_interface	[in] mesh interface for current model
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBQuantizedTree::Refit(const MeshInterface* mesh_interface)
+// ericf change
+bool AABBQuantizedTree::Refit(MeshInterface* mesh_interface)
+{
+	ASSERT(!"Not implemented since requantizing is painful !");
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Walks the tree and call the user back for each node.
+ *	\param		callback	[in] walking callback
+ *	\param		user_data	[in] callback's user data
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const
+{
+	if(!callback)	return false;
+
+	struct Local
+	{
+		static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data)
+		{
+			if(!current_node || !(callback)(current_node, user_data))	return;
+
+			if(!current_node->IsLeaf())
+			{
+				_Walk(current_node->GetPos(), callback, user_data);
+				_Walk(current_node->GetNeg(), callback, user_data);
+			}
+		}
+	};
+	Local::_Walk(mNodes, callback, user_data);
+	return true;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree()
+{
+	DELETEARRAY(mNodes);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Builds the collision tree from a generic AABB tree.
+ *	\param		tree			[in] generic AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBQuantizedNoLeafTree::Build(AABBTree* tree)
+{
+	// Checkings
+	if(!tree)	return false;
+	// Check the input tree is complete
+	udword NbTriangles	= tree->GetNbPrimitives();
+	udword NbNodes		= tree->GetNbNodes();
+	if(NbNodes!=NbTriangles*2-1)	return false;
+
+	// Get nodes
+	mNbNodes = NbTriangles-1;
+	DELETEARRAY(mNodes);
+	AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes];
+	CHECKALLOC(Nodes);
+
+	// Build the tree
+	udword CurID = 1;
+	_BuildNoLeafTree(Nodes, 0, CurID, tree);
+	ASSERT(CurID==mNbNodes);
+
+	// Quantize
+	{
+		mNodes = new AABBQuantizedNoLeafNode[mNbNodes];
+		CHECKALLOC(mNodes);
+
+		// Get max values
+		FIND_MAX_VALUES
+
+		// Quantization
+		INIT_QUANTIZATION
+
+		// Quantize
+		size_t Data;
+		for(udword i=0;i<mNbNodes;i++)
+		{
+			PERFORM_QUANTIZATION
+			REMAP_DATA(mPosData)
+			REMAP_DATA(mNegData)
+		}
+
+		DELETEARRAY(Nodes);
+	}
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Refits the collision tree after vertices have been modified.
+ *	\param		mesh_interface	[in] mesh interface for current model
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* mesh_interface)
+// ericf change
+bool AABBQuantizedNoLeafTree::Refit(MeshInterface* mesh_interface)
+{
+	ASSERT(!"Not implemented since requantizing is painful !");
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Walks the tree and call the user back for each node.
+ *	\param		callback	[in] walking callback
+ *	\param		user_data	[in] callback's user data
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
+{
+	if(!callback)	return false;
+
+	struct Local
+	{
+		static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
+		{
+			if(!current_node || !(callback)(current_node, user_data))	return;
+
+			if(!current_node->HasPosLeaf())	_Walk(current_node->GetPos(), callback, user_data);
+			if(!current_node->HasNegLeaf())	_Walk(current_node->GetNeg(), callback, user_data);
+		}
+	};
+	Local::_Walk(mNodes, callback, user_data);
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_OptimizedTree.h

@@ -0,0 +1,209 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for optimized trees.
+ *	\file		OPC_OptimizedTree.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_OPTIMIZEDTREE_H__
+#define __OPC_OPTIMIZEDTREE_H__
+
+	//! Common interface for a node of an implicit tree
+	#define IMPLEMENT_IMPLICIT_NODE(base_class, volume)														\
+		public:																								\
+		/* Constructor / Destructor */																		\
+		inline_								base_class() : mData(0)	{}										\
+		inline_								~base_class()			{}										\
+		/* Leaf test */																						\
+		inline_			BOOL				IsLeaf()		const	{ return mData&1;					}	\
+		/* Data access */																					\
+		inline_			const base_class*	GetPos()		const	{ return (base_class*)mData;		}	\
+		inline_			const base_class*	GetNeg()		const	{ return ((base_class*)mData)+1;	}	\
+		inline_			udword				GetPrimitive()	const	{ return (mData>>1);				}	\
+		/* Stats */																							\
+		inline_			udword				GetNodeSize()	const	{ return SIZEOFOBJECT;				}	\
+																											\
+						volume				mAABB;															\
+						size_t				mData;
+
+	//! Common interface for a node of a no-leaf tree
+	#define IMPLEMENT_NOLEAF_NODE(base_class, volume)														\
+		public:																								\
+		/* Constructor / Destructor */																		\
+		inline_								base_class() : mPosData(0), mNegData(0)	{}						\
+		inline_								~base_class()							{}						\
+		/* Leaf tests */																					\
+		inline_			BOOL				HasPosLeaf()		const	{ return mPosData&1;			}	\
+		inline_			BOOL				HasNegLeaf()		const	{ return mNegData&1;			}	\
+		/* Data access */																					\
+		inline_			const base_class*	GetPos()			const	{ return (base_class*)mPosData;	}	\
+		inline_			const base_class*	GetNeg()			const	{ return (base_class*)mNegData;	}	\
+		inline_			udword				GetPosPrimitive()	const	{ return (mPosData>>1);			}	\
+		inline_			udword				GetNegPrimitive()	const	{ return (mNegData>>1);			}	\
+		/* Stats */																							\
+		inline_			udword				GetNodeSize()		const	{ return SIZEOFOBJECT;			}	\
+																											\
+						volume				mAABB;															\
+						size_t				mPosData;														\
+						size_t				mNegData;
+
+	class OPCODE_API AABBCollisionNode
+	{
+		IMPLEMENT_IMPLICIT_NODE(AABBCollisionNode, CollisionAABB)
+
+		inline_			float				GetVolume()		const	{ return mAABB.mExtents.x * mAABB.mExtents.y * mAABB.mExtents.z;	}
+		inline_			float				GetSize()		const	{ return mAABB.mExtents.SquareMagnitude();	}
+		inline_			udword				GetRadius()		const
+											{
+												udword* Bits = (udword*)&mAABB.mExtents.x;
+												udword Max = Bits[0];
+												if(Bits[1]>Max)	Max = Bits[1];
+												if(Bits[2]>Max)	Max = Bits[2];
+												return Max;
+											}
+
+		// NB: using the square-magnitude or the true volume of the box, seems to yield better results
+		// (assuming UNC-like informed traversal methods). I borrowed this idea from PQP. The usual "size"
+		// otherwise, is the largest box extent. In SOLID that extent is computed on-the-fly each time it's
+		// needed (the best approach IMHO). In RAPID the rotation matrix is permuted so that Extent[0] is
+		// always the greatest, which saves looking for it at runtime. On the other hand, it yields matrices
+		// whose determinant is not 1, i.e. you can't encode them anymore as unit quaternions. Not a very
+		// good strategy.
+	};
+
+	class OPCODE_API AABBQuantizedNode
+	{
+		IMPLEMENT_IMPLICIT_NODE(AABBQuantizedNode, QuantizedAABB)
+
+		inline_			uword				GetSize()		const
+											{
+												const uword* Bits = mAABB.mExtents;
+												uword Max = Bits[0];
+												if(Bits[1]>Max)	Max = Bits[1];
+												if(Bits[2]>Max)	Max = Bits[2];
+												return Max;
+											}
+		// NB: for quantized nodes I don't feel like computing a square-magnitude with integers all
+		// over the place.......!
+	};
+
+	class OPCODE_API AABBNoLeafNode
+	{
+		IMPLEMENT_NOLEAF_NODE(AABBNoLeafNode, CollisionAABB)
+	};
+
+	class OPCODE_API AABBQuantizedNoLeafNode
+	{
+		IMPLEMENT_NOLEAF_NODE(AABBQuantizedNoLeafNode, QuantizedAABB)
+	};
+
+	//! Common interface for a collision tree
+	#define IMPLEMENT_COLLISION_TREE(base_class, node)																\
+		public:																										\
+		/* Constructor / Destructor */																				\
+													base_class();													\
+		virtual										~base_class();													\
+		/* Builds from a standard tree */																			\
+		override(AABBOptimizedTree)	bool			Build(AABBTree* tree);											\
+		/* Refits the tree */																						\
+		/*ericf change override(AABBOptimizedTree)	bool			Refit(const MeshInterface* mesh_interface);*/ \
+		override(AABBOptimizedTree)	bool			Refit(MeshInterface* mesh_interface);						\
+		/* Walks the tree */																						\
+		override(AABBOptimizedTree)	bool			Walk(GenericWalkingCallback callback, void* user_data) const;	\
+		/* Data access */																							\
+		inline_						const node*		GetNodes()		const	{ return mNodes;					}	\
+		/* Stats */																									\
+		override(AABBOptimizedTree)	udword			GetUsedBytes()	const	{ return mNbNodes*sizeof(node);		}	\
+		private:																									\
+									node*			mNodes;
+
+	typedef		bool				(*GenericWalkingCallback)	(const void* current, void* user_data);
+
+	class OPCODE_API AABBOptimizedTree
+	{
+		public:
+		// Constructor / Destructor
+											AABBOptimizedTree() :
+												mNbNodes	(0)
+																							{}
+		virtual								~AABBOptimizedTree()							{}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Builds the collision tree from a generic AABB tree.
+		 *	\param		tree			[in] generic AABB tree
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual			bool				Build(AABBTree* tree)											= 0;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Refits the collision tree after vertices have been modified.
+		 *	\param		mesh_interface	[in] mesh interface for current model
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//virtual			bool				Refit(const MeshInterface* mesh_interface)						= 0;
+        // ericf change
+		virtual			bool				Refit(MeshInterface* mesh_interface)						= 0;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Walks the tree and call the user back for each node.
+		 *	\param		callback	[in] walking callback
+		 *	\param		user_data	[in] callback's user data
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual			bool				Walk(GenericWalkingCallback callback, void* user_data) const	= 0;
+
+		// Data access
+		virtual			udword				GetUsedBytes()		const										= 0;
+		inline_			udword				GetNbNodes()		const						{ return mNbNodes;	}
+
+		protected:
+						udword				mNbNodes;
+	};
+
+	class OPCODE_API AABBCollisionTree : public AABBOptimizedTree
+	{
+		IMPLEMENT_COLLISION_TREE(AABBCollisionTree, AABBCollisionNode)
+	};
+
+	class OPCODE_API AABBNoLeafTree : public AABBOptimizedTree
+	{
+		IMPLEMENT_COLLISION_TREE(AABBNoLeafTree, AABBNoLeafNode)
+	};
+
+	class OPCODE_API AABBQuantizedTree : public AABBOptimizedTree
+	{
+		IMPLEMENT_COLLISION_TREE(AABBQuantizedTree, AABBQuantizedNode)
+
+		public:
+						Point				mCenterCoeff;
+						Point				mExtentsCoeff;
+	};
+
+	class OPCODE_API AABBQuantizedNoLeafTree : public AABBOptimizedTree
+	{
+		IMPLEMENT_COLLISION_TREE(AABBQuantizedNoLeafTree, AABBQuantizedNoLeafNode)
+
+		public:
+						Point				mCenterCoeff;
+						Point				mExtentsCoeff;
+	};
+
+#endif // __OPC_OPTIMIZEDTREE_H__

src/external/open_dynamics_engine-ef/ode/OPC_Picking.cpp

@@ -0,0 +1,190 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code to perform "picking".
+ *	\file		OPC_Picking.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#ifdef OPC_RAYHIT_CALLBACK
+
+/*
+	Possible RayCollider usages:
+	- boolean query (shadow feeler)
+	- closest hit
+	- all hits
+	- number of intersection (boolean)
+
+*/
+
+bool Opcode::SetupAllHits(RayCollider& collider, CollisionFaces& contacts)
+{
+	struct Local
+	{
+		static void AllContacts(const CollisionFace& hit, void* user_data)
+		{
+			CollisionFaces* CF = (CollisionFaces*)user_data;
+			CF->AddFace(hit);
+		}
+	};
+
+	collider.SetFirstContact(false);
+	collider.SetHitCallback(Local::AllContacts);
+	collider.SetUserData(&contacts);
+	return true;
+}
+
+bool Opcode::SetupClosestHit(RayCollider& collider, CollisionFace& closest_contact)
+{
+	struct Local
+	{
+		static void ClosestContact(const CollisionFace& hit, void* user_data)
+		{
+			CollisionFace* CF = (CollisionFace*)user_data;
+			if(hit.mDistance<CF->mDistance)	*CF = hit;
+		}
+	};
+
+	collider.SetFirstContact(false);
+	collider.SetHitCallback(Local::ClosestContact);
+	collider.SetUserData(&closest_contact);
+	closest_contact.mDistance = MAX_FLOAT;
+	return true;
+}
+
+bool Opcode::SetupShadowFeeler(RayCollider& collider)
+{
+	collider.SetFirstContact(true);
+	collider.SetHitCallback(null);
+	return true;
+}
+
+bool Opcode::SetupInOutTest(RayCollider& collider)
+{
+	collider.SetFirstContact(false);
+	collider.SetHitCallback(null);
+	// Results with collider.GetNbIntersections()
+	return true;
+}
+
+bool Opcode::Picking(
+CollisionFace& picked_face,
+const Ray& world_ray, const Model& model, const Matrix4x4* world,
+float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data)
+{
+	struct Local
+	{
+		struct CullData
+		{
+			CollisionFace*			Closest;
+			float					MinLimit;
+			CullModeCallback		Callback;
+			void*					UserData;
+			Point					ViewPoint;
+			const MeshInterface*	IMesh;
+		};
+
+		// Called for each stabbed face
+		static void RenderCullingCallback(const CollisionFace& hit, void* user_data)
+		{
+			CullData* Data = (CullData*)user_data;
+
+			// Discard face if we already have a closer hit
+			if(hit.mDistance>=Data->Closest->mDistance)	return;
+
+			// Discard face if hit point is smaller than min limit. This mainly happens when the face is in front
+			// of the near clip plane (or straddles it). If we keep the face nonetheless, the user can select an
+			// object that he may not even be able to see, which is very annoying.
+			if(hit.mDistance<=Data->MinLimit)	return;
+
+			// This is the index of currently stabbed triangle.
+			udword StabbedFaceIndex = hit.mFaceID;
+
+			// We may keep it or not, depending on backface culling
+			bool KeepIt = true;
+
+			// Catch *render* cull mode for this face
+			CullMode CM = (Data->Callback)(StabbedFaceIndex, Data->UserData);
+
+			if(CM!=CULLMODE_NONE)	// Don't even compute culling for double-sided triangles
+			{
+				// Compute backface culling for current face
+
+				VertexPointers VP;
+				Data->IMesh->GetTriangle(VP, StabbedFaceIndex);
+				if(VP.BackfaceCulling(Data->ViewPoint))
+				{
+					if(CM==CULLMODE_CW)		KeepIt = false;
+				}
+				else
+				{
+					if(CM==CULLMODE_CCW)	KeepIt = false;
+				}
+			}
+
+			if(KeepIt)	*Data->Closest = hit;
+		}
+	};
+
+	RayCollider RC;
+	RC.SetMaxDist(max_dist);
+	RC.SetTemporalCoherence(false);
+	RC.SetCulling(false);		// We need all faces since some of them can be double-sided
+	RC.SetFirstContact(false);
+	RC.SetHitCallback(Local::RenderCullingCallback);
+
+	picked_face.mFaceID		= INVALID_ID;
+	picked_face.mDistance	= MAX_FLOAT;
+	picked_face.mU			= 0.0f;
+	picked_face.mV			= 0.0f;
+
+	Local::CullData Data;
+	Data.Closest			= &picked_face;
+	Data.MinLimit			= min_dist;
+	Data.Callback			= callback;
+	Data.UserData			= user_data;
+	Data.ViewPoint			= view_point;
+	Data.IMesh				= model.GetMeshInterface();
+
+	if(world)
+	{
+		// Get matrices
+		Matrix4x4 InvWorld;
+		InvertPRMatrix(InvWorld, *world);
+
+		// Compute camera position in mesh space
+		Data.ViewPoint *= InvWorld;
+	}
+
+	RC.SetUserData(&Data);
+	if(RC.Collide(world_ray, model, world))
+	{
+		return picked_face.mFaceID!=INVALID_ID;
+	}
+	return false;
+}
+
+#endif

src/external/open_dynamics_engine-ef/ode/OPC_Picking.h

@@ -0,0 +1,45 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code to perform "picking".
+ *	\file		OPC_Picking.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_PICKING_H__
+#define __OPC_PICKING_H__
+
+#ifdef OPC_RAYHIT_CALLBACK
+
+	enum CullMode
+	{
+		CULLMODE_NONE	= 0,
+		CULLMODE_CW		= 1,
+		CULLMODE_CCW	= 2
+	};
+
+	typedef CullMode (*CullModeCallback)(udword triangle_index, void* user_data);
+
+	OPCODE_API	bool SetupAllHits		(RayCollider& collider, CollisionFaces& contacts);
+	OPCODE_API	bool SetupClosestHit	(RayCollider& collider, CollisionFace& closest_contact);
+	OPCODE_API	bool SetupShadowFeeler	(RayCollider& collider);
+	OPCODE_API	bool SetupInOutTest		(RayCollider& collider);
+
+	OPCODE_API	bool Picking(
+						CollisionFace& picked_face,
+						const Ray& world_ray, const Model& model, const Matrix4x4* world,
+						float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data);
+#endif
+
+#endif //__OPC_PICKING_H__

src/external/open_dynamics_engine-ef/ode/OPC_PlanesAABBOverlap.h

@@ -0,0 +1,50 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Planes-AABB overlap test.
+ *	- original code by Ville Miettinen, from Umbra/dPVS (released on the GD-Algorithms mailing list)
+ *	- almost used "as-is", I even left the comments (hence the frustum-related notes)
+ *
+ *	\param		center			[in] box center
+ *	\param		extents			[in] box extents
+ *	\param		out_clip_mask	[out] bitmask for active planes
+ *	\param		in_clip_mask	[in] bitmask for active planes
+ *	\return		TRUE if boxes overlap planes
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL PlanesCollider::PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask)
+{
+	// Stats
+	mNbVolumeBVTests++;
+
+	const Plane* p = mPlanes;
+
+	// Evaluate through all active frustum planes. We determine the relation 
+	// between the AABB and a plane by using the concept of "near" and "far"
+	// vertices originally described by Zhang (and later by Möller). Our
+	// variant here uses 3 fabs ops, 6 muls, 7 adds and two floating point
+	// comparisons per plane. The routine early-exits if the AABB is found
+	// to be outside any of the planes. The loop also constructs a new output
+	// clip mask. Most FPUs have a native single-cycle fabsf() operation.
+
+	udword Mask				= 1;			// current mask index (1,2,4,8,..)
+	udword TmpOutClipMask	= 0;			// initialize output clip mask into empty. 
+
+	while(Mask<=in_clip_mask)				// keep looping while we have active planes left...
+	{
+		if(in_clip_mask & Mask)				// if clip plane is active, process it..
+		{               
+			float NP = extents.x*fabsf(p->n.x) + extents.y*fabsf(p->n.y) + extents.z*fabsf(p->n.z);	// ### fabsf could be precomputed
+			float MP = center.x*p->n.x + center.y*p->n.y + center.z*p->n.z + p->d;
+
+			if(NP < MP)						// near vertex behind the clip plane... 
+				return FALSE;				// .. so there is no intersection..
+			if((-NP) < MP)					// near and far vertices on different sides of plane..
+				TmpOutClipMask |= Mask;		// .. so update the clip mask...
+		}
+		Mask+=Mask;							// mk = (1<<plane)
+		p++;								// advance to next plane
+	}
+
+	out_clip_mask = TmpOutClipMask;			// copy output value (temp used to resolve aliasing!)
+	return TRUE;							// indicate that AABB intersects frustum
+}

src/external/open_dynamics_engine-ef/ode/OPC_PlanesCollider.cpp

@@ -0,0 +1,665 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a planes collider.
+ *	\file		OPC_PlanesCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 1st, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a Planes-vs-tree collider.
+ *
+ *	\class		PlanesCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		January, 1st, 2002
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_PlanesAABBOverlap.h"
+#include "ode/OPC_PlanesTriOverlap.h"
+
+#define SET_CONTACT(prim_index, flag)		\
+	/* Set contact status */				\
+	mFlags |= flag;							\
+	mTouchedPrimitives->Add(prim_index);
+
+//! Planes-triangle test
+#define PLANES_PRIM(prim_index, flag)		\
+	/* Request vertices from the app */		\
+	mIMesh->GetTriangle(mVP, prim_index);	\
+	/* Perform triangle-box overlap test */	\
+	if(PlanesTriOverlap(clip_mask))			\
+	{										\
+		SET_CONTACT(prim_index, flag)		\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+PlanesCollider::PlanesCollider() :
+	mNbPlanes	(0),
+	mPlanes		(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+PlanesCollider::~PlanesCollider()
+{
+	DELETEARRAY(mPlanes);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Validates current settings. You should call this method after all the settings and callbacks have been defined.
+ *	\return		null if everything is ok, else a string describing the problem
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const char* PlanesCollider::ValidateSettings()
+{
+	if(TemporalCoherenceEnabled() && !FirstContactEnabled())	return "Temporal coherence only works with ""First contact"" mode!";
+
+	return VolumeCollider::ValidateSettings();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic collision query for generic OPCODE models. After the call, access the results:
+ *	- with GetContactStatus()
+ *	- with GetNbTouchedPrimitives()
+ *	- with GetTouchedPrimitives()
+ *
+ *	\param		cache		[in/out] a planes cache
+ *	\param		planes		[in] list of planes in world space
+ *	\param		nb_planes	[in] number of planes
+ *	\param		model		[in] Opcode model to collide with
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm)
+// ericf change
+bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, Model& model, const Matrix4x4* worldm)
+{
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, planes, nb_planes, worldm))	return true;
+
+	udword PlaneMask = (1<<nb_planes)-1;
+
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+			else						_Collide(Tree->GetNodes(), PlaneMask);
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+			else						_Collide(Tree->GetNodes(), PlaneMask);
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+			else						_Collide(Tree->GetNodes(), PlaneMask);
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+			else						_Collide(Tree->GetNodes(), PlaneMask);
+		}
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a collision query :
+ *	- reset stats & contact status
+ *	- compute planes in model space
+ *	- check temporal coherence
+ *
+ *	\param		cache		[in/out] a planes cache
+ *	\param		planes		[in] list of planes
+ *	\param		nb_planes	[in] number of planes
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		TRUE if we can return immediately
+ *	\warning	SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL PlanesCollider::InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm)
+{
+	// 1) Call the base method
+	VolumeCollider::InitQuery();
+
+	// 2) Compute planes in model space
+	if(nb_planes>mNbPlanes)
+	{
+		DELETEARRAY(mPlanes);
+		mPlanes = new Plane[nb_planes];
+	}
+	mNbPlanes = nb_planes;
+
+	if(worldm)
+	{
+		Matrix4x4 InvWorldM;
+		InvertPRMatrix(InvWorldM, *worldm);
+
+//		for(udword i=0;i<nb_planes;i++)	mPlanes[i] = planes[i] * InvWorldM;
+		for(udword i=0;i<nb_planes;i++)	TransformPlane(mPlanes[i], planes[i], InvWorldM);
+	}
+	else CopyMemory(mPlanes, planes, nb_planes*sizeof(Plane));
+
+	// 3) Setup destination pointer
+	mTouchedPrimitives = &cache.TouchedPrimitives;
+
+	// 4) Special case: 1-triangle meshes [Opcode 1.3]
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		if(!SkipPrimitiveTests())
+		{
+			// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
+			mTouchedPrimitives->Reset();
+
+			// Perform overlap test between the unique triangle and the planes (and set contact status if needed)
+			udword clip_mask = (1<<mNbPlanes)-1;
+			PLANES_PRIM(udword(0), OPC_CONTACT)
+
+			// Return immediately regardless of status
+			return TRUE;
+		}
+	}
+
+	// 4) Check temporal coherence:
+	if(TemporalCoherenceEnabled())
+	{
+		// Here we use temporal coherence
+		// => check results from previous frame before performing the collision query
+		if(FirstContactEnabled())
+		{
+			// We're only interested in the first contact found => test the unique previously touched face
+			if(mTouchedPrimitives->GetNbEntries())
+			{
+				// Get index of previously touched face = the first entry in the array
+				udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
+
+				// Then reset the array:
+				// - if the overlap test below is successful, the index we'll get added back anyway
+				// - if it isn't, then the array should be reset anyway for the normal query
+				mTouchedPrimitives->Reset();
+
+				// Perform overlap test between the cached triangle and the planes (and set contact status if needed)
+				udword clip_mask = (1<<mNbPlanes)-1;
+				PLANES_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
+
+				// Return immediately if possible
+				if(GetContactStatus())	return TRUE;
+			}
+			// else no face has been touched during previous query
+			// => we'll have to perform a normal query
+		}
+		else mTouchedPrimitives->Reset();
+	}
+	else
+	{
+		// Here we don't use temporal coherence => do a normal query
+		mTouchedPrimitives->Reset();
+	}
+
+	return FALSE;
+}
+
+#define TEST_CLIP_MASK																					\
+	/* If the box is completely included, so are its children. We don't need to do extra tests, we */	\
+	/* can immediately output a list of visible children. Those ones won't need to be clipped. */		\
+	if(!OutClipMask)																					\
+	{																									\
+		/* Set contact status */																		\
+		mFlags |= OPC_CONTACT;																			\
+		_Dump(node);																					\
+		return;																							\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_Collide(const AABBCollisionNode* node, udword clip_mask)
+{
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->IsLeaf())
+	{
+		PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos(), OutClipMask);
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg(), OutClipMask);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask)
+{
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_Collide(const AABBQuantizedNode* node, udword clip_mask)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->IsLeaf())
+	{
+		PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos(), OutClipMask);
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg(), OutClipMask);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_Collide(const AABBNoLeafNode* node, udword clip_mask)
+{
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->HasPosLeaf())	{ PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos(), OutClipMask);
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg(), OutClipMask);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask)
+{
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->HasPosLeaf())	{ PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos(), OutClipMask);
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg(), OutClipMask);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
+	udword OutClipMask;
+	if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask))	return;
+
+	TEST_CLIP_MASK
+
+	// Else the box straddles one or several planes, so we need to recurse down the tree.
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
+}
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridPlanesCollider::HybridPlanesCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridPlanesCollider::~HybridPlanesCollider()
+{
+}
+
+//bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm)
+// ericf change
+bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, HybridModel& model, const Matrix4x4* worldm)
+{
+	// We don't want primitive tests here!
+	mFlags |= OPC_NO_PRIMITIVE_TESTS;
+
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, planes, nb_planes, worldm))	return true;
+
+	// Special case for 1-leaf trees
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
+		udword Nb = mIMesh->GetNbTriangles();
+
+		// Loop through all triangles
+		udword clip_mask = (1<<mNbPlanes)-1;
+		for(udword i=0;i<Nb;i++)
+		{
+			PLANES_PRIM(i, OPC_CONTACT)
+		}
+		return true;
+	}
+
+	// Override destination array since we're only going to get leaf boxes here
+	mTouchedBoxes.Reset();
+	mTouchedPrimitives = &mTouchedBoxes;
+
+	udword PlaneMask = (1<<nb_planes)-1;
+
+	// Now, do the actual query against leaf boxes
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
+		}
+	}
+
+	// We only have a list of boxes so far
+	if(GetContactStatus())
+	{
+		// Reset contact status, since it currently only reflects collisions with leaf boxes
+		Collider::InitQuery();
+
+		// Change dest container so that we can use built-in overlap tests and get collided primitives
+		cache.TouchedPrimitives.Reset();
+		mTouchedPrimitives = &cache.TouchedPrimitives;
+
+		// Read touched leaf boxes
+		udword Nb = mTouchedBoxes.GetNbEntries();
+		const udword* Touched = mTouchedBoxes.GetEntries();
+
+		const LeafTriangles* LT = model.GetLeafTriangles();
+		const udword* Indices = model.GetIndices();
+
+		// Loop through touched leaves
+		udword clip_mask = (1<<mNbPlanes)-1;
+		while(Nb--)
+		{
+			const LeafTriangles& CurrentLeaf = LT[*Touched++];
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = *T++;
+					PLANES_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = BaseIndex++;
+					PLANES_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_PlanesCollider.h

@@ -0,0 +1,127 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a planes collider.
+ *	\file		OPC_PlanesCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 1st, 2002
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_PLANESCOLLIDER_H__
+#define __OPC_PLANESCOLLIDER_H__
+
+	struct OPCODE_API PlanesCache : VolumeCache
+	{
+					PlanesCache()
+					{
+					}
+	};
+
+	class OPCODE_API PlanesCollider : public VolumeCollider
+	{
+		public:
+		// Constructor / Destructor
+											PlanesCollider();
+		virtual								~PlanesCollider();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic collision query for generic OPCODE models. After the call, access the results:
+		 *	- with GetContactStatus()
+		 *	- with GetNbTouchedPrimitives()
+		 *	- with GetTouchedPrimitives()
+		 *
+		 *	\param		cache			[in/out] a planes cache
+		 *	\param		planes			[in] list of planes in world space
+		 *	\param		nb_planes		[in] number of planes
+		 *	\param		model			[in] Opcode model to collide with
+		 *	\param		worldm			[in] model's world matrix, or null
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        //bool			Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, Model& model, const Matrix4x4* worldm=null);
+
+		// Mutant box-with-planes collision queries
+		//inline_				bool			Collide(PlanesCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null)
+                            // ericf change
+		inline_				bool			Collide(PlanesCache& cache, const OBB& box, Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null)
+											{
+												Plane PL[6];
+
+												if(worldb)
+												{
+													// Create a new OBB in world space
+													OBB WorldBox;
+													box.Rotate(*worldb, WorldBox);
+													// Compute planes from the sides of the box
+													WorldBox.ComputePlanes(PL);
+												}
+												else
+												{
+													// Compute planes from the sides of the box
+													box.ComputePlanes(PL);
+												}
+
+												// Collide with box planes
+												return Collide(cache, PL, 6, model, worldm);
+											}
+		// Settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
+		 *	\return		null if everything is ok, else a string describing the problem
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(Collider)	const char*		ValidateSettings();
+
+		protected:
+		// Planes in model space
+							udword			mNbPlanes;
+							Plane*			mPlanes;
+		// Leaf description
+							VertexPointers	mVP;
+		// Internal methods
+							void			_Collide(const AABBCollisionNode* node, udword clip_mask);
+							void			_Collide(const AABBNoLeafNode* node, udword clip_mask);
+							void			_Collide(const AABBQuantizedNode* node, udword clip_mask);
+							void			_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask);
+							void			_CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask);
+							void			_CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask);
+			// Overlap tests
+		inline_				BOOL			PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask);
+		inline_				BOOL			PlanesTriOverlap(udword in_clip_mask);
+			// Init methods
+							BOOL			InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm=null);
+	};
+
+	class OPCODE_API HybridPlanesCollider : public PlanesCollider
+	{
+		public:
+		// Constructor / Destructor
+											HybridPlanesCollider();
+		virtual								~HybridPlanesCollider();
+
+        //bool			Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, HybridModel& model, const Matrix4x4* worldm=null);
+		protected:
+							Container		mTouchedBoxes;
+	};
+
+#endif // __OPC_PLANESCOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_PlanesTriOverlap.h

@@ -0,0 +1,40 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Planes-triangle overlap test.
+ *	\param		in_clip_mask	[in] bitmask for active planes
+ *	\return		TRUE if triangle overlap planes
+ *	\warning	THIS IS A CONSERVATIVE TEST !! Some triangles will be returned as intersecting, while they're not!
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL PlanesCollider::PlanesTriOverlap(udword in_clip_mask)
+{
+	// Stats
+	mNbVolumePrimTests++;
+
+	const Plane* p = mPlanes;
+	udword Mask = 1;
+
+	while(Mask<=in_clip_mask)
+	{
+		if(in_clip_mask & Mask)
+		{
+			float d0 = p->Distance(*mVP.Vertex[0]);
+			float d1 = p->Distance(*mVP.Vertex[1]);
+			float d2 = p->Distance(*mVP.Vertex[2]);
+			if(d0>0.0f && d1>0.0f && d2>0.0f)	return FALSE;
+//			if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK))	return FALSE;
+		}
+		Mask+=Mask;
+		p++;
+	}
+/*
+	for(udword i=0;i<6;i++)
+	{
+		float d0 = p[i].Distance(mLeafVerts[0]);
+		float d1 = p[i].Distance(mLeafVerts[1]);
+		float d2 = p[i].Distance(mLeafVerts[2]);
+		if(d0>0.0f && d1>0.0f && d2>0.0f)	return false;
+	}
+*/
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_RayAABBOverlap.h

@@ -0,0 +1,63 @@
+// Opcode 1.1: ray-AABB overlap tests based on Woo's code
+// Opcode 1.2: ray-AABB overlap tests based on the separating axis theorem
+//
+// The point of intersection is not computed anymore. The distance to impact is not needed anymore
+// since we now have two different queries for segments or rays.
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes a segment-AABB overlap test using the separating axis theorem. Segment is cached within the class.
+ *	\param		center	[in] AABB center
+ *	\param		extents	[in] AABB extents
+ *	\return		true on overlap
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& extents)
+{
+	// Stats
+	mNbRayBVTests++;
+
+	float Dx = mData2.x - center.x;		if(fabsf(Dx) > extents.x + mFDir.x)	return FALSE;
+	float Dy = mData2.y - center.y;		if(fabsf(Dy) > extents.y + mFDir.y)	return FALSE;
+	float Dz = mData2.z - center.z;		if(fabsf(Dz) > extents.z + mFDir.z)	return FALSE;
+
+	float f;
+	f = mData.y * Dz - mData.z * Dy;	if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y)	return FALSE;
+	f = mData.z * Dx - mData.x * Dz;	if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x)	return FALSE;
+	f = mData.x * Dy - mData.y * Dx;	if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x)	return FALSE;
+
+	return TRUE;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes a ray-AABB overlap test using the separating axis theorem. Ray is cached within the class.
+ *	\param		center	[in] AABB center
+ *	\param		extents	[in] AABB extents
+ *	\return		true on overlap
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& extents)
+{
+	// Stats
+	mNbRayBVTests++;
+
+//	float Dx = mOrigin.x - center.x;	if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f)	return FALSE;
+//	float Dy = mOrigin.y - center.y;	if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f)	return FALSE;
+//	float Dz = mOrigin.z - center.z;	if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f)	return FALSE;
+
+	float Dx = mOrigin.x - center.x;	if(GREATER(Dx, extents.x) && Dx*mDir.x>=0.0f)	return FALSE;
+	float Dy = mOrigin.y - center.y;	if(GREATER(Dy, extents.y) && Dy*mDir.y>=0.0f)	return FALSE;
+	float Dz = mOrigin.z - center.z;	if(GREATER(Dz, extents.z) && Dz*mDir.z>=0.0f)	return FALSE;
+
+//	float Dx = mOrigin.x - center.x;	if(GREATER(Dx, extents.x) && ((SIR(Dx)-1)^SIR(mDir.x))>=0.0f)	return FALSE;
+//	float Dy = mOrigin.y - center.y;	if(GREATER(Dy, extents.y) && ((SIR(Dy)-1)^SIR(mDir.y))>=0.0f)	return FALSE;
+//	float Dz = mOrigin.z - center.z;	if(GREATER(Dz, extents.z) && ((SIR(Dz)-1)^SIR(mDir.z))>=0.0f)	return FALSE;
+
+	float f;
+	f = mDir.y * Dz - mDir.z * Dy;		if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y)	return FALSE;
+	f = mDir.z * Dx - mDir.x * Dz;		if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x)	return FALSE;
+	f = mDir.x * Dy - mDir.y * Dx;		if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x)	return FALSE;
+
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_RayCollider.cpp

@@ -0,0 +1,777 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a ray collider.
+ *	\file		OPC_RayCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a ray-vs-tree collider.
+ *	This class performs a stabbing query on an AABB tree, i.e. does a ray-mesh collision.
+ *
+ *	HIGHER DISTANCE BOUND:
+ *
+ *		If P0 and P1 are two 3D points, let's define:
+ *		- d = distance between P0 and P1
+ *		- Origin	= P0
+ *		- Direction	= (P1 - P0) / d = normalized direction vector
+ *		- A parameter t such as a point P on the line (P0,P1) is P = Origin + t * Direction
+ *		- t = 0  -->  P = P0
+ *		- t = d  -->  P = P1
+ *
+ *		Then we can define a general "ray" as:
+ *
+ *			struct Ray
+ *			{
+ *				Point	Origin;
+ *				Point	Direction;
+ *			};
+ *
+ *		But it actually maps three different things:
+ *		- a segment,   when 0 <= t <= d
+ *		- a half-line, when 0 <= t < +infinity, or -infinity < t <= d
+ *		- a line,      when -infinity < t < +infinity
+ *
+ *		In Opcode, we support segment queries, which yield half-line queries by setting d = +infinity.
+ *		We don't support line-queries. If you need them, shift the origin along the ray by an appropriate margin.
+ *
+ *		In short, the lower bound is always 0, and you can setup the higher bound "d" with RayCollider::SetMaxDist().
+ *
+ *		Query	|segment			|half-line		|line
+ *		--------|-------------------|---------------|----------------
+ *		Usages	|-shadow feelers	|-raytracing	|-
+ *				|-sweep tests		|-in/out tests	|
+ *
+ *	FIRST CONTACT:
+ *
+ *		- You can setup "first contact" mode or "all contacts" mode with RayCollider::SetFirstContact().
+ *		- In "first contact" mode we return as soon as the ray hits one face. If can be useful e.g. for shadow feelers, where
+ *		you want to know whether the path to the light is free or not (a boolean answer is enough).
+ *		- In "all contacts" mode we return all faces hit by the ray.
+ *
+ *	TEMPORAL COHERENCE:
+ *
+ *		- You can enable or disable temporal coherence with RayCollider::SetTemporalCoherence().
+ *		- It currently only works in "first contact" mode.
+ *		- If temporal coherence is enabled, the previously hit triangle is cached during the first query. Then, next queries
+ *		start by colliding the ray against the cached triangle. If they still collide, we return immediately.
+ *
+ *	CLOSEST HIT:
+ *
+ *		- You can enable or disable "closest hit" with RayCollider::SetClosestHit().
+ *		- It currently only works in "all contacts" mode.
+ *		- If closest hit is enabled, faces are sorted by distance on-the-fly and the closest one only is reported.
+ *
+ *	BACKFACE CULLING:
+ *
+ *		- You can enable or disable backface culling with RayCollider::SetCulling().
+ *		- If culling is enabled, ray will not hit back faces (only front faces).
+ *
+ *
+ *
+ *	\class		RayCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		June, 2, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	This class describes a face hit by a ray or segment.
+ *	This is a particular class dedicated to stabbing queries.
+ *
+ *	\class		CollisionFace
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	This class is a dedicated collection of CollisionFace.
+ *
+ *	\class		CollisionFaces
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_RayAABBOverlap.h"
+#include "ode/OPC_RayTriOverlap.h"
+
+#define SET_CONTACT(prim_index, flag)											\
+	mNbIntersections++;															\
+	/* Set contact status */													\
+	mFlags |= flag;																\
+	/* In any case the contact has been found and recorded in mStabbedFace  */	\
+	mStabbedFace.mFaceID = prim_index;
+
+#ifdef OPC_RAYHIT_CALLBACK
+
+	#define HANDLE_CONTACT(prim_index, flag)													\
+		SET_CONTACT(prim_index, flag)															\
+																								\
+		if(mHitCallback)	(mHitCallback)(mStabbedFace, mUserData);
+
+	#define UPDATE_CACHE					\
+		if(cache && GetContactStatus())		\
+		{									\
+			*cache	= mStabbedFace.mFaceID;	\
+		}
+#else
+
+	#define HANDLE_CONTACT(prim_index, flag)													\
+		SET_CONTACT(prim_index, flag)															\
+																								\
+		/* Now we can also record it in mStabbedFaces if available */							\
+		if(mStabbedFaces)																		\
+		{																						\
+			/* If we want all faces or if that's the first one we hit */						\
+			if(!mClosestHit || !mStabbedFaces->GetNbFaces())									\
+			{																					\
+				mStabbedFaces->AddFace(mStabbedFace);											\
+			}																					\
+			else																				\
+			{																					\
+				/* We only keep closest hit */													\
+				CollisionFace* Current = const_cast<CollisionFace*>(mStabbedFaces->GetFaces());	\
+				if(Current && mStabbedFace.mDistance<Current->mDistance)						\
+				{																				\
+					*Current = mStabbedFace;													\
+				}																				\
+			}																					\
+		}
+
+	#define UPDATE_CACHE												\
+		if(cache && GetContactStatus() && mStabbedFaces)				\
+		{																\
+			const CollisionFace* Current = mStabbedFaces->GetFaces();	\
+			if(Current)	*cache	= Current->mFaceID;						\
+			else		*cache	= INVALID_ID;							\
+		}
+#endif
+
+#define SEGMENT_PRIM(prim_index, flag)														\
+	/* Request vertices from the app */														\
+	VertexPointers VP;	mIMesh->GetTriangle(VP, prim_index);								\
+																							\
+	/* Perform ray-tri overlap test and return */											\
+	if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))							\
+	{																						\
+		/* Intersection point is valid if dist < segment's length */						\
+		/* We know dist>0 so we can use integers */											\
+		if(IR(mStabbedFace.mDistance)<IR(mMaxDist))											\
+		{																					\
+			HANDLE_CONTACT(prim_index, flag)												\
+		}																					\
+	}
+
+#define RAY_PRIM(prim_index, flag)															\
+	/* Request vertices from the app */														\
+	VertexPointers VP;	mIMesh->GetTriangle(VP, prim_index);								\
+																							\
+	/* Perform ray-tri overlap test and return */											\
+	if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))							\
+	{																						\
+		HANDLE_CONTACT(prim_index, flag)													\
+	}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+RayCollider::RayCollider() :
+
+#ifdef OPC_RAYHIT_CALLBACK
+	mNbRayBVTests		(0),
+	mNbRayPrimTests		(0),
+	mNbIntersections	(0),
+	mMaxDist			(MAX_FLOAT),
+	mHitCallback		(null),
+	mUserData			(0),
+#else
+	mStabbedFaces		(null),
+	mNbRayBVTests		(0),
+	mNbRayPrimTests		(0),
+	mNbIntersections	(0),
+	mMaxDist			(MAX_FLOAT),
+	mClosestHit			(false),
+#endif
+	mCulling			(true)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+RayCollider::~RayCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Validates current settings. You should call this method after all the settings and callbacks have been defined.
+ *	\return		null if everything is ok, else a string describing the problem
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const char* RayCollider::ValidateSettings()
+{
+	if(mMaxDist<0.0f)											return "Higher distance bound must be positive!";
+	if(TemporalCoherenceEnabled() && !FirstContactEnabled())	return "Temporal coherence only works with ""First contact"" mode!";
+#ifndef OPC_RAYHIT_CALLBACK
+	if(mClosestHit && FirstContactEnabled())					return "Closest hit doesn't work with ""First contact"" mode!";
+	if(TemporalCoherenceEnabled() && mClosestHit)				return "Temporal coherence can't guarantee to report closest hit!";
+#endif
+	if(SkipPrimitiveTests())									return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)";
+	return null;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic stabbing query for generic OPCODE models. After the call, access the results:
+ *	- with GetContactStatus()
+ *	- in the user-provided destination array
+ *
+ *	\param		world_ray		[in] stabbing ray in world space
+ *	\param		model			[in] Opcode model to collide with
+ *	\param		world			[in] model's world matrix, or null
+ *	\param		cache			[in] a possibly cached face index, or null
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache)
+// ericf change
+bool RayCollider::Collide(const Ray& world_ray, Model& model, const Matrix4x4* world, udword* cache)
+{
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(world_ray, world, cache))	return true;
+
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform stabbing query
+			if(IR(mMaxDist)!=IEEE_MAX_FLOAT)	_SegmentStab(Tree->GetNodes());
+			else								_RayStab(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform stabbing query
+			if(IR(mMaxDist)!=IEEE_MAX_FLOAT)	_SegmentStab(Tree->GetNodes());
+			else								_RayStab(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform stabbing query
+			if(IR(mMaxDist)!=IEEE_MAX_FLOAT)	_SegmentStab(Tree->GetNodes());
+			else								_RayStab(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform stabbing query
+			if(IR(mMaxDist)!=IEEE_MAX_FLOAT)	_SegmentStab(Tree->GetNodes());
+			else								_RayStab(Tree->GetNodes());
+		}
+	}
+
+	// Update cache if needed
+	UPDATE_CACHE
+	return true;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a stabbing query :
+ *	- reset stats & contact status
+ *	- compute ray in local space
+ *	- check temporal coherence
+ *
+ *	\param		world_ray	[in] stabbing ray in world space
+ *	\param		world		[in] object's world matrix, or null
+ *	\param		face_id		[in] index of previously stabbed triangle
+ *	\return		TRUE if we can return immediately
+ *	\warning	SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id)
+{
+	// Reset stats & contact status
+	Collider::InitQuery();
+	mNbRayBVTests		= 0;
+	mNbRayPrimTests		= 0;
+	mNbIntersections	= 0;
+#ifndef OPC_RAYHIT_CALLBACK
+	if(mStabbedFaces)	mStabbedFaces->Reset();
+#endif
+
+	// Compute ray in local space
+	// The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
+	if(world)
+	{
+		Matrix3x3 InvWorld = *world;
+		mDir = InvWorld * world_ray.mDir;
+
+		Matrix4x4 World;
+		InvertPRMatrix(World, *world);
+		mOrigin = world_ray.mOrig * World;
+	}
+	else
+	{
+		mDir	= world_ray.mDir;
+		mOrigin	= world_ray.mOrig;
+	}
+
+	// 4) Special case: 1-triangle meshes [Opcode 1.3]
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
+		if(!SkipPrimitiveTests())
+		{
+			// Perform overlap test between the unique triangle and the ray (and set contact status if needed)
+			SEGMENT_PRIM(udword(0), OPC_CONTACT)
+
+			// Return immediately regardless of status
+			return TRUE;
+		}
+	}
+
+	// Check temporal coherence :
+
+	// Test previously colliding primitives first
+	if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID)
+	{
+#ifdef OLD_CODE
+#ifndef OPC_RAYHIT_CALLBACK
+		if(!mClosestHit)
+#endif
+		{
+			// Request vertices from the app
+			VertexPointers VP;
+			mIMesh->GetTriangle(VP, *face_id);
+			// Perform ray-cached tri overlap test
+			if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
+			{
+				// Intersection point is valid if:
+				// - distance is positive (else it can just be a face behind the orig point)
+				// - distance is smaller than a given max distance (useful for shadow feelers)
+//				if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistance<mMaxDist)
+				if(IR(mStabbedFace.mDistance)<IR(mMaxDist))	// The other test is already performed in RayTriOverlap
+				{
+					// Set contact status
+					mFlags |= OPC_TEMPORAL_CONTACT;
+
+					mStabbedFace.mFaceID = *face_id;
+
+#ifndef OPC_RAYHIT_CALLBACK
+					if(mStabbedFaces)	mStabbedFaces->AddFace(mStabbedFace);
+#endif
+					return TRUE;
+				}
+			}
+		}
+#else
+		// New code
+		// We handle both Segment/ray queries with the same segment code, and a possible infinite limit
+		SEGMENT_PRIM(*face_id, OPC_TEMPORAL_CONTACT)
+
+		// Return immediately if possible
+		if(GetContactStatus())	return TRUE;
+#endif
+	}
+
+	// Precompute data (moved after temporal coherence since only needed for ray-AABB)
+	if(IR(mMaxDist)!=IEEE_MAX_FLOAT)
+	{
+		// For Segment-AABB overlap
+		mData = 0.5f * mDir * mMaxDist;
+		mData2 = mOrigin + mData;
+
+		// Precompute mFDir;
+		mFDir.x = fabsf(mData.x);
+		mFDir.y = fabsf(mData.y);
+		mFDir.z = fabsf(mData.z);
+	}
+	else
+	{
+		// For Ray-AABB overlap
+//		udword x = SIR(mDir.x)-1;
+//		udword y = SIR(mDir.y)-1;
+//		udword z = SIR(mDir.z)-1;
+//		mData.x = FR(x);
+//		mData.y = FR(y);
+//		mData.z = FR(z);
+
+		// Precompute mFDir;
+		mFDir.x = fabsf(mDir.x);
+		mFDir.y = fabsf(mDir.y);
+		mFDir.z = fabsf(mDir.z);
+	}
+
+	return FALSE;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Stabbing query for vanilla AABB trees.
+ *	\param		world_ray		[in] stabbing ray in world space
+ *	\param		tree			[in] AABB tree
+ *	\param		box_indices		[out] indices of stabbed boxes
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices)
+{
+	// ### bad design here
+
+	// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
+	// So we don't really have "primitives" to deal with. Hence it doesn't work with
+	// "FirstContact" + "TemporalCoherence".
+	ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
+
+	// Checkings
+	if(!tree)					return false;
+
+	// Init collision query
+	// Basically this is only called to initialize precomputed data
+	if(InitQuery(world_ray))	return true;
+
+	// Perform stabbing query
+	if(IR(mMaxDist)!=IEEE_MAX_FLOAT)	_SegmentStab(tree, box_indices);
+	else								_RayStab(tree, box_indices);
+
+	return true;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_SegmentStab(const AABBCollisionNode* node)
+{
+	// Perform Segment-AABB overlap test
+	if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	if(node->IsLeaf())
+	{
+		SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_SegmentStab(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_SegmentStab(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_SegmentStab(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Segment-AABB overlap test
+	if(!SegmentAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf())
+	{
+		SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_SegmentStab(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_SegmentStab(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_SegmentStab(const AABBNoLeafNode* node)
+{
+	// Perform Segment-AABB overlap test
+	if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	if(node->HasPosLeaf())
+	{
+		SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
+	}
+	else _SegmentStab(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())
+	{
+		SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
+	}
+	else _SegmentStab(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_SegmentStab(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Segment-AABB overlap test
+	if(!SegmentAABBOverlap(Center, Extents))	return;
+
+	if(node->HasPosLeaf())
+	{
+		SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
+	}
+	else _SegmentStab(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())
+	{
+		SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
+	}
+	else _SegmentStab(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for vanilla AABB trees.
+ *	\param		node		[in] current collision node
+ *	\param		box_indices	[out] indices of stabbed boxes
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices)
+{
+	// Test the box against the segment
+	Point Center, Extents;
+	node->GetAABB()->GetCenter(Center);
+	node->GetAABB()->GetExtents(Extents);
+	if(!SegmentAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf())
+	{
+		box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
+	}
+	else
+	{
+		_SegmentStab(node->GetPos(), box_indices);
+		_SegmentStab(node->GetNeg(), box_indices);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_RayStab(const AABBCollisionNode* node)
+{
+	// Perform Ray-AABB overlap test
+	if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	if(node->IsLeaf())
+	{
+		RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_RayStab(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_RayStab(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_RayStab(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Ray-AABB overlap test
+	if(!RayAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf())
+	{
+		RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_RayStab(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_RayStab(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_RayStab(const AABBNoLeafNode* node)
+{
+	// Perform Ray-AABB overlap test
+	if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	if(node->HasPosLeaf())
+	{
+		RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
+	}
+	else _RayStab(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())
+	{
+		RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
+	}
+	else _RayStab(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_RayStab(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Ray-AABB overlap test
+	if(!RayAABBOverlap(Center, Extents))	return;
+
+	if(node->HasPosLeaf())
+	{
+		RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
+	}
+	else _RayStab(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())
+	{
+		RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
+	}
+	else _RayStab(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive stabbing query for vanilla AABB trees.
+ *	\param		node		[in] current collision node
+ *	\param		box_indices	[out] indices of stabbed boxes
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices)
+{
+	// Test the box against the ray
+	Point Center, Extents;
+	node->GetAABB()->GetCenter(Center);
+	node->GetAABB()->GetExtents(Extents);
+	if(!RayAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf())
+	{
+		mFlags |= OPC_CONTACT;
+		box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
+	}
+	else
+	{
+		_RayStab(node->GetPos(), box_indices);
+		_RayStab(node->GetNeg(), box_indices);
+	}
+}

src/external/open_dynamics_engine-ef/ode/OPC_RayCollider.h

@@ -0,0 +1,227 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a ray collider.
+ *	\file		OPC_RayCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_RAYCOLLIDER_H__
+#define __OPC_RAYCOLLIDER_H__
+
+	class OPCODE_API CollisionFace
+	{
+		public:
+		//! Constructor
+		inline_				CollisionFace()			{}
+		//! Destructor
+		inline_				~CollisionFace()		{}
+
+				udword		mFaceID;				//!< Index of touched face
+				float		mDistance;				//!< Distance from collider to hitpoint
+				float		mU, mV;					//!< Impact barycentric coordinates
+	};
+
+	class OPCODE_API CollisionFaces : private Container
+	{
+		public:
+		//! Constructor
+										CollisionFaces()						{}
+		//! Destructor
+										~CollisionFaces()						{}
+
+		inline_	udword					GetNbFaces()					const	{ return GetNbEntries()>>2;						}
+		inline_	const CollisionFace*	GetFaces()						const	{ return (const CollisionFace*)GetEntries();	}
+
+		inline_	void					Reset()									{ Container::Reset();							}
+
+		inline_	void					AddFace(const CollisionFace& face)		{ Add(face.mFaceID).Add(face.mDistance).Add(face.mU).Add(face.mV);	}
+	};
+
+#ifdef OPC_RAYHIT_CALLBACK
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	User-callback, called by OPCODE to record a hit.
+	 *	\param		hit			[in] current hit
+	 *	\param		user_data	[in] user-defined data from SetCallback()
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	typedef void	(*HitCallback)	(const CollisionFace& hit, void* user_data);
+#endif
+
+	class OPCODE_API RayCollider : public Collider
+	{
+		public:
+		// Constructor / Destructor
+											RayCollider();
+		virtual								~RayCollider();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic stabbing query for generic OPCODE models. After the call, access the results:
+		 *	- with GetContactStatus()
+		 *	- in the user-provided destination array
+		 *
+		 *	\param		world_ray		[in] stabbing ray in world space
+		 *	\param		model			[in] Opcode model to collide with
+		 *	\param		world			[in] model's world matrix, or null
+		 *	\param		cache			[in] a possibly cached face index, or null
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        //bool			Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world=null, udword* cache=null);
+        // ericf
+							bool			Collide(const Ray& world_ray, Model& model, const Matrix4x4* world=null, udword* cache=null);
+		//
+							bool			Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices);
+		// Settings
+
+#ifndef OPC_RAYHIT_CALLBACK
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: enable or disable "closest hit" mode.
+		 *	\param		flag		[in] true to report closest hit only
+		 *	\see		SetCulling(bool flag)
+		 *	\see		SetMaxDist(float max_dist)
+		 *	\see		SetDestination(StabbedFaces* sf)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetClosestHit(bool flag)				{ mClosestHit	= flag;		}
+#endif
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: enable or disable backface culling.
+		 *	\param		flag		[in] true to enable backface culling
+		 *	\see		SetClosestHit(bool flag)
+		 *	\see		SetMaxDist(float max_dist)
+		 *	\see		SetDestination(StabbedFaces* sf)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetCulling(bool flag)					{ mCulling		= flag;		}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: sets the higher distance bound.
+		 *	\param		max_dist	[in] higher distance bound. Default = maximal value, for ray queries (else segment)
+		 *	\see		SetClosestHit(bool flag)
+		 *	\see		SetCulling(bool flag)
+		 *	\see		SetDestination(StabbedFaces* sf)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetMaxDist(float max_dist=MAX_FLOAT)	{ mMaxDist		= max_dist;	}
+
+#ifdef OPC_RAYHIT_CALLBACK
+		inline_				void			SetHitCallback(HitCallback cb)			{ mHitCallback	= cb;			}
+		inline_				void			SetUserData(void* user_data)			{ mUserData		= user_data;	}
+#else
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: sets the destination array for stabbed faces.
+		 *	\param		cf			[in] destination array, filled during queries
+		 *	\see		SetClosestHit(bool flag)
+		 *	\see		SetCulling(bool flag)
+		 *	\see		SetMaxDist(float max_dist)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetDestination(CollisionFaces* cf)		{ mStabbedFaces	= cf;		}
+#endif
+		// Stats
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of Ray-BV overlap tests after a collision query.
+		 *	\see		GetNbRayPrimTests()
+		 *	\see		GetNbIntersections()
+		 *	\return		the number of Ray-BV tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbRayBVTests()				const	{ return mNbRayBVTests;		}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of Ray-Triangle overlap tests after a collision query.
+		 *	\see		GetNbRayBVTests()
+		 *	\see		GetNbIntersections()
+		 *	\return		the number of Ray-Triangle tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbRayPrimTests()				const	{ return mNbRayPrimTests;	}
+
+		// In-out test
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of intersection found after a collision query. Can be used for in/out tests.
+		 *	\see		GetNbRayBVTests()
+		 *	\see		GetNbRayPrimTests()
+		 *	\return		the number of valid intersections during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbIntersections()			const	{ return mNbIntersections;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
+		 *	\return		null if everything is ok, else a string describing the problem
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(Collider)	const char*		ValidateSettings();
+
+		protected:
+		// Ray in local space
+							Point			mOrigin;			//!< Ray origin
+							Point			mDir;				//!< Ray direction (normalized)
+							Point			mFDir;				//!< fabsf(mDir)
+							Point			mData, mData2;
+		// Stabbed faces
+							CollisionFace	mStabbedFace;		//!< Current stabbed face
+#ifdef OPC_RAYHIT_CALLBACK
+							HitCallback		mHitCallback;		//!< Callback used to record a hit
+							void*			mUserData;			//!< User-defined data
+#else
+							CollisionFaces*	mStabbedFaces;		//!< List of stabbed faces
+#endif
+		// Stats
+							udword			mNbRayBVTests;		//!< Number of Ray-BV tests
+							udword			mNbRayPrimTests;	//!< Number of Ray-Primitive tests
+		// In-out test
+							udword			mNbIntersections;	//!< Number of valid intersections
+		// Dequantization coeffs
+							Point			mCenterCoeff;
+							Point			mExtentsCoeff;
+		// Settings
+							float			mMaxDist;			//!< Valid segment on the ray
+#ifndef OPC_RAYHIT_CALLBACK
+							bool			mClosestHit;		//!< Report closest hit only
+#endif
+							bool			mCulling;			//!< Stab culled faces or not
+		// Internal methods
+							void			_SegmentStab(const AABBCollisionNode* node);
+							void			_SegmentStab(const AABBNoLeafNode* node);
+							void			_SegmentStab(const AABBQuantizedNode* node);
+							void			_SegmentStab(const AABBQuantizedNoLeafNode* node);
+							void			_SegmentStab(const AABBTreeNode* node, Container& box_indices);
+							void			_RayStab(const AABBCollisionNode* node);
+							void			_RayStab(const AABBNoLeafNode* node);
+							void			_RayStab(const AABBQuantizedNode* node);
+							void			_RayStab(const AABBQuantizedNoLeafNode* node);
+							void			_RayStab(const AABBTreeNode* node, Container& box_indices);
+			// Overlap tests
+		inline_				BOOL			RayAABBOverlap(const Point& center, const Point& extents);
+		inline_				BOOL			SegmentAABBOverlap(const Point& center, const Point& extents);
+		inline_				BOOL			RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2);
+			// Init methods
+							BOOL			InitQuery(const Ray& world_ray, const Matrix4x4* world=null, udword* face_id=null);
+	};
+
+#endif // __OPC_RAYCOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_RayTriOverlap.h

@@ -0,0 +1,89 @@
+#define LOCAL_EPSILON 0.000001f
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes a ray-triangle intersection test.
+ *	Original code from Tomas Möller's "Fast Minimum Storage Ray-Triangle Intersection".
+ *	It's been optimized a bit with integer code, and modified to return a non-intersection if distance from
+ *	ray origin to triangle is negative.
+ *
+ *	\param		vert0	[in] triangle vertex
+ *	\param		vert1	[in] triangle vertex
+ *	\param		vert2	[in] triangle vertex
+ *	\return		true on overlap. mStabbedFace is filled with relevant info.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL RayCollider::RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
+{
+	// Stats
+	mNbRayPrimTests++;
+
+	// Find vectors for two edges sharing vert0
+	Point edge1 = vert1 - vert0;
+	Point edge2 = vert2 - vert0;
+
+	// Begin calculating determinant - also used to calculate U parameter
+	Point pvec = mDir^edge2;
+
+	// If determinant is near zero, ray lies in plane of triangle
+	float det = edge1|pvec;
+
+	if(mCulling)
+	{
+		if(det<LOCAL_EPSILON)														return FALSE;
+		// From here, det is > 0. So we can use integer cmp.
+
+		// Calculate distance from vert0 to ray origin
+		Point tvec = mOrigin - vert0;
+
+		// Calculate U parameter and test bounds
+		mStabbedFace.mU = tvec|pvec;
+//		if(IR(u)&0x80000000 || u>det)					return FALSE;
+		if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IR(det))		return FALSE;
+
+		// Prepare to test V parameter
+		Point qvec = tvec^edge1;
+
+		// Calculate V parameter and test bounds
+		mStabbedFace.mV = mDir|qvec;
+		if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>det)	return FALSE;
+
+		// Calculate t, scale parameters, ray intersects triangle
+		mStabbedFace.mDistance = edge2|qvec;
+		// Det > 0 so we can early exit here
+		// Intersection point is valid if distance is positive (else it can just be a face behind the orig point)
+		if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance))								return FALSE;
+		// Else go on
+		float OneOverDet = 1.0f / det;
+		mStabbedFace.mDistance *= OneOverDet;
+		mStabbedFace.mU *= OneOverDet;
+		mStabbedFace.mV *= OneOverDet;
+	}
+	else
+	{
+		// the non-culling branch
+		if(det>-LOCAL_EPSILON && det<LOCAL_EPSILON)									return FALSE;
+		float OneOverDet = 1.0f / det;
+
+		// Calculate distance from vert0 to ray origin
+		Point tvec = mOrigin - vert0;
+
+		// Calculate U parameter and test bounds
+		mStabbedFace.mU = (tvec|pvec) * OneOverDet;
+//		if(IR(u)&0x80000000 || u>1.0f)					return FALSE;
+		if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IEEE_1_0)		return FALSE;
+
+		// prepare to test V parameter
+		Point qvec = tvec^edge1;
+
+		// Calculate V parameter and test bounds
+		mStabbedFace.mV = (mDir|qvec) * OneOverDet;
+		if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>1.0f)	return FALSE;
+
+		// Calculate t, ray intersects triangle
+		mStabbedFace.mDistance = (edge2|qvec) * OneOverDet;
+		// Intersection point is valid if distance is positive (else it can just be a face behind the orig point)
+		if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance))								return FALSE;
+	}
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_Settings.h

@@ -0,0 +1,49 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains compilation flags.
+ *	\file		OPC_Settings.h
+ *	\author		Pierre Terdiman
+ *	\date		May, 12, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_SETTINGS_H__
+#define __OPC_SETTINGS_H__
+
+	//! Use CPU comparisons (comment that line to use standard FPU compares)
+	#define OPC_CPU_COMPARE
+
+	//! Use FCOMI / FCMOV on Pentium-Pro based processors (comment that line to use plain C++)
+	#define OPC_USE_FCOMI
+
+	//! Use epsilon value in tri-tri overlap test
+	#define OPC_TRITRI_EPSILON_TEST
+
+	//! Use tree-coherence or not [not implemented yet]
+//	#define OPC_USE_TREE_COHERENCE
+
+	//! Use callbacks or direct pointers. Using callbacks might be a bit slower (but probably not much)
+//	#define OPC_USE_CALLBACKS
+
+	//! Support triangle and vertex strides or not. Using strides might be a bit slower (but probably not much)
+	#define OPC_USE_STRIDE
+
+	//! Discard negative pointer in vanilla trees
+	#define OPC_NO_NEG_VANILLA_TREE
+
+	//! Use a callback in the ray collider
+	//#define OPC_RAYHIT_CALLBACK
+
+	// NB: no compilation flag to enable/disable stats since they're actually needed in the box/box overlap test
+
+#endif //__OPC_SETTINGS_H__

src/external/open_dynamics_engine-ef/ode/OPC_SphereAABBOverlap.h

@@ -0,0 +1,128 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Sphere-AABB overlap test, based on Jim Arvo's code.
+ *	\param		center		[in] box center
+ *	\param		extents		[in] box extents
+ *	\return		TRUE on overlap
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL SphereCollider::SphereAABBOverlap(const Point& center, const Point& extents)
+{ 
+	// Stats
+	mNbVolumeBVTests++;
+
+	float d = 0.0f;
+
+	//find the square of the distance
+	//from the sphere to the box
+#ifdef OLDIES
+	for(udword i=0;i<3;i++)
+	{
+		float tmp = mCenter[i] - center[i];
+		float s = tmp + extents[i];
+
+		if(s<0.0f)	d += s*s;
+		else
+		{
+			s = tmp - extents[i];
+			if(s>0.0f)	d += s*s;
+		}
+	}
+#endif
+
+//#ifdef NEW_TEST
+
+//	float tmp = mCenter.x - center.x;
+//	float s = tmp + extents.x;
+
+	float tmp,s;
+
+	tmp = mCenter.x - center.x;
+	s = tmp + extents.x;
+
+	if(s<0.0f)
+	{
+		d += s*s;
+		if(d>mRadius2)	return FALSE;
+	}
+	else
+	{
+		s = tmp - extents.x;
+		if(s>0.0f)
+		{
+			d += s*s;
+			if(d>mRadius2)	return FALSE;
+		}
+	}
+
+	tmp = mCenter.y - center.y;
+	s = tmp + extents.y;
+
+	if(s<0.0f)
+	{
+		d += s*s;
+		if(d>mRadius2)	return FALSE;
+	}
+	else
+	{
+		s = tmp - extents.y;
+		if(s>0.0f)
+		{
+			d += s*s;
+			if(d>mRadius2)	return FALSE;
+		}
+	}
+
+	tmp = mCenter.z - center.z;
+	s = tmp + extents.z;
+
+	if(s<0.0f)
+	{
+		d += s*s;
+		if(d>mRadius2)	return FALSE;
+	}
+	else
+	{
+		s = tmp - extents.z;
+		if(s>0.0f)
+		{
+			d += s*s;
+			if(d>mRadius2)	return FALSE;
+		}
+	}
+//#endif
+
+#ifdef OLDIES
+//	Point Min = center - extents;
+//	Point Max = center + extents;
+
+	float d = 0.0f;
+
+	//find the square of the distance
+	//from the sphere to the box
+	for(udword i=0;i<3;i++)
+	{
+float Min = center[i] - extents[i];
+
+//		if(mCenter[i]<Min[i])
+		if(mCenter[i]<Min)
+		{
+//			float s = mCenter[i] - Min[i];
+			float s = mCenter[i] - Min;
+			d += s*s;
+		}
+		else
+		{
+float Max = center[i] + extents[i];
+
+//			if(mCenter[i]>Max[i])
+			if(mCenter[i]>Max)
+			{
+				float s = mCenter[i] - Max;
+				d += s*s;
+			}
+		}
+	}
+#endif
+	return d <= mRadius2;
+}

src/external/open_dynamics_engine-ef/ode/OPC_SphereCollider.cpp

@@ -0,0 +1,738 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a sphere collider.
+ *	\file		OPC_SphereCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains a sphere-vs-tree collider.
+ *	This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision,
+ *	in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a
+ *	debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of
+ *	efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever).
+ *
+ *	\class		SphereCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		June, 2, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_SphereAABBOverlap.h"
+#include "ode/OPC_SphereTriOverlap.h"
+
+#define SET_CONTACT(prim_index, flag)									\
+	/* Set contact status */											\
+	mFlags |= flag;														\
+	mTouchedPrimitives->Add(prim_index);
+
+//! Sphere-triangle overlap test
+#define SPHERE_PRIM(prim_index, flag)									\
+	/* Request vertices from the app */									\
+	VertexPointers VP;	mIMesh->GetTriangle(VP, prim_index);			\
+																		\
+	/* Perform sphere-tri overlap test */								\
+	if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))	\
+	{																	\
+		SET_CONTACT(prim_index, flag)									\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SphereCollider::SphereCollider()
+{
+	mCenter.Zero();
+	mRadius2 = 0.0f;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SphereCollider::~SphereCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic collision query for generic OPCODE models. After the call, access the results:
+ *	- with GetContactStatus()
+ *	- with GetNbTouchedPrimitives()
+ *	- with GetTouchedPrimitives()
+ *
+ *	\param		cache		[in/out] a sphere cache
+ *	\param		sphere		[in] collision sphere in local space
+ *	\param		model		[in] Opcode model to collide with
+ *	\param		worlds		[in] sphere's world matrix, or null
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
+// ericf change
+bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
+{
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, sphere, worlds, worldm))	return true;
+
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query
+			if(SkipPrimitiveTests())	_CollideNoPrimitiveTest(Tree->GetNodes());
+			else						_Collide(Tree->GetNodes());
+		}
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a collision query :
+ *	- reset stats & contact status
+ *	- setup matrices
+ *	- check temporal coherence
+ *
+ *	\param		cache		[in/out] a sphere cache
+ *	\param		sphere		[in] sphere in local space
+ *	\param		worlds		[in] sphere's world matrix, or null
+ *	\param		worldm		[in] model's world matrix, or null
+ *	\return		TRUE if we can return immediately
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+BOOL SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm)
+{
+	// 1) Call the base method
+	VolumeCollider::InitQuery();
+
+	// 2) Compute sphere in model space:
+	// - Precompute R^2
+	mRadius2 = sphere.mRadius * sphere.mRadius;
+	// - Compute center position
+	mCenter = sphere.mCenter;
+	// -> to world space
+	if(worlds)	mCenter *= *worlds;
+	// -> to model space
+	if(worldm)
+	{
+		// Invert model matrix
+		Matrix4x4 InvWorldM;
+		InvertPRMatrix(InvWorldM, *worldm);
+
+		mCenter *= InvWorldM;
+	}
+
+	// 3) Setup destination pointer
+	mTouchedPrimitives = &cache.TouchedPrimitives;
+
+	// 4) Special case: 1-triangle meshes [Opcode 1.3]
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		if(!SkipPrimitiveTests())
+		{
+			// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
+			mTouchedPrimitives->Reset();
+
+			// Perform overlap test between the unique triangle and the sphere (and set contact status if needed)
+			SPHERE_PRIM(udword(0), OPC_CONTACT)
+
+			// Return immediately regardless of status
+			return TRUE;
+		}
+	}
+
+	// 5) Check temporal coherence :
+	if(TemporalCoherenceEnabled())
+	{
+		// Here we use temporal coherence
+		// => check results from previous frame before performing the collision query
+		if(FirstContactEnabled())
+		{
+			// We're only interested in the first contact found => test the unique previously touched face
+			if(mTouchedPrimitives->GetNbEntries())
+			{
+				// Get index of previously touched face = the first entry in the array
+				udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
+
+				// Then reset the array:
+				// - if the overlap test below is successful, the index we'll get added back anyway
+				// - if it isn't, then the array should be reset anyway for the normal query
+				mTouchedPrimitives->Reset();
+
+				// Perform overlap test between the cached triangle and the sphere (and set contact status if needed)
+				SPHERE_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
+
+				// Return immediately if possible
+				if(GetContactStatus())	return TRUE;
+			}
+			// else no face has been touched during previous query
+			// => we'll have to perform a normal query
+		}
+		else
+		{
+			// We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious):
+			float r = sqrtf(cache.FatRadius2) - sphere.mRadius;
+			if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r)
+			{
+				// - if N is included in P, return previous list
+				// => we simply leave the list (mTouchedFaces) unchanged
+
+				// Set contact status if needed
+				if(mTouchedPrimitives->GetNbEntries())	mFlags |= OPC_TEMPORAL_CONTACT;
+
+				// In any case we don't need to do a query
+				return TRUE;
+			}
+			else
+			{
+				// - else do the query using a fat N
+
+				// Reset cache since we'll about to perform a real query
+				mTouchedPrimitives->Reset();
+
+				// Make a fat sphere so that coherence will work for subsequent frames
+				mRadius2 *= cache.FatCoeff;
+//				mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff);
+
+				// Update cache with query data (signature for cached faces)
+				cache.Center = mCenter;
+				cache.FatRadius2 = mRadius2;
+			}
+		}
+	}
+	else
+	{
+		// Here we don't use temporal coherence => do a normal query
+		mTouchedPrimitives->Reset();
+	}
+
+	return FALSE;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for vanilla AABB trees.
+ *	\param		cache		[in/out] a sphere cache
+ *	\param		sphere		[in] collision sphere in world space
+ *	\param		tree		[in] AABB tree
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree)
+{
+	// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
+	// So we don't really have "primitives" to deal with. Hence it doesn't work with
+	// "FirstContact" + "TemporalCoherence".
+	ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
+
+	// Checkings
+	if(!tree)	return false;
+
+	// Init collision query
+	if(InitQuery(cache, sphere))	return true;
+
+	// Perform collision query
+	_Collide(tree);
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Checks the sphere completely contains the box. In which case we can end the query sooner.
+ *	\param		bc	[in] box center
+ *	\param		be	[in] box extents
+ *	\return		true if the sphere contains the whole box
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be)
+{
+	// I assume if all 8 box vertices are inside the sphere, so does the whole box.
+	// Sounds ok but maybe there's a better way?
+	Point p;
+	p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z;	if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x-be.x;									if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x+be.x; p.y=bc.y-be.y;					if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x-be.x;									if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z;	if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x-be.x;									if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x+be.x; p.y=bc.y-be.y;					if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+	p.x=bc.x-be.x;									if(mCenter.SquareDistance(p)>=mRadius2)	return FALSE;
+
+	return TRUE;
+}
+
+#define TEST_BOX_IN_SPHERE(center, extents)	\
+	if(SphereContainsBox(center, extents))	\
+	{										\
+		/* Set contact status */			\
+		mFlags |= OPC_CONTACT;				\
+		_Dump(node);						\
+		return;								\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_Collide(const AABBCollisionNode* node)
+{
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
+{
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_Collide(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_SPHERE(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_Collide(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_Collide(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_SPHERE(Center, Extents)
+
+	if(node->IsLeaf())
+	{
+		SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
+	}
+	else
+	{
+		_CollideNoPrimitiveTest(node->GetPos());
+
+		if(ContactFound()) return;
+
+		_CollideNoPrimitiveTest(node->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_Collide(const AABBNoLeafNode* node)
+{
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
+{
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))	return;
+
+	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_Collide(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_SPHERE(Center, Extents)
+
+	if(node->HasPosLeaf())	{ SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_Collide(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
+{
+	// Dequantize box
+	const QuantizedAABB& Box = node->mAABB;
+	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
+	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
+
+	// Perform Sphere-AABB overlap test
+	if(!SphereAABBOverlap(Center, Extents))	return;
+
+	TEST_BOX_IN_SPHERE(Center, Extents)
+
+	if(node->HasPosLeaf())	{ SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetPos());
+
+	if(ContactFound()) return;
+
+	if(node->HasNegLeaf())	{ SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
+	else					_CollideNoPrimitiveTest(node->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for vanilla AABB trees.
+ *	\param		node	[in] current collision node
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void SphereCollider::_Collide(const AABBTreeNode* node)
+{
+	// Perform Sphere-AABB overlap test
+	Point Center, Extents;
+	node->GetAABB()->GetCenter(Center);
+	node->GetAABB()->GetExtents(Extents);
+	if(!SphereAABBOverlap(Center, Extents))	return;
+
+	if(node->IsLeaf() || SphereContainsBox(Center, Extents))
+	{
+		mFlags |= OPC_CONTACT;
+		mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
+	}
+	else
+	{
+		_Collide(node->GetPos());
+		_Collide(node->GetNeg());
+	}
+}
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridSphereCollider::HybridSphereCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridSphereCollider::~HybridSphereCollider()
+{
+}
+
+//bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
+// ericf change
+bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
+{
+	// We don't want primitive tests here!
+	mFlags |= OPC_NO_PRIMITIVE_TESTS;
+
+	// Checkings
+	if(!Setup(&model))	return false;
+
+	// Init collision query
+	if(InitQuery(cache, sphere, worlds, worldm))	return true;
+
+	// Special case for 1-leaf trees
+	if(mCurrentModel && mCurrentModel->HasSingleNode())
+	{
+		// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
+		udword Nb = mIMesh->GetNbTriangles();
+
+		// Loop through all triangles
+		for(udword i=0;i<Nb;i++)
+		{
+			SPHERE_PRIM(i, OPC_CONTACT)
+		}
+		return true;
+	}
+
+	// Override destination array since we're only going to get leaf boxes here
+	mTouchedBoxes.Reset();
+	mTouchedPrimitives = &mTouchedBoxes;
+
+	// Now, do the actual query against leaf boxes
+	if(!model.HasLeafNodes())
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+	else
+	{
+		if(model.IsQuantized())
+		{
+			const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
+
+			// Setup dequantization coeffs
+			mCenterCoeff	= Tree->mCenterCoeff;
+			mExtentsCoeff	= Tree->mExtentsCoeff;
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+		else
+		{
+			const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
+
+			// Perform collision query - we don't want primitive tests here!
+			_CollideNoPrimitiveTest(Tree->GetNodes());
+		}
+	}
+
+	// We only have a list of boxes so far
+	if(GetContactStatus())
+	{
+		// Reset contact status, since it currently only reflects collisions with leaf boxes
+		Collider::InitQuery();
+
+		// Change dest container so that we can use built-in overlap tests and get collided primitives
+		cache.TouchedPrimitives.Reset();
+		mTouchedPrimitives = &cache.TouchedPrimitives;
+
+		// Read touched leaf boxes
+		udword Nb = mTouchedBoxes.GetNbEntries();
+		const udword* Touched = mTouchedBoxes.GetEntries();
+
+		const LeafTriangles* LT = model.GetLeafTriangles();
+		const udword* Indices = model.GetIndices();
+
+		// Loop through touched leaves
+		while(Nb--)
+		{
+			const LeafTriangles& CurrentLeaf = LT[*Touched++];
+
+			// Each leaf box has a set of triangles
+			udword NbTris = CurrentLeaf.GetNbTriangles();
+			if(Indices)
+			{
+				const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = *T++;
+					SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+			else
+			{
+				udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+				// Loop through triangles and test each of them
+				while(NbTris--)
+				{
+					udword TriangleIndex = BaseIndex++;
+					SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
+				}
+			}
+		}
+	}
+
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_SphereCollider.h

@@ -0,0 +1,100 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a sphere collider.
+ *	\file		OPC_SphereCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_SPHERECOLLIDER_H__
+#define __OPC_SPHERECOLLIDER_H__
+
+	struct OPCODE_API SphereCache : VolumeCache
+	{
+					SphereCache() : Center(0.0f,0.0f,0.0f), FatRadius2(0.0f), FatCoeff(1.1f)	{}
+					~SphereCache()																{}
+
+		// Cached faces signature
+		Point		Center;		//!< Sphere used when performing the query resulting in cached faces
+		float		FatRadius2;	//!< Sphere used when performing the query resulting in cached faces
+		// User settings
+		float		FatCoeff;	//!< mRadius2 multiplier used to create a fat sphere
+	};
+
+	class OPCODE_API SphereCollider : public VolumeCollider
+	{
+		public:
+		// Constructor / Destructor
+											SphereCollider();
+		virtual								~SphereCollider();
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic collision query for generic OPCODE models. After the call, access the results:
+		 *	- with GetContactStatus()
+		 *	- with GetNbTouchedPrimitives()
+		 *	- with GetTouchedPrimitives()
+		 *
+		 *	\param		cache			[in/out] a sphere cache
+		 *	\param		sphere			[in] collision sphere in local space
+		 *	\param		model			[in] Opcode model to collide with
+		 *	\param		worlds			[in] sphere's world matrix, or null
+		 *	\param		worldm			[in] model's world matrix, or null
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+        //bool			Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(SphereCache& cache, const Sphere& sphere, Model& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
+
+		// 
+							bool			Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree);
+		protected:
+		// Sphere in model space
+							Point			mCenter;			//!< Sphere center
+							float			mRadius2;			//!< Sphere radius squared
+		// Internal methods
+							void			_Collide(const AABBCollisionNode* node);
+							void			_Collide(const AABBNoLeafNode* node);
+							void			_Collide(const AABBQuantizedNode* node);
+							void			_Collide(const AABBQuantizedNoLeafNode* node);
+							void			_Collide(const AABBTreeNode* node);
+							void			_CollideNoPrimitiveTest(const AABBCollisionNode* node);
+							void			_CollideNoPrimitiveTest(const AABBNoLeafNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNode* node);
+							void			_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
+			// Overlap tests
+		inline_				BOOL			SphereContainsBox(const Point& bc, const Point& be);
+		inline_				BOOL			SphereAABBOverlap(const Point& center, const Point& extents);
+							BOOL			SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2);
+			// Init methods
+							BOOL			InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
+	};
+
+	class OPCODE_API HybridSphereCollider : public SphereCollider
+	{
+		public:
+		// Constructor / Destructor
+											HybridSphereCollider();
+		virtual								~HybridSphereCollider();
+
+        //bool			Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
+        // ericf change
+							bool			Collide(SphereCache& cache, const Sphere& sphere, HybridModel& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
+		protected:
+							Container		mTouchedBoxes;
+	};
+
+#endif // __OPC_SPHERECOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_SphereTriOverlap.h

@@ -0,0 +1,187 @@
+
+// This is collision detection. If you do another distance test for collision *response*,
+// if might be useful to simply *skip* the test below completely, and report a collision.
+// - if sphere-triangle overlap, result is ok
+// - if they don't, we'll discard them during collision response with a similar test anyway
+// Overall this approach should run faster.
+
+// Original code by David Eberly in Magic.
+BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
+{
+	// Stats
+	mNbVolumePrimTests++;
+
+	// Early exit if one of the vertices is inside the sphere
+	Point kDiff = vert2 - mCenter;
+	float fC = kDiff.SquareMagnitude();
+	if(fC <= mRadius2)	return TRUE;
+
+	kDiff = vert1 - mCenter;
+	fC = kDiff.SquareMagnitude();
+	if(fC <= mRadius2)	return TRUE;
+
+	kDiff = vert0 - mCenter;
+	fC = kDiff.SquareMagnitude();
+	if(fC <= mRadius2)	return TRUE;
+
+	// Else do the full distance test
+	Point TriEdge0	= vert1 - vert0;
+	Point TriEdge1	= vert2 - vert0;
+
+//Point kDiff	= vert0 - mCenter;
+	float fA00	= TriEdge0.SquareMagnitude();
+	float fA01	= TriEdge0 | TriEdge1;
+	float fA11	= TriEdge1.SquareMagnitude();
+	float fB0	= kDiff | TriEdge0;
+	float fB1	= kDiff | TriEdge1;
+//float fC	= kDiff.SquareMagnitude();
+	float fDet	= fabsf(fA00*fA11 - fA01*fA01);
+	float u		= fA01*fB1-fA11*fB0;
+	float v		= fA01*fB0-fA00*fB1;
+	float SqrDist;
+
+	if(u + v <= fDet)
+	{
+		if(u < 0.0f)
+		{
+			if(v < 0.0f)  // region 4
+			{
+				if(fB0 < 0.0f)
+				{
+//					v = 0.0f;
+					if(-fB0>=fA00)			{ /*u = 1.0f;*/		SqrDist = fA00+2.0f*fB0+fC;	}
+					else					{ u = -fB0/fA00;	SqrDist = fB0*u+fC;			}
+				}
+				else
+				{
+//					u = 0.0f;
+					if(fB1>=0.0f)			{ /*v = 0.0f;*/		SqrDist = fC;				}
+					else if(-fB1>=fA11)		{ /*v = 1.0f;*/		SqrDist = fA11+2.0f*fB1+fC;	}
+					else					{ v = -fB1/fA11;	SqrDist = fB1*v+fC;			}
+				}
+			}
+			else  // region 3
+			{
+//				u = 0.0f;
+				if(fB1>=0.0f)				{ /*v = 0.0f;*/		SqrDist = fC;				}
+				else if(-fB1>=fA11)			{ /*v = 1.0f;*/		SqrDist = fA11+2.0f*fB1+fC;	}
+				else						{ v = -fB1/fA11;	SqrDist = fB1*v+fC;			}
+			}
+		}
+		else if(v < 0.0f)  // region 5
+		{
+//			v = 0.0f;
+			if(fB0>=0.0f)					{ /*u = 0.0f;*/		SqrDist = fC;				}
+			else if(-fB0>=fA00)				{ /*u = 1.0f;*/		SqrDist = fA00+2.0f*fB0+fC;	}
+			else							{ u = -fB0/fA00;	SqrDist = fB0*u+fC;			}
+		}
+		else  // region 0
+		{
+			// minimum at interior point
+			if(fDet==0.0f)
+			{
+//				u = 0.0f;
+//				v = 0.0f;
+				SqrDist = MAX_FLOAT;
+			}
+			else
+			{
+				float fInvDet = 1.0f/fDet;
+				u *= fInvDet;
+				v *= fInvDet;
+				SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
+			}
+		}
+	}
+	else
+	{
+		float fTmp0, fTmp1, fNumer, fDenom;
+
+		if(u < 0.0f)  // region 2
+		{
+			fTmp0 = fA01 + fB0;
+			fTmp1 = fA11 + fB1;
+			if(fTmp1 > fTmp0)
+			{
+				fNumer = fTmp1 - fTmp0;
+				fDenom = fA00-2.0f*fA01+fA11;
+				if(fNumer >= fDenom)
+				{
+//					u = 1.0f;
+//					v = 0.0f;
+					SqrDist = fA00+2.0f*fB0+fC;
+				}
+				else
+				{
+					u = fNumer/fDenom;
+					v = 1.0f - u;
+					SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
+				}
+			}
+			else
+			{
+//				u = 0.0f;
+				if(fTmp1 <= 0.0f)		{ /*v = 1.0f;*/		SqrDist = fA11+2.0f*fB1+fC;	}
+				else if(fB1 >= 0.0f)	{ /*v = 0.0f;*/		SqrDist = fC;				}
+				else					{ v = -fB1/fA11;	SqrDist = fB1*v+fC;			}
+			}
+		}
+		else if(v < 0.0f)  // region 6
+		{
+			fTmp0 = fA01 + fB1;
+			fTmp1 = fA00 + fB0;
+			if(fTmp1 > fTmp0)
+			{
+				fNumer = fTmp1 - fTmp0;
+				fDenom = fA00-2.0f*fA01+fA11;
+				if(fNumer >= fDenom)
+				{
+//					v = 1.0f;
+//					u = 0.0f;
+					SqrDist = fA11+2.0f*fB1+fC;
+				}
+				else
+				{
+					v = fNumer/fDenom;
+					u = 1.0f - v;
+					SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
+				}
+			}
+			else
+			{
+//				v = 0.0f;
+				if(fTmp1 <= 0.0f)		{ /*u = 1.0f;*/		SqrDist = fA00+2.0f*fB0+fC;	}
+				else if(fB0 >= 0.0f)	{ /*u = 0.0f;*/		SqrDist = fC;				}
+				else					{ u = -fB0/fA00;	SqrDist = fB0*u+fC;			}
+			}
+		}
+		else  // region 1
+		{
+			fNumer = fA11 + fB1 - fA01 - fB0;
+			if(fNumer <= 0.0f)
+			{
+//				u = 0.0f;
+//				v = 1.0f;
+				SqrDist = fA11+2.0f*fB1+fC;
+			}
+			else
+			{
+				fDenom = fA00-2.0f*fA01+fA11;
+				if(fNumer >= fDenom)
+				{
+//					u = 1.0f;
+//					v = 0.0f;
+					SqrDist = fA00+2.0f*fB0+fC;
+				}
+				else
+				{
+					u = fNumer/fDenom;
+					v = 1.0f - u;
+					SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
+				}
+			}
+		}
+	}
+
+	return fabsf(SqrDist) < mRadius2;
+}

src/external/open_dynamics_engine-ef/ode/OPC_Stdafx.cpp

@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+
+//#define ICE_MAIN
+#include "OPC_Stdafx.h"

src/external/open_dynamics_engine-ef/ode/OPC_Stdafx.h

@@ -0,0 +1,24 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#if !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_)
+#define AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif // _MSC_VER > 1000
+
+// Insert your headers here
+#define WIN32_LEAN_AND_MEAN		// Exclude rarely-used stuff from Windows headers
+
+#include "ode/ode_Opcode.h"
+
+//{{AFX_INSERT_LOCATION}}
+// Microsoft Visual C++ will insert additional declarations immediately before the previous line.
+
+#endif // !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_)

src/external/open_dynamics_engine-ef/ode/OPC_SweepAndPrune.cpp

@@ -0,0 +1,672 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide)
+ *	\file		OPC_SweepAndPrune.cpp
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+inline_ void Sort(udword& id0, udword& id1)
+{
+	if(id0>id1)	Swap(id0, id1);
+}
+
+	class Opcode::SAP_Element
+	{
+		public:
+		inline_					SAP_Element()														{}
+		inline_					SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next)	{}
+		inline_					~SAP_Element()														{}
+
+				udword			mID;
+				SAP_Element*	mNext;
+	};
+
+	class Opcode::SAP_Box
+	{
+		public:
+				SAP_EndPoint*	Min[3];
+				SAP_EndPoint*	Max[3];
+	};
+
+	class Opcode::SAP_EndPoint
+	{
+		public:
+				float			Value;		// Min or Max value
+				SAP_EndPoint*	Previous;	// Previous EndPoint whose Value is smaller than ours (or null)
+				SAP_EndPoint*	Next;		// Next EndPoint whose Value is greater than ours (or null)
+				udword			Data;		// Parent box ID *2 | MinMax flag
+
+		inline_	void			SetData(udword box_id, BOOL is_max)			{ Data = (box_id<<1)|is_max;	}
+		inline_	BOOL			IsMax()								const	{ return Data & 1;				}
+		inline_	udword			GetBoxID()							const	{ return Data>>1;				}
+
+		inline_	void InsertAfter(SAP_EndPoint* element)
+		{
+			if(this!=element && this!=element->Next)
+			{
+				// Remove
+				if(Previous)	Previous->Next = Next;
+				if(Next)		Next->Previous = Previous;
+
+				// Insert
+				Next = element->Next;
+				if(Next)	Next->Previous = this;
+
+				element->Next = this;
+				Previous = element;
+			}
+		}
+
+		inline_	void InsertBefore(SAP_EndPoint* element)
+		{
+			if(this!=element && this!=element->Previous)
+			{
+				// Remove
+				if(Previous)	Previous->Next = Next;
+				if(Next)		Next->Previous = Previous;
+
+				// Insert
+				Previous = element->Previous;
+				element->Previous = this;
+
+				Next = element;
+				if(Previous)	Previous->Next = this;
+			}
+		}
+	};
+
+
+
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SAP_PairData::SAP_PairData() :
+	mNbElements		(0),
+	mNbUsedElements	(0),
+	mElementPool	(null),
+	mFirstFree		(null),
+	mNbObjects		(0),
+	mArray			(null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SAP_PairData::~SAP_PairData()
+{
+	Release();
+}
+
+void SAP_PairData::Release()
+{
+	mNbElements		= 0;
+	mNbUsedElements	= 0;
+	mNbObjects		= 0;
+	DELETEARRAY(mElementPool);
+	DELETEARRAY(mArray);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes.
+ *	\param		nb_objects	[in]
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool SAP_PairData::Init(udword nb_objects)
+{
+	// Make sure everything has been released
+	Release();
+	if(!nb_objects)	return false;
+
+	mArray = new SAP_Element*[nb_objects];
+	CHECKALLOC(mArray);
+	ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*));
+	mNbObjects = nb_objects;
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Remaps a pointer when pool gets resized.
+ *	\param		element	[in/out] remapped element
+ *	\param		delta	[in] offset in bytes
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ void Remap(SAP_Element*& element, size_t delta)
+{
+	if(element)	element = (SAP_Element*)(element + delta);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Gets a free element in the pool.
+ *	\param		id		[in] element id
+ *	\param		next	[in] next element
+ *	\param		remap	[out] possible remapping offset
+ *	\return		the new element
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap)
+{
+	if(remap)	*remap = 0;
+
+	SAP_Element* FreeElem;
+	if(mFirstFree)
+	{
+		// Recycle
+		FreeElem = mFirstFree;
+		mFirstFree = mFirstFree->mNext;	// First free = next free (or null)
+	}
+	else
+	{
+		if(mNbUsedElements==mNbElements)
+		{
+			// Resize
+			mNbElements = mNbElements ? (mNbElements<<1) : 2;
+
+			SAP_Element* NewElems = new SAP_Element[mNbElements];
+
+			if(mNbUsedElements)	CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element));
+
+			// Remap everything
+			{
+				size_t Delta = size_t(NewElems) - size_t(mElementPool);
+
+				for(udword i=0;i<mNbUsedElements;i++)	Remap(NewElems[i].mNext, Delta);
+				for(udword i=0;i<mNbObjects;i++)		Remap(mArray[i], Delta);
+
+				Remap(mFirstFree, Delta);
+				Remap(next, Delta);
+
+				if(remap)	*remap = Delta;
+			}
+
+			DELETEARRAY(mElementPool);
+			mElementPool = NewElems;
+		}
+
+		FreeElem = &mElementPool[mNbUsedElements++];
+	}
+
+	FreeElem->mID = id;
+	FreeElem->mNext = next;
+
+	return FreeElem;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Frees an element of the pool.
+ *	\param		elem	[in] element to free/recycle
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ void SAP_PairData::FreeElem(SAP_Element* elem)
+{
+	elem->mNext = mFirstFree;	// Next free
+	mFirstFree = elem;
+}
+
+// Add a pair to the set.
+void SAP_PairData::AddPair(udword id1, udword id2)
+{
+	// Order the ids
+	Sort(id1, id2);
+
+	ASSERT(id1<mNbObjects);
+	if(id1>=mNbObjects)	return;
+
+	// Select the right list from "mArray".
+	SAP_Element* Current = mArray[id1];
+
+	if(!Current)
+	{
+		// Empty slot => create new element
+		mArray[id1] = GetFreeElem(id2, null);
+	}
+	else if(Current->mID>id2)
+	{
+		// The list is not empty but all elements are greater than id2 => insert id2 in the front.
+		mArray[id1]	= GetFreeElem(id2, mArray[id1]);
+	}
+	else
+	{
+		// Else find the correct location in the sorted list (ascending order) and insert id2 there.
+		while(Current->mNext)
+		{
+			if(Current->mNext->mID > id2)	break;
+
+			Current = Current->mNext;
+		}
+
+		if(Current->mID==id2)	return;	// The pair already exists
+
+//		Current->mNext = GetFreeElem(id2, Current->mNext);
+		udword Delta;
+		SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta);
+		if(Delta)	Remap(Current, Delta);
+		Current->mNext = E;
+	}
+}
+
+// Delete a pair from the set.
+void SAP_PairData::RemovePair(udword id1, udword id2)
+{
+	// Order the ids.
+	Sort(id1, id2);
+
+	// Exit if the pair doesn't exist in the set
+	if(id1>=mNbObjects)	return;
+
+	// Otherwise, select the correct list.
+	SAP_Element* Current = mArray[id1];
+
+	// If this list is empty, the pair doesn't exist.
+	if(!Current) return;
+
+	// Otherwise, if id2 is the first element, delete it.
+	if(Current->mID==id2)
+	{
+		mArray[id1] = Current->mNext;
+		FreeElem(Current);
+	}
+	else
+	{
+		// If id2 is not the first element, start traversing the sorted list.
+		while(Current->mNext)
+		{
+			// If we have moved too far away without hitting id2, then the pair doesn't exist
+			if(Current->mNext->mID > id2)	return;
+
+			// Otherwise, delete id2.
+			if(Current->mNext->mID == id2)
+			{
+				SAP_Element* Temp = Current->mNext;
+				Current->mNext = Temp->mNext;
+				FreeElem(Temp);
+				return;
+			}
+			Current = Current->mNext;
+		}
+	}
+}
+
+void SAP_PairData::DumpPairs(Pairs& pairs) const
+{
+	// ### Ugly and slow
+	for(udword i=0;i<mNbObjects;i++)
+	{
+		SAP_Element* Current = mArray[i];
+		while(Current)
+		{
+			ASSERT(Current->mID<mNbObjects);
+
+			pairs.AddPair(i, Current->mID);
+			Current = Current->mNext;
+		}
+	}
+}
+
+void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const
+{
+	if(!callback)	return;
+
+	// ### Ugly and slow
+	for(udword i=0;i<mNbObjects;i++)
+	{
+		SAP_Element* Current = mArray[i];
+		while(Current)
+		{
+			ASSERT(Current->mID<mNbObjects);
+
+			if(!(callback)(i, Current->mID, user_data))	return;
+			Current = Current->mNext;
+		}
+	}
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SweepAndPrune::SweepAndPrune()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+SweepAndPrune::~SweepAndPrune()
+{
+}
+
+void SweepAndPrune::GetPairs(Pairs& pairs) const
+{
+	mPairs.DumpPairs(pairs);
+}
+
+void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const
+{
+	mPairs.DumpPairs(callback, user_data);
+}
+
+bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes)
+{
+	// 1) Create sorted lists
+	mNbObjects = nb_objects;
+
+	mBoxes = new SAP_Box[nb_objects];
+//	for(udword i=0;i<nb_objects;i++)	mBoxes[i].Box = *boxes[i];
+
+	float* Data = new float[nb_objects*2];
+
+	for(udword Axis=0;Axis<3;Axis++)
+	{
+		mList[Axis] = new SAP_EndPoint[nb_objects*2];
+
+		for(udword i=0;i<nb_objects;i++)
+		{
+			Data[i*2+0] = boxes[i]->GetMin(Axis);
+			Data[i*2+1] = boxes[i]->GetMax(Axis);
+		}
+		RadixSort RS;
+		const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks();
+
+		SAP_EndPoint* PreviousEndPoint = null;
+
+		for(udword i=0;i<nb_objects*2;i++)
+		{
+			udword SortedIndex	= *Sorted++;
+			float SortedCoord	= Data[SortedIndex];
+			udword BoxIndex		= SortedIndex>>1;
+
+			ASSERT(BoxIndex<nb_objects);
+
+			SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex];
+			CurrentEndPoint->Value		= SortedCoord;
+//			CurrentEndPoint->IsMax		= SortedIndex&1;		// ### could be implicit ?
+//			CurrentEndPoint->ID			= BoxIndex;				// ### could be implicit ?
+			CurrentEndPoint->SetData(BoxIndex, SortedIndex&1);	// ### could be implicit ?
+			CurrentEndPoint->Previous	= PreviousEndPoint;
+			CurrentEndPoint->Next		= null;
+			if(PreviousEndPoint)	PreviousEndPoint->Next = CurrentEndPoint;
+
+			if(CurrentEndPoint->IsMax())	mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint;
+			else							mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint;
+
+			PreviousEndPoint = CurrentEndPoint;
+		}
+	}
+
+	DELETEARRAY(Data);
+
+	CheckListsIntegrity();
+
+	// 2) Quickly find starting pairs
+
+	mPairs.Init(nb_objects);
+
+	{
+		Pairs P;
+		CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY));
+		for(udword i=0;i<P.GetNbPairs();i++)
+		{
+			const Pair* PP = P.GetPair(i);
+
+			udword id0 = PP->id0;
+			udword id1 = PP->id1;
+
+			if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1]))
+			{
+				mPairs.AddPair(id0, id1);
+			}
+			else ASSERT(0);
+		}
+	}
+
+	return true;
+}
+
+bool SweepAndPrune::CheckListsIntegrity()
+{
+	for(udword Axis=0;Axis<3;Axis++)
+	{
+		// Find list head
+		SAP_EndPoint* Current = mList[Axis];
+		while(Current->Previous)	Current = Current->Previous;
+
+		udword Nb = 0;
+
+		SAP_EndPoint* Previous = null;
+		while(Current)
+		{
+			Nb++;
+
+			if(Previous)
+			{
+				ASSERT(Previous->Value <= Current->Value);
+				if(Previous->Value > Current->Value)	return false;
+			}
+
+			ASSERT(Current->Previous==Previous);
+			if(Current->Previous!=Previous)	return false;
+
+			Previous = Current;
+			Current = Current->Next;
+		}
+
+		ASSERT(Nb==mNbObjects*2);
+	}
+	return true;
+}
+
+inline_ BOOL Intersect(const AABB& a, const SAP_Box& b)
+{
+	if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value
+	|| b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value
+	|| b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value)	return FALSE;
+
+	return TRUE;
+}
+
+
+
+bool SweepAndPrune::UpdateObject(udword i, const AABB& box)
+{
+	for(udword Axis=0;Axis<3;Axis++)
+	{
+//		udword Base = (udword)&mList[Axis][0];
+
+		// Update min
+		{
+			SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis];
+			ASSERT(!CurrentMin->IsMax());
+
+			const float Limit = box.GetMin(Axis);
+			if(Limit == CurrentMin->Value)
+			{
+			}
+			else if(Limit < CurrentMin->Value)
+			{
+				CurrentMin->Value = Limit;
+
+				// Min is moving left:
+				SAP_EndPoint* NewPos = CurrentMin;
+				ASSERT(NewPos);
+
+				SAP_EndPoint* tmp;
+				while((tmp = NewPos->Previous) && tmp->Value > Limit)
+				{
+					NewPos = tmp;
+
+					if(NewPos->IsMax())
+					{
+						// Our min passed a max => start overlap
+						//udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint);
+						const udword id0 = CurrentMin->GetBoxID();
+						const udword id1 = NewPos->GetBoxID();
+
+						if(id0!=id1 && Intersect(box, mBoxes[id1]))	mPairs.AddPair(id0, id1);
+					}
+				}
+
+				CurrentMin->InsertBefore(NewPos);
+			}
+			else// if(Limit > CurrentMin->Value)
+			{
+				CurrentMin->Value = Limit;
+
+				// Min is moving right:
+				SAP_EndPoint* NewPos = CurrentMin;
+				ASSERT(NewPos);
+
+				SAP_EndPoint* tmp;
+				while((tmp = NewPos->Next) && tmp->Value < Limit)
+				{
+					NewPos = tmp;
+
+					if(NewPos->IsMax())
+					{
+						// Our min passed a max => stop overlap
+						const udword id0 = CurrentMin->GetBoxID();
+						const udword id1 = NewPos->GetBoxID();
+
+						if(id0!=id1)	mPairs.RemovePair(id0, id1);
+					}
+				}
+
+				CurrentMin->InsertAfter(NewPos);
+			}
+		}
+
+		// Update max
+		{
+			SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis];
+			ASSERT(CurrentMax->IsMax());
+
+			const float Limit = box.GetMax(Axis);
+			if(Limit == CurrentMax->Value)
+			{
+			}
+			else if(Limit > CurrentMax->Value)
+			{
+				CurrentMax->Value = Limit;
+
+				// Max is moving right:
+				SAP_EndPoint* NewPos = CurrentMax;
+				ASSERT(NewPos);
+
+				SAP_EndPoint* tmp;
+				while((tmp = NewPos->Next) && tmp->Value < Limit)
+				{
+					NewPos = tmp;
+
+					if(!NewPos->IsMax())
+					{
+						// Our max passed a min => start overlap
+						const udword id0 = CurrentMax->GetBoxID();
+						const udword id1 = NewPos->GetBoxID();
+
+						if(id0!=id1 && Intersect(box, mBoxes[id1]))	mPairs.AddPair(id0, id1);
+					}
+				}
+
+				CurrentMax->InsertAfter(NewPos);
+			}
+			else// if(Limit < CurrentMax->Value)
+			{
+				CurrentMax->Value = Limit;
+
+				// Max is moving left:
+				SAP_EndPoint* NewPos = CurrentMax;
+				ASSERT(NewPos);
+
+				SAP_EndPoint* tmp;
+				while((tmp = NewPos->Previous) && tmp->Value > Limit)
+				{
+					NewPos = tmp;
+
+					if(!NewPos->IsMax())
+					{
+						// Our max passed a min => stop overlap
+						const udword id0 = CurrentMax->GetBoxID();
+						const udword id1 = NewPos->GetBoxID();
+
+						if(id0!=id1)	mPairs.RemovePair(id0, id1);
+					}
+				}
+
+				CurrentMax->InsertBefore(NewPos);
+			}
+		}
+	}
+
+	return true;
+}

src/external/open_dynamics_engine-ef/ode/OPC_SweepAndPrune.h

@@ -0,0 +1,86 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide)
+ *	\file		OPC_SweepAndPrune.h
+ *	\author		Pierre Terdiman
+ *	\date		January, 29, 2000
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_SWEEPANDPRUNE_H__
+#define __OPC_SWEEPANDPRUNE_H__
+
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	/**
+	 *	User-callback, called by OPCODE for each colliding pairs.
+	 *	\param		id0			[in] id of colliding object
+	 *	\param		id1			[in] id of colliding object
+	 *	\param		user_data	[in] user-defined data
+	 *	\return		TRUE to continue enumeration
+	 */
+	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+	typedef BOOL	(*PairCallback)	(udword id0, udword id1, void* user_data);
+
+	class SAP_Element;
+	class SAP_EndPoint;
+	class SAP_Box;
+
+	class OPCODE_API SAP_PairData
+	{
+		public:
+								SAP_PairData();
+								~SAP_PairData();
+
+				bool			Init(udword nb_objects);
+
+				void			AddPair(udword id1, udword id2);
+				void			RemovePair(udword id1, udword id2);
+
+				void			DumpPairs(Pairs& pairs)								const;
+				void			DumpPairs(PairCallback callback, void* user_data)	const;
+		private:
+				udword			mNbElements;		//!< Total number of elements in the pool
+				udword			mNbUsedElements;	//!< Number of used elements
+				SAP_Element*	mElementPool;		//!< Array of mNbElements elements
+				SAP_Element*	mFirstFree;			//!< First free element in the pool
+
+				udword			mNbObjects;			//!< Max number of objects we can handle
+				SAP_Element**	mArray;				//!< Pointers to pool
+		// Internal methods
+				SAP_Element*	GetFreeElem(udword id, SAP_Element* next, udword* remap=null);
+		inline_	void			FreeElem(SAP_Element* elem);
+				void			Release();
+	};
+
+	class OPCODE_API SweepAndPrune
+	{
+		public:
+								SweepAndPrune();
+								~SweepAndPrune();
+
+				bool			Init(udword nb_objects, const AABB** boxes);
+				bool			UpdateObject(udword i, const AABB& box);
+
+				void			GetPairs(Pairs& pairs)								const;
+				void			GetPairs(PairCallback callback, void* user_data)	const;
+		private:
+				SAP_PairData	mPairs;
+
+				udword			mNbObjects;
+				SAP_Box*		mBoxes;
+				SAP_EndPoint*	mList[3];
+		// Internal methods
+				bool			CheckListsIntegrity();
+	};
+
+#endif //__OPC_SWEEPANDPRUNE_H__

src/external/open_dynamics_engine-ef/ode/OPC_TreeBuilders.cpp

@@ -0,0 +1,269 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for tree builders.
+ *	\file		OPC_TreeBuilders.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A builder for AABB-trees of vertices.
+ *
+ *	\class		AABBTreeOfVerticesBuilder
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A builder for AABB-trees of AABBs.
+ *
+ *	\class		AABBTreeOfAABBsBuilder
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	A builder for AABB-trees of triangles.
+ *
+ *	\class		AABBTreeOfTrianglesBuilder
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the AABB of a set of primitives.
+ *	\param		primitives		[in] list of indices of primitives
+ *	\param		nb_prims		[in] number of indices
+ *	\param		global_box		[out] global AABB enclosing the set of input primitives
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBTreeOfAABBsBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const
+// ericf change
+bool AABBTreeOfAABBsBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)
+{
+	// Checkings
+	if(!primitives || !nb_prims)	return false;
+
+	// Initialize global box
+	global_box = mAABBArray[primitives[0]];
+
+	// Loop through boxes
+	for(udword i=1;i<nb_prims;i++)
+	{
+		// Update global box
+		global_box.Add(mAABBArray[primitives[i]]);
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the splitting value along a given axis for a given primitive.
+ *	\param		index		[in] index of the primitive to split
+ *	\param		axis		[in] axis index (0,1,2)
+ *	\return		splitting value
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float AABBTreeOfAABBsBuilder::GetSplittingValue(udword index, udword axis) const
+{
+	// For an AABB, the splitting value is the middle of the given axis,
+	// i.e. the corresponding component of the center point
+	return mAABBArray[index].GetCenter(axis);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the AABB of a set of primitives.
+ *	\param		primitives		[in] list of indices of primitives
+ *	\param		nb_prims		[in] number of indices
+ *	\param		global_box		[out] global AABB enclosing the set of input primitives
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBTreeOfTrianglesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const
+// ericf change
+bool AABBTreeOfTrianglesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)
+{
+	// Checkings
+	if(!primitives || !nb_prims)	return false;
+
+	// Initialize global box
+	Point Min(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT);
+	Point Max(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
+
+	// Loop through triangles
+	VertexPointers VP;
+	while(nb_prims--)
+	{
+		// Get current triangle-vertices
+		mIMesh->GetTriangle(VP, *primitives++);
+		// Update global box
+		Min.Min(*VP.Vertex[0]).Min(*VP.Vertex[1]).Min(*VP.Vertex[2]);
+		Max.Max(*VP.Vertex[0]).Max(*VP.Vertex[1]).Max(*VP.Vertex[2]);
+	}
+	global_box.SetMinMax(Min, Max);
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the splitting value along a given axis for a given primitive.
+ *	\param		index		[in] index of the primitive to split
+ *	\param		axis		[in] axis index (0,1,2)
+ *	\return		splitting value
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float AABBTreeOfTrianglesBuilder::GetSplittingValue(udword index, udword axis) const
+{
+/*	// Compute center of triangle
+	Point Center;
+	mTriList[index].Center(mVerts, Center);
+	// Return value
+	return Center[axis];*/
+
+	// Compute correct component from center of triangle
+//	return	(mVerts[mTriList[index].mVRef[0]][axis]
+//			+mVerts[mTriList[index].mVRef[1]][axis]
+//			+mVerts[mTriList[index].mVRef[2]][axis])*INV3;
+
+	VertexPointers VP;
+	mIMesh->GetTriangle(VP, index);
+
+	// Compute correct component from center of triangle
+	return	((*VP.Vertex[0])[axis]
+			+(*VP.Vertex[1])[axis]
+			+(*VP.Vertex[2])[axis])*INV3;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the splitting value along a given axis for a given node.
+ *	\param		primitives		[in] list of indices of primitives
+ *	\param		nb_prims		[in] number of indices
+ *	\param		global_box		[in] global AABB enclosing the set of input primitives
+ *	\param		axis			[in] axis index (0,1,2)
+ *	\return		splitting value
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float AABBTreeOfTrianglesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis)	const
+{
+	if(mSettings.mRules&SPLIT_GEOM_CENTER)
+	{
+		// Loop through triangles
+		float SplitValue = 0.0f;
+		VertexPointers VP;
+		for(udword i=0;i<nb_prims;i++)
+		{
+			// Get current triangle-vertices
+			mIMesh->GetTriangle(VP, primitives[i]);
+			// Update split value
+			SplitValue += (*VP.Vertex[0])[axis];
+			SplitValue += (*VP.Vertex[1])[axis];
+			SplitValue += (*VP.Vertex[2])[axis];
+		}
+		return SplitValue / float(nb_prims*3);
+	}
+	else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the AABB of a set of primitives.
+ *	\param		primitives		[in] list of indices of primitives
+ *	\param		nb_prims		[in] number of indices
+ *	\param		global_box		[out] global AABB enclosing the set of input primitives
+ *	\return		true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//bool AABBTreeOfVerticesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const
+// ericf change
+bool AABBTreeOfVerticesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)
+{
+	// Checkings
+	if(!primitives || !nb_prims)	return false;
+
+	// Initialize global box
+	global_box.SetEmpty();
+
+	// Loop through vertices
+	for(udword i=0;i<nb_prims;i++)
+	{
+		// Update global box
+		global_box.Extend(mVertexArray[primitives[i]]);
+	}
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the splitting value along a given axis for a given primitive.
+ *	\param		index		[in] index of the primitive to split
+ *	\param		axis		[in] axis index (0,1,2)
+ *	\return		splitting value
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float AABBTreeOfVerticesBuilder::GetSplittingValue(udword index, udword axis) const
+{
+	// For a vertex, the splitting value is simply the vertex coordinate.
+	return mVertexArray[index][axis];
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Computes the splitting value along a given axis for a given node.
+ *	\param		primitives		[in] list of indices of primitives
+ *	\param		nb_prims		[in] number of indices
+ *	\param		global_box		[in] global AABB enclosing the set of input primitives
+ *	\param		axis			[in] axis index (0,1,2)
+ *	\return		splitting value
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float AABBTreeOfVerticesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis)	const
+{
+	if(mSettings.mRules&SPLIT_GEOM_CENTER)
+	{
+		// Loop through vertices
+		float SplitValue = 0.0f;
+		for(udword i=0;i<nb_prims;i++)
+		{
+			// Update split value
+			SplitValue += mVertexArray[primitives[i]][axis];
+		}
+		return SplitValue / float(nb_prims);
+	}
+	else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis);
+}

src/external/open_dynamics_engine-ef/ode/OPC_TreeBuilders.h

@@ -0,0 +1,183 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for tree builders.
+ *	\file		OPC_TreeBuilders.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_TREEBUILDERS_H__
+#define __OPC_TREEBUILDERS_H__
+
+	//! Tree splitting rules
+	enum SplittingRules
+	{
+		// Primitive split
+		SPLIT_LARGEST_AXIS		= (1<<0),		//!< Split along the largest axis
+		SPLIT_SPLATTER_POINTS	= (1<<1),		//!< Splatter primitive centers (QuickCD-style)
+		SPLIT_BEST_AXIS			= (1<<2),		//!< Try largest axis, then second, then last
+		SPLIT_BALANCED			= (1<<3),		//!< Try to keep a well-balanced tree
+		SPLIT_FIFTY				= (1<<4),		//!< Arbitrary 50-50 split
+		// Node split
+		SPLIT_GEOM_CENTER		= (1<<5),		//!< Split at geometric center (else split in the middle)
+		//
+		SPLIT_FORCE_DWORD		= 0x7fffffff
+	};
+
+	//! Simple wrapper around build-related settings [Opcode 1.3]
+	struct OPCODE_API BuildSettings
+	{
+		inline_	BuildSettings() : mLimit(1), mRules(SPLIT_FORCE_DWORD)	{}
+
+		udword	mLimit;		//!< Limit number of primitives / node. If limit is 1, build a complete tree (2*N-1 nodes)
+		udword	mRules;		//!< Building/Splitting rules (a combination of SplittingRules flags)
+	};
+
+	class OPCODE_API AABBTreeBuilder
+	{
+		public:
+		//! Constructor
+													AABBTreeBuilder() :
+														mNbPrimitives(0),
+														mNodeBase(null),
+														mCount(0),
+														mNbInvalidSplits(0)		{}
+		//! Destructor
+		virtual										~AABBTreeBuilder()			{}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the AABB of a set of primitives.
+		 *	\param		primitives		[in] list of indices of primitives
+		 *	\param		nb_prims		[in] number of indices
+		 *	\param		global_box		[out] global AABB enclosing the set of input primitives
+		 *	\return		true if success
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		//virtual						bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)	const	= 0;
+        // ericf change
+		virtual						bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)	= 0;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the splitting value along a given axis for a given primitive.
+		 *	\param		index			[in] index of the primitive to split
+		 *	\param		axis			[in] axis index (0,1,2)
+		 *	\return		splitting value
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual						float			GetSplittingValue(udword index, udword axis)	const	= 0;
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Computes the splitting value along a given axis for a given node.
+		 *	\param		primitives		[in] list of indices of primitives
+		 *	\param		nb_prims		[in] number of indices
+		 *	\param		global_box		[in] global AABB enclosing the set of input primitives
+		 *	\param		axis			[in] axis index (0,1,2)
+		 *	\return		splitting value
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual						float			GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis)	const
+													{
+														// Default split value = middle of the axis (using only the box)
+														return global_box.GetCenter(axis);
+													}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates node subdivision. This is called each time a node is considered for subdivision, during tree building.
+		 *	\param		primitives		[in] list of indices of primitives
+		 *	\param		nb_prims		[in] number of indices
+		 *	\param		global_box		[in] global AABB enclosing the set of input primitives
+		 *	\return		TRUE if the node should be subdivised
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		virtual						BOOL			ValidateSubdivision(const udword* primitives, udword nb_prims, const AABB& global_box)
+													{
+														// Check the user-defined limit
+														if(nb_prims<=mSettings.mLimit)	return FALSE;
+
+														return TRUE;
+													}
+
+									BuildSettings	mSettings;			//!< Splitting rules & split limit [Opcode 1.3]
+									udword			mNbPrimitives;		//!< Total number of primitives.
+									void*			mNodeBase;			//!< Address of node pool [Opcode 1.3]
+		// Stats
+		inline_						void			SetCount(udword nb)				{ mCount=nb;				}
+		inline_						void			IncreaseCount(udword nb)		{ mCount+=nb;				}
+		inline_						udword			GetCount()				const	{ return mCount;			}
+		inline_						void			SetNbInvalidSplits(udword nb)	{ mNbInvalidSplits=nb;		}
+		inline_						void			IncreaseNbInvalidSplits()		{ mNbInvalidSplits++;		}
+		inline_						udword			GetNbInvalidSplits()	const	{ return mNbInvalidSplits;	}
+
+		private:
+									udword			mCount;				//!< Stats: number of nodes created
+									udword			mNbInvalidSplits;	//!< Stats: number of invalid splits
+	};
+
+	class OPCODE_API AABBTreeOfVerticesBuilder : public AABBTreeBuilder
+	{
+		public:
+		//! Constructor
+													AABBTreeOfVerticesBuilder() : mVertexArray(null)	{}
+		//! Destructor
+		virtual										~AABBTreeOfVerticesBuilder()						{}
+
+		//override(AABBTreeBuilder)	bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)	const;
+        // ericf change
+		override(AABBTreeBuilder)	bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box);
+		override(AABBTreeBuilder)	float			GetSplittingValue(udword index, udword axis)									const;
+		override(AABBTreeBuilder)	float			GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis)	const;
+
+		const						Point*			mVertexArray;		//!< Shortcut to an app-controlled array of vertices.
+	};
+
+	class OPCODE_API AABBTreeOfAABBsBuilder : public AABBTreeBuilder
+	{
+		public:
+		//! Constructor
+													AABBTreeOfAABBsBuilder() : mAABBArray(null)	{}
+		//! Destructor
+		virtual										~AABBTreeOfAABBsBuilder()					{}
+
+		//override(AABBTreeBuilder)	bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)	const;
+        // ericf change
+		override(AABBTreeBuilder)	bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box);
+		override(AABBTreeBuilder)	float			GetSplittingValue(udword index, udword axis)									const;
+
+		const						AABB*			mAABBArray;			//!< Shortcut to an app-controlled array of AABBs.
+	};
+
+	class OPCODE_API AABBTreeOfTrianglesBuilder : public AABBTreeBuilder
+	{
+		public:
+		//! Constructor
+													AABBTreeOfTrianglesBuilder() : mIMesh(null)										{}
+		//! Destructor
+		virtual										~AABBTreeOfTrianglesBuilder()													{}
+
+		//override(AABBTreeBuilder)	bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box)	const;
+        // ericf change
+		override(AABBTreeBuilder)	bool			ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box);
+		override(AABBTreeBuilder)	float			GetSplittingValue(udword index, udword axis)									const;
+		override(AABBTreeBuilder)	float			GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis)	const;
+
+		//const				MeshInterface*			mIMesh;			//!< Shortcut to an app-controlled mesh interface
+        // ericf change
+		MeshInterface*			mIMesh;			//!< Shortcut to an app-controlled mesh interface
+	};
+
+#endif // __OPC_TREEBUILDERS_H__

src/external/open_dynamics_engine-ef/ode/OPC_TreeCollider.cpp

@@ -0,0 +1,951 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a tree collider.
+ *	\file		OPC_TreeCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains an AABB tree collider.
+ *	This class performs a collision test between two AABB trees.
+ *
+ *	\class		AABBTreeCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		March, 20, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+#include "ode/OPC_BoxBoxOverlap.h"
+#include "ode/OPC_TriBoxOverlap.h"
+#include "ode/OPC_TriTriOverlap.h"
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBTreeCollider::AABBTreeCollider() :
+	mIMesh0				(null),
+	mIMesh1				(null),
+	mNbBVBVTests		(0),
+	mNbPrimPrimTests	(0),
+	mNbBVPrimTests		(0),
+	mFullBoxBoxTest		(true),
+	mFullPrimBoxTest	(true)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+AABBTreeCollider::~AABBTreeCollider()
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Validates current settings. You should call this method after all the settings and callbacks have been defined.
+ *	\return		null if everything is ok, else a string describing the problem
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const char* AABBTreeCollider::ValidateSettings()
+{
+	if(TemporalCoherenceEnabled() && !FirstContactEnabled())	return "Temporal coherence only works with ""First contact"" mode!";
+	return null;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Generic collision query for generic OPCODE models. After the call, access the results with:
+ *	- GetContactStatus()
+ *	- GetNbPairs()
+ *	- GetPairs()
+ *
+ *	\param		cache			[in] collision cache for model pointers and a colliding pair of primitives
+ *	\param		world0			[in] world matrix for first object
+ *	\param		world1			[in] world matrix for second object
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1)
+{
+	// Checkings
+	if(!cache.Model0 || !cache.Model1)								return false;
+	if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes())	return false;
+	if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized())	return false;
+
+	/*
+
+	  Rules:
+		- perform hull test
+		- when hulls collide, disable hull test
+		- if meshes overlap, reset countdown
+		- if countdown reaches 0, enable hull test
+
+	*/
+
+#ifdef __MESHMERIZER_H__
+	// Handle hulls
+	if(cache.HullTest)
+	{
+		if(cache.Model0->GetHull() && cache.Model1->GetHull())
+		{
+			struct Local
+			{
+				static Point* SVCallback(const Point& sv, udword& previndex, udword user_data)
+				{
+					CollisionHull* Hull = (CollisionHull*)user_data;
+					previndex = Hull->ComputeSupportingVertex(sv, previndex);
+					return (Point*)&Hull->GetVerts()[previndex];
+				}
+			};
+
+			bool Collide;
+
+			if(0)
+			{
+				static GJKEngine GJK;
+				static bool GJKInitDone=false;
+				if(!GJKInitDone)
+				{
+					GJK.Enable(GJK_BACKUP_PROCEDURE);
+					GJK.Enable(GJK_DEGENERATE);
+					GJK.Enable(GJK_HILLCLIMBING);
+					GJKInitDone = true;
+				}
+				GJK.SetCallbackObj0(Local::SVCallback);
+				GJK.SetCallbackObj1(Local::SVCallback);
+				GJK.SetUserData0(udword(cache.Model0->GetHull()));
+				GJK.SetUserData1(udword(cache.Model1->GetHull()));
+				Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
+			}
+			else
+			{
+				static SVEngine SVE;
+				SVE.SetCallbackObj0(Local::SVCallback);
+				SVE.SetCallbackObj1(Local::SVCallback);
+				SVE.SetUserData0(udword(cache.Model0->GetHull()));
+				SVE.SetUserData1(udword(cache.Model1->GetHull()));
+				Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
+			}
+
+			if(!Collide)
+			{
+		// Reset stats & contact status
+		mFlags &= ~OPC_CONTACT;
+		mNbBVBVTests		= 0;
+		mNbPrimPrimTests	= 0;
+		mNbBVPrimTests		= 0;
+		mPairs.Reset();
+		return true;
+			}
+		}
+	}
+
+	// Here, hulls collide
+	cache.HullTest = false;
+#endif // __MESHMERIZER_H__
+
+	// Checkings
+	if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface()))	return false;
+
+	// Simple double-dispatch
+	bool Status;
+	if(!cache.Model0->HasLeafNodes())
+	{
+		if(cache.Model0->IsQuantized())
+		{
+			const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
+			const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
+			Status = Collide(T0, T1, world0, world1, &cache);
+		}
+		else
+		{
+			const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
+			const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
+			Status = Collide(T0, T1, world0, world1, &cache);
+		}
+	}
+	else
+	{
+		if(cache.Model0->IsQuantized())
+		{
+			const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
+			const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
+			Status = Collide(T0, T1, world0, world1, &cache);
+		}
+		else
+		{
+			const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
+			const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
+			Status = Collide(T0, T1, world0, world1, &cache);
+		}
+	}
+
+#ifdef __MESHMERIZER_H__
+	if(Status)
+	{
+		// Reset counter as long as overlap occurs
+		if(GetContactStatus())	cache.ResetCountDown();
+
+		// Enable hull test again when counter reaches zero
+		cache.CountDown--;
+		if(!cache.CountDown)
+		{
+			cache.ResetCountDown();
+			cache.HullTest = true;
+		}
+	}
+#endif
+	return Status;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Initializes a collision query :
+ *	- reset stats & contact status
+ *	- setup matrices
+ *
+ *	\param		world0			[in] world matrix for first object
+ *	\param		world1			[in] world matrix for second object
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1)
+{
+	// Reset stats & contact status
+	Collider::InitQuery();
+	mNbBVBVTests		= 0;
+	mNbPrimPrimTests	= 0;
+	mNbBVPrimTests		= 0;
+	mPairs.Reset();
+
+	// Setup matrices
+	Matrix4x4 InvWorld0, InvWorld1;
+	if(world0)	InvertPRMatrix(InvWorld0, *world0);
+	else		InvWorld0.Identity();
+
+	if(world1)	InvertPRMatrix(InvWorld1, *world1);
+	else		InvWorld1.Identity();
+
+	Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
+	Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;
+
+	mR0to1 = World0to1;		World0to1.GetTrans(mT0to1);
+	mR1to0 = World1to0;		World1to0.GetTrans(mT1to0);
+
+	// Precompute absolute 1-to-0 rotation matrix
+	for(udword i=0;i<3;i++)
+	{
+		for(udword j=0;j<3;j++)
+		{
+			// Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
+			mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
+		}
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Takes advantage of temporal coherence.
+ *	\param		cache	[in] cache for a pair of previously colliding primitives
+ *	\return		true if we can return immediately
+ *	\warning	only works for "First Contact" mode
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
+{
+	// Checkings
+	if(!cache)	return false;
+
+	// Test previously colliding primitives first
+	if(TemporalCoherenceEnabled() && FirstContactEnabled())
+	{
+		PrimTest(cache->id0, cache->id1);
+		if(GetContactStatus())	return true;
+	}
+	return false;
+}
+
+#define UPDATE_CACHE						\
+	if(cache && GetContactStatus())			\
+	{										\
+		cache->id0 = mPairs.GetEntry(0);	\
+		cache->id1 = mPairs.GetEntry(1);	\
+	}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for normal AABB trees.
+ *	\param		tree0			[in] AABB tree from first object
+ *	\param		tree1			[in] AABB tree from second object
+ *	\param		world0			[in] world matrix for first object
+ *	\param		world1			[in] world matrix for second object
+ *	\param		cache			[in/out] cache for a pair of previously colliding primitives
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
+{
+	// Init collision query
+	InitQuery(world0, world1);
+
+	// Check previous state
+	if(CheckTemporalCoherence(cache))		return true;
+
+	// Perform collision query
+	_Collide(tree0->GetNodes(), tree1->GetNodes());
+
+	UPDATE_CACHE
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for no-leaf AABB trees.
+ *	\param		tree0			[in] AABB tree from first object
+ *	\param		tree1			[in] AABB tree from second object
+ *	\param		world0			[in] world matrix for first object
+ *	\param		world1			[in] world matrix for second object
+ *	\param		cache			[in/out] cache for a pair of previously colliding primitives
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
+{
+	// Init collision query
+	InitQuery(world0, world1);
+
+	// Check previous state
+	if(CheckTemporalCoherence(cache))		return true;
+
+	// Perform collision query
+	_Collide(tree0->GetNodes(), tree1->GetNodes());
+
+	UPDATE_CACHE
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for quantized AABB trees.
+ *	\param		tree0			[in] AABB tree from first object
+ *	\param		tree1			[in] AABB tree from second object
+ *	\param		world0			[in] world matrix for first object
+ *	\param		world1			[in] world matrix for second object
+ *	\param		cache			[in/out] cache for a pair of previously colliding primitives
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
+{
+	// Init collision query
+	InitQuery(world0, world1);
+
+	// Check previous state
+	if(CheckTemporalCoherence(cache))		return true;
+
+	// Setup dequantization coeffs
+	mCenterCoeff0	= tree0->mCenterCoeff;
+	mExtentsCoeff0	= tree0->mExtentsCoeff;
+	mCenterCoeff1	= tree1->mCenterCoeff;
+	mExtentsCoeff1	= tree1->mExtentsCoeff;
+
+	// Dequantize box A
+	const AABBQuantizedNode* N0 = tree0->GetNodes();
+	const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z);
+	const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z);
+	// Dequantize box B
+	const AABBQuantizedNode* N1 = tree1->GetNodes();
+	const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z);
+	const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z);
+
+	// Perform collision query
+	_Collide(N0, N1, a, Pa, b, Pb);
+
+	UPDATE_CACHE
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Collision query for quantized no-leaf AABB trees.
+ *	\param		tree0			[in] AABB tree from first object
+ *	\param		tree1			[in] AABB tree from second object
+ *	\param		world0			[in] world matrix for first object
+ *	\param		world1			[in] world matrix for second object
+ *	\param		cache			[in/out] cache for a pair of previously colliding primitives
+ *	\return		true if success
+ *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
+{
+	// Init collision query
+	InitQuery(world0, world1);
+
+	// Check previous state
+	if(CheckTemporalCoherence(cache))		return true;
+
+	// Setup dequantization coeffs
+	mCenterCoeff0	= tree0->mCenterCoeff;
+	mExtentsCoeff0	= tree0->mExtentsCoeff;
+	mCenterCoeff1	= tree1->mCenterCoeff;
+	mExtentsCoeff1	= tree1->mExtentsCoeff;
+
+	// Perform collision query
+	_Collide(tree0->GetNodes(), tree1->GetNodes());
+
+	UPDATE_CACHE
+
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Standard trees
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// The normal AABB tree can use 2 different descent rules (with different performances)
+//#define ORIGINAL_CODE			//!< UNC-like descent rules
+#define ALTERNATIVE_CODE		//!< Alternative descent rules
+
+#ifdef ORIGINAL_CODE
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		b0		[in] collision node from first tree
+ *	\param		b1		[in] collision node from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
+{
+	// Perform BV-BV overlap test
+	if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))	return;
+
+	if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
+
+	if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
+	{
+		_Collide(b0->GetNeg(), b1);
+		if(ContactFound()) return;
+		_Collide(b0->GetPos(), b1);
+	}
+	else
+	{
+		_Collide(b0, b1->GetNeg());
+		if(ContactFound()) return;
+		_Collide(b0, b1->GetPos());
+	}
+}
+#endif
+
+#ifdef ALTERNATIVE_CODE
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for normal AABB trees.
+ *	\param		b0		[in] collision node from first tree
+ *	\param		b1		[in] collision node from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
+{
+	// Perform BV-BV overlap test
+	if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))
+	{
+		return;
+	}
+
+	if(b0->IsLeaf())
+	{
+		if(b1->IsLeaf())
+		{
+			PrimTest(b0->GetPrimitive(), b1->GetPrimitive());
+		}
+		else
+		{
+			_Collide(b0, b1->GetNeg());
+			if(ContactFound()) return;
+			_Collide(b0, b1->GetPos());
+		}
+	}
+	else if(b1->IsLeaf())
+	{
+		_Collide(b0->GetNeg(), b1);
+		if(ContactFound()) return;
+		_Collide(b0->GetPos(), b1);
+	}
+	else
+	{
+		_Collide(b0->GetNeg(), b1->GetNeg());
+		if(ContactFound()) return;
+		_Collide(b0->GetNeg(), b1->GetPos());
+		if(ContactFound()) return;
+		_Collide(b0->GetPos(), b1->GetNeg());
+		if(ContactFound()) return;
+		_Collide(b0->GetPos(), b1->GetPos());
+	}
+}
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// No-leaf trees
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Leaf-leaf test for two primitive indices.
+ *	\param		id0		[in] index from first leaf-triangle
+ *	\param		id1		[in] index from second leaf-triangle
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::PrimTest(udword id0, udword id1)
+{
+	// Request vertices from the app
+	VertexPointers VP0;
+	VertexPointers VP1;
+	mIMesh0->GetTriangle(VP0, id0);
+	mIMesh1->GetTriangle(VP1, id1);
+
+	// Transform from space 1 to space 0
+	Point u0,u1,u2;
+	TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0);
+	TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0);
+	TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0);
+
+	// Perform triangle-triangle overlap test
+	if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2))
+	{
+		// Keep track of colliding pairs
+		mPairs.Add(id0).Add(id1);
+		// Set contact status
+		mFlags |= OPC_CONTACT;
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B.
+ *	\param		id1		[in] leaf-triangle index from tree B
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
+{
+	// Request vertices from the app
+	VertexPointers VP;
+	mIMesh1->GetTriangle(VP, id1);
+
+	// Perform triangle-triangle overlap test
+	if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
+	{
+		// Keep track of colliding pairs
+		mPairs.Add(mLeafIndex).Add(id1);
+		// Set contact status
+		mFlags |= OPC_CONTACT;
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A.
+ *	\param		id0		[in] leaf-triangle index from tree A
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
+{
+	// Request vertices from the app
+	VertexPointers VP;
+	mIMesh0->GetTriangle(VP, id0);
+
+	// Perform triangle-triangle overlap test
+	if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
+	{
+		// Keep track of colliding pairs
+		mPairs.Add(id0).Add(mLeafIndex);
+		// Set contact status
+		mFlags |= OPC_CONTACT;
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision of a leaf node from A and a branch from B.
+ *	\param		b		[in] collision node from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
+{
+	// Perform triangle-box overlap test
+	if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents))	return;
+
+	// Keep same triangle, deal with first child
+	if(b->HasPosLeaf())	PrimTestTriIndex(b->GetPosPrimitive());
+	else				_CollideTriBox(b->GetPos());
+
+	if(ContactFound()) return;
+
+	// Keep same triangle, deal with second child
+	if(b->HasNegLeaf())	PrimTestTriIndex(b->GetNegPrimitive());
+	else				_CollideTriBox(b->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision of a leaf node from B and a branch from A.
+ *	\param		b		[in] collision node from first tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
+{
+	// Perform triangle-box overlap test
+	if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents))	return;
+
+	// Keep same triangle, deal with first child
+	if(b->HasPosLeaf())	PrimTestIndexTri(b->GetPosPrimitive());
+	else				_CollideBoxTri(b->GetPos());
+
+	if(ContactFound()) return;
+
+	// Keep same triangle, deal with second child
+	if(b->HasNegLeaf())	PrimTestIndexTri(b->GetNegPrimitive());
+	else				_CollideBoxTri(b->GetNeg());
+}
+
+//! Request triangle vertices from the app and transform them
+#define FETCH_LEAF(prim_index, imesh, rot, trans)				\
+	mLeafIndex = prim_index;									\
+	/* Request vertices from the app */							\
+	VertexPointers VP;	imesh->GetTriangle(VP, prim_index);		\
+	/* Transform them in a common space */						\
+	TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans);	\
+	TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans);	\
+	TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for no-leaf AABB trees.
+ *	\param		a	[in] collision node from first tree
+ *	\param		b	[in] collision node from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
+{
+	// Perform BV-BV overlap test
+	if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter))	return;
+
+	// Catch leaf status
+	BOOL BHasPosLeaf = b->HasPosLeaf();
+	BOOL BHasNegLeaf = b->HasNegLeaf();
+
+	if(a->HasPosLeaf())
+	{
+		FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
+
+		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
+		else			_CollideTriBox(b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
+		else			_CollideTriBox(b->GetNeg());
+	}
+	else
+	{
+		if(BHasPosLeaf)
+		{
+			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetPos());
+		}
+		else _Collide(a->GetPos(), b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)
+		{
+			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetPos());
+		}
+		else _Collide(a->GetPos(), b->GetNeg());
+	}
+
+	if(ContactFound()) return;
+
+	if(a->HasNegLeaf())
+	{
+		FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
+
+		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
+		else			_CollideTriBox(b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
+		else			_CollideTriBox(b->GetNeg());
+	}
+	else
+	{
+		if(BHasPosLeaf)
+		{
+			// ### That leaf has possibly already been fetched
+			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetNeg());
+		}
+		else _Collide(a->GetNeg(), b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)
+		{
+			// ### That leaf has possibly already been fetched
+			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetNeg());
+		}
+		else _Collide(a->GetNeg(), b->GetNeg());
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Quantized trees
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized AABB trees.
+ *	\param		b0		[in] collision node from first tree
+ *	\param		b1		[in] collision node from second tree
+ *	\param		a		[in] extent from box A
+ *	\param		Pa		[in] center from box A
+ *	\param		b		[in] extent from box B
+ *	\param		Pb		[in] center from box B
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
+{
+	// Perform BV-BV overlap test
+	if(!BoxBoxOverlap(a, Pa, b, Pb))	return;
+
+	if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
+
+	if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
+	{
+		// Dequantize box
+		const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
+		const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
+		const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
+		_Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);
+
+		if(ContactFound()) return;
+
+		// Dequantize box
+		Box = &b0->GetPos()->mAABB;
+		const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
+		const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
+		_Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
+	}
+	else
+	{
+		// Dequantize box
+		const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
+		const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
+		const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
+		_Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);
+
+		if(ContactFound()) return;
+
+		// Dequantize box
+		Box = &b1->GetPos()->mAABB;
+		const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
+		const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
+		_Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Quantized no-leaf trees
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision of a leaf node from A and a quantized branch from B.
+ *	param		leaf	[in] leaf triangle from first tree
+ *	\param		b		[in] collision node from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
+{
+	// Dequantize box
+	const QuantizedAABB* bb = &b->mAABB;
+	const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
+	const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
+
+	// Perform triangle-box overlap test
+	if(!TriBoxOverlap(Pb, eb))	return;
+
+	if(b->HasPosLeaf())	PrimTestTriIndex(b->GetPosPrimitive());
+	else				_CollideTriBox(b->GetPos());
+
+	if(ContactFound()) return;
+
+	if(b->HasNegLeaf())	PrimTestTriIndex(b->GetNegPrimitive());
+	else				_CollideTriBox(b->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision of a leaf node from B and a quantized branch from A.
+ *	\param		b		[in] collision node from first tree
+ *	param		leaf	[in] leaf triangle from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
+{
+	// Dequantize box
+	const QuantizedAABB* bb = &b->mAABB;
+	const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
+	const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);
+
+	// Perform triangle-box overlap test
+	if(!TriBoxOverlap(Pa, ea))	return;
+
+	if(b->HasPosLeaf())	PrimTestIndexTri(b->GetPosPrimitive());
+	else				_CollideBoxTri(b->GetPos());
+
+	if(ContactFound()) return;
+
+	if(b->HasNegLeaf())	PrimTestIndexTri(b->GetNegPrimitive());
+	else				_CollideBoxTri(b->GetNeg());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Recursive collision query for quantized no-leaf AABB trees.
+ *	\param		a	[in] collision node from first tree
+ *	\param		b	[in] collision node from second tree
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
+{
+	// Dequantize box A
+	const QuantizedAABB* ab = &a->mAABB;
+	const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
+	const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
+	// Dequantize box B
+	const QuantizedAABB* bb = &b->mAABB;
+	const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
+	const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
+
+	// Perform BV-BV overlap test
+	if(!BoxBoxOverlap(ea, Pa, eb, Pb))	return;
+
+	// Catch leaf status
+	BOOL BHasPosLeaf = b->HasPosLeaf();
+	BOOL BHasNegLeaf = b->HasNegLeaf();
+
+	if(a->HasPosLeaf())
+	{
+		FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
+
+		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
+		else			_CollideTriBox(b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
+		else			_CollideTriBox(b->GetNeg());
+	}
+	else
+	{
+		if(BHasPosLeaf)
+		{
+			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetPos());
+		}
+		else _Collide(a->GetPos(), b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)
+		{
+			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetPos());
+		}
+		else _Collide(a->GetPos(), b->GetNeg());
+	}
+
+	if(ContactFound()) return;
+
+	if(a->HasNegLeaf())
+	{
+		FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
+
+		if(BHasPosLeaf)	PrimTestTriIndex(b->GetPosPrimitive());
+		else			_CollideTriBox(b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)	PrimTestTriIndex(b->GetNegPrimitive());
+		else			_CollideTriBox(b->GetNeg());
+	}
+	else
+	{
+		if(BHasPosLeaf)
+		{
+			// ### That leaf has possibly already been fetched
+			FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetNeg());
+		}
+		else _Collide(a->GetNeg(), b->GetPos());
+
+		if(ContactFound()) return;
+
+		if(BHasNegLeaf)
+		{
+			// ### That leaf has possibly already been fetched
+			FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
+
+			_CollideBoxTri(a->GetNeg());
+		}
+		else _Collide(a->GetNeg(), b->GetNeg());
+	}
+}

src/external/open_dynamics_engine-ef/ode/OPC_TreeCollider.h

@@ -0,0 +1,252 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains code for a tree collider.
+ *	\file		OPC_TreeCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_TREECOLLIDER_H__
+#define __OPC_TREECOLLIDER_H__
+
+	//! This structure holds cached information used by the algorithm.
+	//! Two model pointers and two colliding primitives are cached. Model pointers are assigned
+	//! to their respective meshes, and the pair of colliding primitives is used for temporal
+	//! coherence. That is, in case temporal coherence is enabled, those two primitives are
+	//! tested for overlap before everything else. If they still collide, we're done before
+	//! even entering the recursive collision code.
+	struct OPCODE_API BVTCache : Pair
+	{
+		//! Constructor
+		inline_				BVTCache()
+							{
+								ResetCache();
+								ResetCountDown();
+							}
+
+					void	ResetCache()
+							{
+								Model0			= null;
+								Model1			= null;
+								id0				= 0;
+								id1				= 1;
+#ifdef __MESHMERIZER_H__		// Collision hulls only supported within ICE !
+								HullTest		= true;
+								SepVector.pid	= 0;
+								SepVector.qid	= 0;
+								SepVector.SV	= Point(1.0f, 0.0f, 0.0f);
+#endif // __MESHMERIZER_H__
+							}
+
+				void	ResetCountDown()
+							{
+#ifdef __MESHMERIZER_H__		// Collision hulls only supported within ICE !
+								CountDown		= 50;
+#endif // __MESHMERIZER_H__
+							}
+
+                // ericf change
+		Model*		Model0;	//!< Model for first object
+		Model*		Model1;	//!< Model for second object
+		// const Model*		Model0;	//!< Model for first object
+		// const Model*		Model1;	//!< Model for second object
+
+#ifdef __MESHMERIZER_H__	// Collision hulls only supported within ICE !
+		SVCache				SepVector;
+		udword				CountDown;
+		bool				HullTest;
+#endif // __MESHMERIZER_H__
+	};
+
+	class OPCODE_API AABBTreeCollider : public Collider
+	{
+		public:
+		// Constructor / Destructor
+											AABBTreeCollider();
+		virtual								~AABBTreeCollider();
+		// Generic collision query
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Generic collision query for generic OPCODE models. After the call, access the results with:
+		 *	- GetContactStatus()
+		 *	- GetNbPairs()
+		 *	- GetPairs()
+		 *
+		 *	\param		cache			[in] collision cache for model pointers and a colliding pair of primitives
+		 *	\param		world0			[in] world matrix for first object, or null
+		 *	\param		world1			[in] world matrix for second object, or null
+		 *	\return		true if success
+		 *	\warning	SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+							bool			Collide(BVTCache& cache, const Matrix4x4* world0=null, const Matrix4x4* world1=null);
+
+		// Collision queries
+							bool			Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1,				const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
+							bool			Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1,					const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
+							bool			Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1,				const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
+							bool			Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1,	const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
+		// Settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: selects between full box-box tests or "SAT-lite" tests (where Class III axes are discarded)
+		 *	\param		flag		[in] true for full tests, false for coarse tests
+		 *	\see		SetFullPrimBoxTest(bool flag)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetFullBoxBoxTest(bool flag)			{ mFullBoxBoxTest		= flag;					}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Settings: selects between full triangle-box tests or "SAT-lite" tests (where Class III axes are discarded)
+		 *	\param		flag		[in] true for full tests, false for coarse tests
+		 *	\see		SetFullBoxBoxTest(bool flag)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				void			SetFullPrimBoxTest(bool flag)			{ mFullPrimBoxTest		= flag;					}
+
+		// Stats
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of BV-BV overlap tests after a collision query.
+		 *	\see		GetNbPrimPrimTests()
+		 *	\see		GetNbBVPrimTests()
+		 *	\return		the number of BV-BV tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbBVBVTests()				const	{ return mNbBVBVTests;							}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of Triangle-Triangle overlap tests after a collision query.
+		 *	\see		GetNbBVBVTests()
+		 *	\see		GetNbBVPrimTests()
+		 *	\return		the number of Triangle-Triangle tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbPrimPrimTests()			const	{ return mNbPrimPrimTests;						}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of BV-Triangle overlap tests after a collision query.
+		 *	\see		GetNbBVBVTests()
+		 *	\see		GetNbPrimPrimTests()
+		 *	\return		the number of BV-Triangle tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbBVPrimTests()				const	{ return mNbBVPrimTests;						}
+
+		// Data access
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the number of contacts after a collision query.
+		 *	\see		GetContactStatus()
+		 *	\see		GetPairs()
+		 *	\return		the number of contacts / colliding pairs.
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbPairs()					const	{ return mPairs.GetNbEntries()>>1;				}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the pairs of colliding triangles after a collision query.
+		 *	\see		GetContactStatus()
+		 *	\see		GetNbPairs()
+		 *	\return		the list of colliding pairs (triangle indices)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				const Pair*		GetPairs()						const	{ return (const Pair*)mPairs.GetEntries();		}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
+		 *	\return		null if everything is ok, else a string describing the problem
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(Collider)	const char*		ValidateSettings();
+
+		protected:
+		// Colliding pairs
+							Container		mPairs;				//!< Pairs of colliding primitives
+		// User mesh interfaces
+                            // ericf change
+					MeshInterface*	mIMesh0;			//!< User-defined mesh interface for object0
+					MeshInterface*	mIMesh1;			//!< User-defined mesh interface for object1
+					// const	MeshInterface*	mIMesh0;			//!< User-defined mesh interface for object0
+					// const	MeshInterface*	mIMesh1;			//!< User-defined mesh interface for object1
+		// Stats
+							udword			mNbBVBVTests;		//!< Number of BV-BV tests
+							udword			mNbPrimPrimTests;	//!< Number of Primitive-Primitive tests
+							udword			mNbBVPrimTests;		//!< Number of BV-Primitive tests
+		// Precomputed data
+							Matrix3x3		mAR;				//!< Absolute rotation matrix
+							Matrix3x3		mR0to1;				//!< Rotation from object0 to object1
+							Matrix3x3		mR1to0;				//!< Rotation from object1 to object0
+							Point			mT0to1;				//!< Translation from object0 to object1
+							Point			mT1to0;				//!< Translation from object1 to object0
+		// Dequantization coeffs
+							Point			mCenterCoeff0;
+							Point			mExtentsCoeff0;
+							Point			mCenterCoeff1;
+							Point			mExtentsCoeff1;
+		// Leaf description
+							Point			mLeafVerts[3];		//!< Triangle vertices
+							udword			mLeafIndex;			//!< Triangle index
+		// Settings
+							bool			mFullBoxBoxTest;	//!< Perform full BV-BV tests (true) or SAT-lite tests (false)
+							bool			mFullPrimBoxTest;	//!< Perform full Primitive-BV tests (true) or SAT-lite tests (false)
+		// Internal methods
+
+			// Standard AABB trees
+							void			_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1);
+			// Quantized AABB trees
+							void			_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb);
+			// No-leaf AABB trees
+							void			_CollideTriBox(const AABBNoLeafNode* b);
+							void			_CollideBoxTri(const AABBNoLeafNode* b);
+							void			_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b);
+			// Quantized no-leaf AABB trees
+							void			_CollideTriBox(const AABBQuantizedNoLeafNode* b);
+							void			_CollideBoxTri(const AABBQuantizedNoLeafNode* b);
+							void			_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b);
+			// Overlap tests
+							void			PrimTest(udword id0, udword id1);
+			inline_			void			PrimTestTriIndex(udword id1);
+			inline_			void			PrimTestIndexTri(udword id0);
+
+			inline_			BOOL			BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb);
+			inline_			BOOL			TriBoxOverlap(const Point& center, const Point& extents);
+			inline_			BOOL			TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2);
+			// Init methods
+							void			InitQuery(const Matrix4x4* world0=null, const Matrix4x4* world1=null);
+							bool			CheckTemporalCoherence(Pair* cache);
+
+                            //inline_				BOOL			Setup(const MeshInterface* mi0, const MeshInterface* mi1)
+                            // ericf change
+		inline_				BOOL			Setup(MeshInterface* mi0, MeshInterface* mi1)
+											{
+												mIMesh0	= mi0;
+												mIMesh1	= mi1;
+
+												if(!mIMesh0 || !mIMesh1)	return FALSE;
+
+												return TRUE;
+											}
+	};
+
+#endif // __OPC_TREECOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/OPC_TriBoxOverlap.h

@@ -0,0 +1,339 @@
+
+//! This macro quickly finds the min & max values among 3 variables
+#define FINDMINMAX(x0, x1, x2, min, max)	\
+	min = max = x0;							\
+	if(x1<min) min=x1;						\
+	if(x1>max) max=x1;						\
+	if(x2<min) min=x2;						\
+	if(x2>max) max=x2;
+
+//! TO BE DOCUMENTED
+inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox)
+{
+	Point vmin, vmax;
+	for(udword q=0;q<=2;q++)
+	{
+		if(normal[q]>0.0f)	{ vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; }
+		else				{ vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; }
+	}
+	if((normal|vmin)+d>0.0f) return FALSE;
+	if((normal|vmax)+d>=0.0f) return TRUE;
+
+	return FALSE;
+}
+
+//! TO BE DOCUMENTED
+#define AXISTEST_X01(a, b, fa, fb)							\
+	min = a*v0.y - b*v0.z;									\
+	max = a*v2.y - b*v2.z;									\
+	if(min>max) {const float tmp=max; max=min; min=tmp;	}	\
+	rad = fa * extents.y + fb * extents.z;					\
+	if(min>rad || max<-rad) return FALSE;
+
+//! TO BE DOCUMENTED
+#define AXISTEST_X2(a, b, fa, fb)							\
+	min = a*v0.y - b*v0.z;									\
+	max = a*v1.y - b*v1.z;									\
+	if(min>max) {const float tmp=max; max=min; min=tmp;	}	\
+	rad = fa * extents.y + fb * extents.z;					\
+	if(min>rad || max<-rad) return FALSE;
+
+//! TO BE DOCUMENTED
+#define AXISTEST_Y02(a, b, fa, fb)							\
+	min = b*v0.z - a*v0.x;									\
+	max = b*v2.z - a*v2.x;									\
+	if(min>max) {const float tmp=max; max=min; min=tmp;	}	\
+	rad = fa * extents.x + fb * extents.z;					\
+	if(min>rad || max<-rad) return FALSE;
+
+//! TO BE DOCUMENTED
+#define AXISTEST_Y1(a, b, fa, fb)							\
+	min = b*v0.z - a*v0.x;									\
+	max = b*v1.z - a*v1.x;									\
+	if(min>max) {const float tmp=max; max=min; min=tmp;	}	\
+	rad = fa * extents.x + fb * extents.z;					\
+	if(min>rad || max<-rad) return FALSE;
+
+//! TO BE DOCUMENTED
+#define AXISTEST_Z12(a, b, fa, fb)							\
+	min = a*v1.x - b*v1.y;									\
+	max = a*v2.x - b*v2.y;									\
+	if(min>max) {const float tmp=max; max=min; min=tmp;	}	\
+	rad = fa * extents.x + fb * extents.y;					\
+	if(min>rad || max<-rad) return FALSE;
+
+//! TO BE DOCUMENTED
+#define AXISTEST_Z0(a, b, fa, fb)							\
+	min = a*v0.x - b*v0.y;									\
+	max = a*v1.x - b*v1.y;									\
+	if(min>max) {const float tmp=max; max=min; min=tmp;	}	\
+	rad = fa * extents.x + fb * extents.y;					\
+	if(min>rad || max<-rad) return FALSE;
+
+// compute triangle edges
+// - edges lazy evaluated to take advantage of early exits
+// - fabs precomputed (half less work, possible since extents are always >0)
+// - customized macros to take advantage of the null component
+// - axis vector discarded, possibly saves useless movs
+#define IMPLEMENT_CLASS3_TESTS						\
+	float rad;										\
+	float min, max;									\
+													\
+	const float fey0 = fabsf(e0.y);					\
+	const float fez0 = fabsf(e0.z);					\
+	AXISTEST_X01(e0.z, e0.y, fez0, fey0);			\
+	const float fex0 = fabsf(e0.x);					\
+	AXISTEST_Y02(e0.z, e0.x, fez0, fex0);			\
+	AXISTEST_Z12(e0.y, e0.x, fey0, fex0);			\
+													\
+	const float fey1 = fabsf(e1.y);					\
+	const float fez1 = fabsf(e1.z);					\
+	AXISTEST_X01(e1.z, e1.y, fez1, fey1);			\
+	const float fex1 = fabsf(e1.x);					\
+	AXISTEST_Y02(e1.z, e1.x, fez1, fex1);			\
+	AXISTEST_Z0(e1.y, e1.x, fey1, fex1);			\
+													\
+	const Point e2 = mLeafVerts[0] - mLeafVerts[2];	\
+	const float fey2 = fabsf(e2.y);					\
+	const float fez2 = fabsf(e2.z);					\
+	AXISTEST_X2(e2.z, e2.y, fez2, fey2);			\
+	const float fex2 = fabsf(e2.x);					\
+	AXISTEST_Y1(e2.z, e2.x, fez2, fex2);			\
+	AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Triangle-Box overlap test using the separating axis theorem.
+ *	This is the code from Tomas Möller, a bit optimized:
+ *	- with some more lazy evaluation (faster path on PC)
+ *	- with a tiny bit of assembly
+ *	- with "SAT-lite" applied if needed
+ *	- and perhaps with some more minor modifs...
+ *
+ *	\param		center		[in] box center
+ *	\param		extents		[in] box extents
+ *	\return		true if triangle & box overlap
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents)
+{
+	// Stats
+	mNbBVPrimTests++;
+
+	// use separating axis theorem to test overlap between triangle and box 
+	// need to test for overlap in these directions: 
+	// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
+	//    we do not even need to test these) 
+	// 2) normal of the triangle 
+	// 3) crossproduct(edge from tri, {x,y,z}-directin) 
+	//    this gives 3x3=9 more tests 
+
+	// move everything so that the boxcenter is in (0,0,0) 
+	Point v0, v1, v2;
+	v0.x = mLeafVerts[0].x - center.x;
+	v1.x = mLeafVerts[1].x - center.x;
+	v2.x = mLeafVerts[2].x - center.x;
+
+	// First, test overlap in the {x,y,z}-directions
+#ifdef OPC_USE_FCOMI
+	// find min, max of the triangle in x-direction, and test for overlap in X
+	if(FCMin3(v0.x, v1.x, v2.x)>extents.x)	return FALSE;
+	if(FCMax3(v0.x, v1.x, v2.x)<-extents.x)	return FALSE;
+
+	// same for Y
+	v0.y = mLeafVerts[0].y - center.y;
+	v1.y = mLeafVerts[1].y - center.y;
+	v2.y = mLeafVerts[2].y - center.y;
+
+	if(FCMin3(v0.y, v1.y, v2.y)>extents.y)	return FALSE;
+	if(FCMax3(v0.y, v1.y, v2.y)<-extents.y)	return FALSE;
+
+	// same for Z
+	v0.z = mLeafVerts[0].z - center.z;
+	v1.z = mLeafVerts[1].z - center.z;
+	v2.z = mLeafVerts[2].z - center.z;
+
+	if(FCMin3(v0.z, v1.z, v2.z)>extents.z)	return FALSE;
+	if(FCMax3(v0.z, v1.z, v2.z)<-extents.z)	return FALSE;
+#else
+	float min,max;
+	// Find min, max of the triangle in x-direction, and test for overlap in X
+	FINDMINMAX(v0.x, v1.x, v2.x, min, max);
+	if(min>extents.x || max<-extents.x) return FALSE;
+
+	// Same for Y
+	v0.y = mLeafVerts[0].y - center.y;
+	v1.y = mLeafVerts[1].y - center.y;
+	v2.y = mLeafVerts[2].y - center.y;
+
+	FINDMINMAX(v0.y, v1.y, v2.y, min, max);
+	if(min>extents.y || max<-extents.y) return FALSE;
+
+	// Same for Z
+	v0.z = mLeafVerts[0].z - center.z;
+	v1.z = mLeafVerts[1].z - center.z;
+	v2.z = mLeafVerts[2].z - center.z;
+
+	FINDMINMAX(v0.z, v1.z, v2.z, min, max);
+	if(min>extents.z || max<-extents.z) return FALSE;
+#endif
+	// 2) Test if the box intersects the plane of the triangle
+	// compute plane equation of triangle: normal*x+d=0
+	// ### could be precomputed since we use the same leaf triangle several times
+	const Point e0 = v1 - v0;
+	const Point e1 = v2 - v1;
+	const Point normal = e0 ^ e1;
+	const float d = -normal|v0;
+	if(!planeBoxOverlap(normal, d, extents)) return FALSE;
+
+	// 3) "Class III" tests
+	if(mFullPrimBoxTest)
+	{
+		IMPLEMENT_CLASS3_TESTS
+	}
+	return TRUE;
+}
+
+//! A dedicated version where the box is constant
+inline_ BOOL OBBCollider::TriBoxOverlap()
+{
+	// Stats
+	mNbVolumePrimTests++;
+
+	// Hook
+	const Point& extents = mBoxExtents;
+	const Point& v0 = mLeafVerts[0];
+	const Point& v1 = mLeafVerts[1];
+	const Point& v2 = mLeafVerts[2];
+
+	// use separating axis theorem to test overlap between triangle and box 
+	// need to test for overlap in these directions: 
+	// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
+	//    we do not even need to test these) 
+	// 2) normal of the triangle 
+	// 3) crossproduct(edge from tri, {x,y,z}-directin) 
+	//    this gives 3x3=9 more tests 
+
+	// Box center is already in (0,0,0)
+
+	// First, test overlap in the {x,y,z}-directions
+#ifdef OPC_USE_FCOMI
+	// find min, max of the triangle in x-direction, and test for overlap in X
+	if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x)	return FALSE;
+	if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x)	return FALSE;
+
+	if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y)	return FALSE;
+	if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y)	return FALSE;
+
+	if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z)	return FALSE;
+	if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z)	return FALSE;
+#else
+	float min,max;
+	// Find min, max of the triangle in x-direction, and test for overlap in X
+	FINDMINMAX(v0.x, v1.x, v2.x, min, max);
+	if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE;
+
+	FINDMINMAX(v0.y, v1.y, v2.y, min, max);
+	if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE;
+
+	FINDMINMAX(v0.z, v1.z, v2.z, min, max);
+	if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE;
+#endif
+	// 2) Test if the box intersects the plane of the triangle
+	// compute plane equation of triangle: normal*x+d=0
+	// ### could be precomputed since we use the same leaf triangle several times
+	const Point e0 = v1 - v0;
+	const Point e1 = v2 - v1;
+	const Point normal = e0 ^ e1;
+	const float d = -normal|v0;
+	if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
+
+	// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
+	{
+		IMPLEMENT_CLASS3_TESTS
+	}
+	return TRUE;
+}
+
+//! ...and another one, jeez
+inline_ BOOL AABBCollider::TriBoxOverlap()
+{
+	// Stats
+	mNbVolumePrimTests++;
+
+	// Hook
+	const Point& center		= mBox.mCenter;
+	const Point& extents	= mBox.mExtents;
+
+	// use separating axis theorem to test overlap between triangle and box 
+	// need to test for overlap in these directions: 
+	// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
+	//    we do not even need to test these) 
+	// 2) normal of the triangle 
+	// 3) crossproduct(edge from tri, {x,y,z}-directin) 
+	//    this gives 3x3=9 more tests 
+
+	// move everything so that the boxcenter is in (0,0,0) 
+	Point v0, v1, v2;
+	v0.x = mLeafVerts[0].x - center.x;
+	v1.x = mLeafVerts[1].x - center.x;
+	v2.x = mLeafVerts[2].x - center.x;
+
+	// First, test overlap in the {x,y,z}-directions
+#ifdef OPC_USE_FCOMI
+	// find min, max of the triangle in x-direction, and test for overlap in X
+	if(FCMin3(v0.x, v1.x, v2.x)>extents.x)	return FALSE;
+	if(FCMax3(v0.x, v1.x, v2.x)<-extents.x)	return FALSE;
+
+	// same for Y
+	v0.y = mLeafVerts[0].y - center.y;
+	v1.y = mLeafVerts[1].y - center.y;
+	v2.y = mLeafVerts[2].y - center.y;
+
+	if(FCMin3(v0.y, v1.y, v2.y)>extents.y)	return FALSE;
+	if(FCMax3(v0.y, v1.y, v2.y)<-extents.y)	return FALSE;
+
+	// same for Z
+	v0.z = mLeafVerts[0].z - center.z;
+	v1.z = mLeafVerts[1].z - center.z;
+	v2.z = mLeafVerts[2].z - center.z;
+
+	if(FCMin3(v0.z, v1.z, v2.z)>extents.z)	return FALSE;
+	if(FCMax3(v0.z, v1.z, v2.z)<-extents.z)	return FALSE;
+#else
+	float min,max;
+	// Find min, max of the triangle in x-direction, and test for overlap in X
+	FINDMINMAX(v0.x, v1.x, v2.x, min, max);
+	if(min>extents.x || max<-extents.x) return FALSE;
+
+	// Same for Y
+	v0.y = mLeafVerts[0].y - center.y;
+	v1.y = mLeafVerts[1].y - center.y;
+	v2.y = mLeafVerts[2].y - center.y;
+
+	FINDMINMAX(v0.y, v1.y, v2.y, min, max);
+	if(min>extents.y || max<-extents.y) return FALSE;
+
+	// Same for Z
+	v0.z = mLeafVerts[0].z - center.z;
+	v1.z = mLeafVerts[1].z - center.z;
+	v2.z = mLeafVerts[2].z - center.z;
+
+	FINDMINMAX(v0.z, v1.z, v2.z, min, max);
+	if(min>extents.z || max<-extents.z) return FALSE;
+#endif
+	// 2) Test if the box intersects the plane of the triangle
+	// compute plane equation of triangle: normal*x+d=0
+	// ### could be precomputed since we use the same leaf triangle several times
+	const Point e0 = v1 - v0;
+	const Point e1 = v2 - v1;
+	const Point normal = e0 ^ e1;
+	const float d = -normal|v0;
+	if(!planeBoxOverlap(normal, d, extents)) return FALSE;
+
+	// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
+	{
+		IMPLEMENT_CLASS3_TESTS
+	}
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_TriTriOverlap.h

@@ -0,0 +1,279 @@
+
+//! if OPC_TRITRI_EPSILON_TEST is true then we do a check (if |dv|<EPSILON then dv=0.0;) else no check is done (which is less robust, but faster)
+#define LOCAL_EPSILON 0.000001f
+
+//! sort so that a<=b
+#define SORT(a,b)			\
+	if(a>b)					\
+	{						\
+		const float c=a;	\
+		a=b;				\
+		b=c;				\
+	}
+
+//! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202
+#define EDGE_EDGE_TEST(V0, U0, U1)						\
+	Bx = U0[i0] - U1[i0];								\
+	By = U0[i1] - U1[i1];								\
+	Cx = V0[i0] - U0[i0];								\
+	Cy = V0[i1] - U0[i1];								\
+	f  = Ay*Bx - Ax*By;									\
+	d  = By*Cx - Bx*Cy;									\
+	if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f))	\
+	{													\
+		const float e=Ax*Cy - Ay*Cx;					\
+		if(f>0.0f)										\
+		{												\
+			if(e>=0.0f && e<=f) return TRUE;			\
+		}												\
+		else											\
+		{												\
+			if(e<=0.0f && e>=f) return TRUE;			\
+		}												\
+	}
+
+//! TO BE DOCUMENTED
+#define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2)		\
+{														\
+	float Bx,By,Cx,Cy,d,f;								\
+	const float Ax = V1[i0] - V0[i0];					\
+	const float Ay = V1[i1] - V0[i1];					\
+	/* test edge U0,U1 against V0,V1 */					\
+	EDGE_EDGE_TEST(V0, U0, U1);							\
+	/* test edge U1,U2 against V0,V1 */					\
+	EDGE_EDGE_TEST(V0, U1, U2);							\
+	/* test edge U2,U1 against V0,V1 */					\
+	EDGE_EDGE_TEST(V0, U2, U0);							\
+}
+
+//! TO BE DOCUMENTED
+#define POINT_IN_TRI(V0, U0, U1, U2)					\
+{														\
+	/* is T1 completly inside T2? */					\
+	/* check if V0 is inside tri(U0,U1,U2) */			\
+	float a  = U1[i1] - U0[i1];							\
+	float b  = -(U1[i0] - U0[i0]);						\
+	float c  = -a*U0[i0] - b*U0[i1];					\
+	float d0 = a*V0[i0] + b*V0[i1] + c;					\
+														\
+	a  = U2[i1] - U1[i1];								\
+	b  = -(U2[i0] - U1[i0]);							\
+	c  = -a*U1[i0] - b*U1[i1];							\
+	const float d1 = a*V0[i0] + b*V0[i1] + c;			\
+														\
+	a  = U0[i1] - U2[i1];								\
+	b  = -(U0[i0] - U2[i0]);							\
+	c  = -a*U2[i0] - b*U2[i1];							\
+	const float d2 = a*V0[i0] + b*V0[i1] + c;			\
+	if(d0*d1>0.0f)										\
+	{													\
+		if(d0*d2>0.0f) return TRUE;						\
+	}													\
+}
+
+//! TO BE DOCUMENTED
+BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2)
+{
+	float A[3];
+	short i0,i1;
+	/* first project onto an axis-aligned plane, that maximizes the area */
+	/* of the triangles, compute indices: i0,i1. */
+	A[0] = fabsf(n[0]);
+	A[1] = fabsf(n[1]);
+	A[2] = fabsf(n[2]);
+	if(A[0]>A[1])
+	{
+		if(A[0]>A[2])
+		{
+			i0=1;      /* A[0] is greatest */
+			i1=2;
+		}
+		else
+		{
+			i0=0;      /* A[2] is greatest */
+			i1=1;
+		}
+	}
+	else   /* A[0]<=A[1] */
+	{
+		if(A[2]>A[1])
+		{
+			i0=0;      /* A[2] is greatest */
+			i1=1;
+		}
+		else
+		{
+			i0=0;      /* A[1] is greatest */
+			i1=2;
+		}
+	}
+
+	/* test all edges of triangle 1 against the edges of triangle 2 */
+	EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2);
+	EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2);
+	EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2);
+
+	/* finally, test if tri1 is totally contained in tri2 or vice versa */
+	POINT_IN_TRI(v0, u0, u1, u2);
+	POINT_IN_TRI(u0, v0, v1, v2);
+
+	return FALSE;
+}
+
+//! TO BE DOCUMENTED
+#define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1)	\
+{																						\
+	if(D0D1>0.0f)																		\
+	{																					\
+		/* here we know that D0D2<=0.0 */												\
+		/* that is D0, D1 are on the same side, D2 on the other or on the plane */		\
+		A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;				\
+	}																					\
+	else if(D0D2>0.0f)																	\
+	{																					\
+		/* here we know that d0d1<=0.0 */												\
+		A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;				\
+	}																					\
+	else if(D1*D2>0.0f || D0!=0.0f)														\
+	{																					\
+		/* here we know that d0d1<=0.0 or that D0!=0.0 */								\
+		A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2;				\
+	}																					\
+	else if(D1!=0.0f)																	\
+	{																					\
+		A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;				\
+	}																					\
+	else if(D2!=0.0f)																	\
+	{																					\
+		A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;				\
+	}																					\
+	else																				\
+	{																					\
+		/* triangles are coplanar */													\
+		return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2);								\
+	}																					\
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Triangle/triangle intersection test routine,
+ *	by Tomas Moller, 1997.
+ *	See article "A Fast Triangle-Triangle Intersection Test",
+ *	Journal of Graphics Tools, 2(2), 1997
+ *
+ *	Updated June 1999: removed the divisions -- a little faster now!
+ *	Updated October 1999: added {} to CROSS and SUB macros 
+ *
+ *	int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
+ *                      float U0[3],float U1[3],float U2[3])
+ *
+ *	\param		V0		[in] triangle 0, vertex 0
+ *	\param		V1		[in] triangle 0, vertex 1
+ *	\param		V2		[in] triangle 0, vertex 2
+ *	\param		U0		[in] triangle 1, vertex 0
+ *	\param		U1		[in] triangle 1, vertex 1
+ *	\param		U2		[in] triangle 1, vertex 2
+ *	\return		true if triangles overlap
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+inline_ BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
+{
+	// Stats
+	mNbPrimPrimTests++;
+
+	// Compute plane equation of triangle(V0,V1,V2)
+	Point E1 = V1 - V0;
+	Point E2 = V2 - V0;
+	const Point N1 = E1 ^ E2;
+	const float d1 =-N1 | V0;
+	// Plane equation 1: N1.X+d1=0
+
+	// Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
+	float du0 = (N1|U0) + d1;
+	float du1 = (N1|U1) + d1;
+	float du2 = (N1|U2) + d1;
+
+	// Coplanarity robustness check
+#ifdef OPC_TRITRI_EPSILON_TEST
+	if(fabsf(du0)<LOCAL_EPSILON) du0 = 0.0f;
+	if(fabsf(du1)<LOCAL_EPSILON) du1 = 0.0f;
+	if(fabsf(du2)<LOCAL_EPSILON) du2 = 0.0f;
+#endif
+	const float du0du1 = du0 * du1;
+	const float du0du2 = du0 * du2;
+
+	if(du0du1>0.0f && du0du2>0.0f)	// same sign on all of them + not equal 0 ?
+		return FALSE;				// no intersection occurs
+
+	// Compute plane of triangle (U0,U1,U2)
+	E1 = U1 - U0;
+	E2 = U2 - U0;
+	const Point N2 = E1 ^ E2;
+	const float d2=-N2 | U0;
+	// plane equation 2: N2.X+d2=0
+
+	// put V0,V1,V2 into plane equation 2
+	float dv0 = (N2|V0) + d2;
+	float dv1 = (N2|V1) + d2;
+	float dv2 = (N2|V2) + d2;
+
+#ifdef OPC_TRITRI_EPSILON_TEST
+	if(fabsf(dv0)<LOCAL_EPSILON) dv0 = 0.0f;
+	if(fabsf(dv1)<LOCAL_EPSILON) dv1 = 0.0f;
+	if(fabsf(dv2)<LOCAL_EPSILON) dv2 = 0.0f;
+#endif
+
+	const float dv0dv1 = dv0 * dv1;
+	const float dv0dv2 = dv0 * dv2;
+
+	if(dv0dv1>0.0f && dv0dv2>0.0f)	// same sign on all of them + not equal 0 ?
+		return FALSE;				// no intersection occurs
+
+	// Compute direction of intersection line
+	const Point D = N1^N2;
+
+	// Compute and index to the largest component of D
+	float max=fabsf(D[0]);
+	short index=0;
+	float bb=fabsf(D[1]);
+	float cc=fabsf(D[2]);
+	if(bb>max) {max=bb;index=1;}
+	if(cc>max) {max=cc;index=2;}
+
+	// This is the simplified projection onto L
+	const float vp0 = V0[index];
+	const float vp1 = V1[index];
+	const float vp2 = V2[index];
+
+	const float up0 = U0[index];
+	const float up1 = U1[index];
+	const float up2 = U2[index];
+
+	// Compute interval for triangle 1
+	float a,b,c,x0,x1;
+	NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
+
+	// Compute interval for triangle 2
+	float d,e,f,y0,y1;
+	NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
+
+	const float xx=x0*x1;
+	const float yy=y0*y1;
+	const float xxyy=xx*yy;
+
+	float isect1[2], isect2[2];
+
+	float tmp=a*xxyy;
+	isect1[0]=tmp+b*x1*yy;
+	isect1[1]=tmp+c*x0*yy;
+
+	tmp=d*xxyy;
+	isect2[0]=tmp+e*xx*y1;
+	isect2[1]=tmp+f*xx*y0;
+
+	SORT(isect1[0],isect1[1]);
+	SORT(isect2[0],isect2[1]);
+
+	if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return FALSE;
+	return TRUE;
+}

src/external/open_dynamics_engine-ef/ode/OPC_VolumeCollider.cpp

@@ -0,0 +1,111 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains base volume collider class.
+ *	\file		OPC_VolumeCollider.cpp
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains the abstract class for volume colliders.
+ *
+ *	\class		VolumeCollider
+ *	\author		Pierre Terdiman
+ *	\version	1.3
+ *	\date		June, 2, 2001
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+VolumeCollider::VolumeCollider() :
+	mTouchedPrimitives	(null),
+	mNbVolumeBVTests	(0),
+	mNbVolumePrimTests	(0)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+VolumeCollider::~VolumeCollider()
+{
+	mTouchedPrimitives = null;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider.
+ *	\return		null if everything is ok, else a string describing the problem
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const char* VolumeCollider::ValidateSettings()
+{
+	return null;
+}
+
+// Pretty dumb way to dump - to do better - one day...
+
+#define IMPLEMENT_NOLEAFDUMP(type)												\
+void VolumeCollider::_Dump(const type* node)									\
+{																				\
+	if(node->HasPosLeaf())	mTouchedPrimitives->Add(node->GetPosPrimitive());	\
+	else					_Dump(node->GetPos());								\
+																				\
+	if(ContactFound()) return;													\
+																				\
+	if(node->HasNegLeaf())	mTouchedPrimitives->Add(node->GetNegPrimitive());	\
+	else					_Dump(node->GetNeg());								\
+}
+
+#define IMPLEMENT_LEAFDUMP(type)						\
+void VolumeCollider::_Dump(const type* node)			\
+{														\
+	if(node->IsLeaf())									\
+	{													\
+		mTouchedPrimitives->Add(node->GetPrimitive());	\
+	}													\
+	else												\
+	{													\
+		_Dump(node->GetPos());							\
+														\
+		if(ContactFound()) return;						\
+														\
+		_Dump(node->GetNeg());							\
+	}													\
+}
+
+IMPLEMENT_NOLEAFDUMP(AABBNoLeafNode)
+IMPLEMENT_NOLEAFDUMP(AABBQuantizedNoLeafNode)
+
+IMPLEMENT_LEAFDUMP(AABBCollisionNode)
+IMPLEMENT_LEAFDUMP(AABBQuantizedNode)

src/external/open_dynamics_engine-ef/ode/OPC_VolumeCollider.h

@@ -0,0 +1,138 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Contains base volume collider class.
+ *	\file		OPC_VolumeCollider.h
+ *	\author		Pierre Terdiman
+ *	\date		June, 2, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Include Guard
+#ifndef __OPC_VOLUMECOLLIDER_H__
+#define __OPC_VOLUMECOLLIDER_H__
+
+	struct OPCODE_API VolumeCache
+	{
+							VolumeCache() : Model(null)		{}
+							~VolumeCache()					{}
+
+		Container			TouchedPrimitives;	//!< Indices of touched primitives
+		const BaseModel*	Model;				//!< Owner
+	};
+
+	class OPCODE_API VolumeCollider : public Collider
+	{
+		public:
+		// Constructor / Destructor
+											VolumeCollider();
+		virtual								~VolumeCollider() = 0;
+
+		// Collision report
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the number of touched primitives after a collision query.
+		 *	\see		GetContactStatus()
+		 *	\see		GetTouchedPrimitives()
+		 *	\return		the number of touched primitives
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbTouchedPrimitives()	const	{ return mTouchedPrimitives ? mTouchedPrimitives->GetNbEntries() : 0;	}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Gets the list of touched primitives after a collision query.
+		 *	\see		GetContactStatus()
+		 *	\see		GetNbTouchedPrimitives()
+		 *	\return		the list of touched primitives (primitive indices)
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_		const	udword*			GetTouchedPrimitives()		const	{ return mTouchedPrimitives ? mTouchedPrimitives->GetEntries() : null;	}
+
+		// Stats
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of Volume-BV overlap tests after a collision query.
+		 *	\see		GetNbVolumePrimTests()
+		 *	\return		the number of Volume-BV tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbVolumeBVTests()		const	{ return mNbVolumeBVTests;												}
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Stats: gets the number of Volume-Triangle overlap tests after a collision query.
+		 *	\see		GetNbVolumeBVTests()
+		 *	\return		the number of Volume-Triangle tests performed during last query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		inline_				udword			GetNbVolumePrimTests()		const	{ return mNbVolumePrimTests;											}
+
+		// Settings
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider.
+		 *	\return		null if everything is ok, else a string describing the problem
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(Collider)	const char*		ValidateSettings();
+
+		protected:
+		// Touched primitives
+							Container*		mTouchedPrimitives;	//!< List of touched primitives
+
+		// Dequantization coeffs
+							Point			mCenterCoeff;
+							Point			mExtentsCoeff;
+		// Stats
+							udword			mNbVolumeBVTests;	//!< Number of Volume-BV tests
+							udword			mNbVolumePrimTests;	//!< Number of Volume-Primitive tests
+		// Internal methods
+							void			_Dump(const AABBCollisionNode* node);
+							void			_Dump(const AABBNoLeafNode* node);
+							void			_Dump(const AABBQuantizedNode* node);
+							void			_Dump(const AABBQuantizedNoLeafNode* node);
+
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		/**
+		 *	Initializes a query
+		 */
+		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+		override(Collider) inline_	void	InitQuery()
+											{
+												// Reset stats & contact status
+												mNbVolumeBVTests	= 0;
+												mNbVolumePrimTests	= 0;
+												Collider::InitQuery();
+											}
+
+		inline_				BOOL			IsCacheValid(VolumeCache& cache)
+											{
+												// We're going to do a volume-vs-model query.
+												if(cache.Model!=mCurrentModel)
+												{
+													// Cached list was for another model so we can't keep it
+													// Keep track of new owner and reset cache
+													cache.Model = mCurrentModel;
+													return FALSE;
+												}
+												else
+												{
+													// Same models, no problem
+													return TRUE;
+												}
+											}
+	};
+
+#endif // __OPC_VOLUMECOLLIDER_H__

src/external/open_dynamics_engine-ef/ode/Opcode.cpp

@@ -0,0 +1,73 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *	OPCODE - Optimized Collision Detection
+ *	Copyright (C) 2001 Pierre Terdiman
+ *	Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *	Main file for Opcode.dll.
+ *	\file		Opcode.cpp
+ *	\author		Pierre Terdiman
+ *	\date		March, 20, 2001
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/*
+	Finding a good name is difficult!
+	Here's the draft for this lib.... Spooky, uh?
+
+	VOID?			Very Optimized Interference Detection
+	ZOID?			Zappy's Optimized Interference Detection
+	CID?			Custom/Clever Interference Detection
+	AID / ACID!		Accurate Interference Detection
+	QUID?			Quick Interference Detection
+	RIDE?			Realtime Interference DEtection
+	WIDE?			Wicked Interference DEtection (....)
+	GUID!
+	KID !			k-dop interference detection :)
+	OPCODE!			OPtimized COllision DEtection
+*/
+
+// Silence warnings about shortening 64 vit values into 32 bit containers
+#if __clang__
+#pragma clang diagnostic ignored "-Wshorten-64-to-32"
+#endif
+#ifdef _MSC_VER
+#pragma warning( disable: 4267)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "OPC_Stdafx.h"
+
+bool Opcode::InitOpcode()
+{
+	Log("// Initializing OPCODE\n\n");
+//	LogAPIInfo();
+	return true;
+}
+
+void ReleasePruningSorters();
+bool Opcode::CloseOpcode()
+{
+	Log("// Closing OPCODE\n\n");
+
+	ReleasePruningSorters();
+
+	return true;
+}
+
+#ifdef ICE_MAIN
+
+void ModuleAttach(HINSTANCE hinstance)
+{
+}
+
+void ModuleDetach()
+{
+}
+
+#endif

src/external/open_dynamics_engine-ef/ode/ode.h

@@ -0,0 +1,47 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library 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 files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_ODE_H_
+#define _ODE_ODE_H_
+
+/* include *everything* here */
+
+#include "ode/ode_config.h"
+#include "ode/ode_compatibility.h"
+#include "ode/ode_common.h"
+#include "ode/ode_contact.h"
+#include "ode/ode_error.h"
+#include "ode/ode_memory.h"
+#include "ode/ode_math.h"
+#include "ode/ode_matrix.h"
+#include "ode/ode_timer.h"
+#include "ode/ode_rotation.h"
+#include "ode/ode_mass.h"
+#include "ode/ode_misc.h"
+#include "ode/ode_objects.h"
+#include "ode/odecpp.h"
+#include "ode/ode_collision_space.h"
+#include "ode/ode_collision.h"
+#include "ode/odecpp_collision.h"
+#include "ode/ode_export-dif.h"
+
+#endif