diff --git a/ballisticacore-cmake/.idea/inspectionProfiles/Project_Default.xml b/ballisticacore-cmake/.idea/inspectionProfiles/Project_Default.xml index 4db46f7f..26034b2d 100644 --- a/ballisticacore-cmake/.idea/inspectionProfiles/Project_Default.xml +++ b/ballisticacore-cmake/.idea/inspectionProfiles/Project_Default.xml @@ -6,6 +6,7 @@ + @@ -51,6 +52,7 @@ + diff --git a/src/external/httprequest/httprequest.hpp b/src/external/httprequest/httprequest.hpp new file mode 100644 index 00000000..11869512 --- /dev/null +++ b/src/external/httprequest/httprequest.hpp @@ -0,0 +1,689 @@ +// +// https://github.com/elnormous/HTTPRequest +// + +#ifndef HTTPREQUEST_HPP +#define HTTPREQUEST_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +# if _WIN32_WINNT < _WIN32_WINNT_WINXP +char* strdup(const char* src) +{ + std::size_t length = 0; + while (src[length]) ++length; + char* result = static_cast(malloc(length + 1)); + char* p = result; + while (*src) *p++ = *src++; + *p = '\0'; + return result; +} +# include +# endif +# include +# pragma pop_macro("WIN32_LEAN_AND_MEAN") +# pragma pop_macro("NOMINMAX") +#else +# include +# include +# include +# include +# include +#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(buffer), + static_cast(length), flags); + + while (result == -1 && WSAGetLastError() == WSAEINTR) + result = ::send(endpoint, reinterpret_cast(buffer), + static_cast(length), flags); + +#else + auto result = ::send(endpoint, reinterpret_cast(buffer), + length, flags); + + while (result == -1 && errno == EINTR) + result = ::send(endpoint, reinterpret_cast(buffer), + length, flags); +#endif + if (result == -1) + throw std::system_error(getLastError(), std::system_category(), "Failed to send data"); + + return static_cast(result); + } + + size_t recv(void* buffer, size_t length, int flags) + { +#ifdef _WIN32 + auto result = ::recv(endpoint, reinterpret_cast(buffer), + static_cast(length), flags); + + while (result == -1 && WSAGetLastError() == WSAEINTR) + result = ::recv(endpoint, reinterpret_cast(buffer), + static_cast(length), flags); +#else + auto result = ::recv(endpoint, reinterpret_cast(buffer), + length, flags); + + while (result == -1 && errno == EINTR) + result = ::recv(endpoint, reinterpret_cast(buffer), + length, flags); +#endif + if (result == -1) + throw std::system_error(getLastError(), std::system_category(), "Failed to read data"); + + return static_cast(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(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 headers; + std::vector 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& parameters, + const std::vector& 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& headers = {}) + { + return send(method, + std::vector(body.begin(), body.end()), + headers); + } + + Response send(const std::string& method, + const std::vector& body, + const std::vector& 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 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 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(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 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 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::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(toWrite)); + responseData.erase(responseData.begin(), responseData.begin() + static_cast(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 diff --git a/src/external/open_dynamics_engine-ef/ode/IceAABB.cpp b/src/external/open_dynamics_engine-ef/ode/IceAABB.cpp new file mode 100644 index 00000000..5fce7cb8 --- /dev/null +++ b/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) 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 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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceAxes.h b/src/external/open_dynamics_engine-ef/ode/IceAxes.h new file mode 100644 index 00000000..1afe4cec --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceBoundingSphere.h b/src/external/open_dynamics_engine-ef/ode/IceBoundingSphere.h new file mode 100644 index 00000000..945d38cf --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceContainer.cpp b/src/external/open_dynamics_engine-ef/ode/IceContainer.cpp new file mode 100644 index 00000000..0599ba46 --- /dev/null +++ b/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(mMaxNbEntriesmMaxNbEntries) 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=0 && i>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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceHPoint.cpp b/src/external/open_dynamics_engine-ef/ode/IceHPoint.cpp new file mode 100644 index 00000000..79d60cf3 --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceHPoint.h b/src/external/open_dynamics_engine-ef/ode/IceHPoint.h new file mode 100644 index 00000000..c2248e07 --- /dev/null +++ b/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__ + diff --git a/src/external/open_dynamics_engine-ef/ode/IceIndexedTriangle.cpp b/src/external/open_dynamics_engine-ef/ode/IceIndexedTriangle.cpp new file mode 100644 index 00000000..7e5012b7 --- /dev/null +++ b/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])); +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceIndexedTriangle.h b/src/external/open_dynamics_engine-ef/ode/IceIndexedTriangle.h new file mode 100644 index 00000000..b34c485d --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceLSS.h b/src/external/open_dynamics_engine-ef/ode/IceLSS.h new file mode 100644 index 00000000..bd260c1e --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceMatrix3x3.cpp b/src/external/open_dynamics_engine-ef/ode/IceMatrix3x3.cpp new file mode 100644 index 00000000..bd9ce41d --- /dev/null +++ b/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); +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceMatrix3x3.h b/src/external/open_dynamics_engine-ef/ode/IceMatrix3x3.h new file mode 100644 index 00000000..a30680da --- /dev/null +++ b/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__ + diff --git a/src/external/open_dynamics_engine-ef/ode/IceMatrix4x4.cpp b/src/external/open_dynamics_engine-ef/ode/IceMatrix4x4.cpp new file mode 100644 index 00000000..f2b9e9e0 --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceMatrix4x4.h b/src/external/open_dynamics_engine-ef/ode/IceMatrix4x4.h new file mode 100644 index 00000000..45919be7 --- /dev/null +++ b/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__ + diff --git a/src/external/open_dynamics_engine-ef/ode/IceMemoryMacros.h b/src/external/open_dynamics_engine-ef/ode/IceMemoryMacros.h new file mode 100644 index 00000000..7ea8d4d0 --- /dev/null +++ b/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;iRelease(); (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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceOBB.cpp b/src/external/open_dynamics_engine-ef/ode/IceOBB.cpp new file mode 100644 index 00000000..d54fc902 --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceOBB.h b/src/external/open_dynamics_engine-ef/ode/IceOBB.h new file mode 100644 index 00000000..d6cf43e0 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IcePairs.h b/src/external/open_dynamics_engine-ef/ode/IcePairs.h new file mode 100644 index 00000000..2c09b929 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IcePlane.cpp b/src/external/open_dynamics_engine-ef/ode/IcePlane.cpp new file mode 100644 index 00000000..6d1aad9f --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/IcePlane.h b/src/external/open_dynamics_engine-ef/ode/IcePlane.h new file mode 100644 index 00000000..4d470814 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IcePoint.cpp b/src/external/open_dynamics_engine-ef/ode/IcePoint.cpp new file mode 100644 index 00000000..25b4f4ad --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/IcePoint.h b/src/external/open_dynamics_engine-ef/ode/IcePoint.h new file mode 100644 index 00000000..dd8d28ec --- /dev/null +++ b/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(xmax) x=max; + if(ymax) y=max; + if(zmax) 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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IcePreprocessor.h b/src/external/open_dynamics_engine-ef/ode/IcePreprocessor.h new file mode 100644 index 00000000..dbeca382 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceRandom.cpp b/src/external/open_dynamics_engine-ef/ode/IceRandom.cpp new file mode 100644 index 00000000..ce85af8d --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceRandom.h b/src/external/open_dynamics_engine-ef/ode/IceRandom.h new file mode 100644 index 00000000..3170b33d --- /dev/null +++ b/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__ + diff --git a/src/external/open_dynamics_engine-ef/ode/IceRay.cpp b/src/external/open_dynamics_engine-ef/ode/IceRay.cpp new file mode 100644 index 00000000..3288fe2b --- /dev/null +++ b/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(); +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceRay.h b/src/external/open_dynamics_engine-ef/ode/IceRay.h new file mode 100644 index 00000000..02682876 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceRevisitedRadix.cpp b/src/external/open_dynamics_engine-ef/ode/IceRevisitedRadix.cpp new file mode 100644 index 00000000..c31ed3f0 --- /dev/null +++ b/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(ValCurSize) 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>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>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=SqrLen) + { + fT = 1.0f; + Diff -= Dir; + } + else + { + fT /= SqrLen; + Diff -= fT*Dir; + } + } + + if(t) *t = fT; + + return Diff.SquareMagnitude(); +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceSegment.h b/src/external/open_dynamics_engine-ef/ode/IceSegment.h new file mode 100644 index 00000000..8d663226 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceTriList.h b/src/external/open_dynamics_engine-ef/ode/IceTriList.h new file mode 100644 index 00000000..d505459e --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceTriangle.cpp b/src/external/open_dynamics_engine-ef/ode/IceTriangle.cpp new file mode 100644 index 00000000..3d8ee96b --- /dev/null +++ b/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; + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/IceTriangle.h b/src/external/open_dynamics_engine-ef/ode/IceTriangle.h new file mode 100644 index 00000000..a984db83 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/IceTypes.h b/src/external/open_dynamics_engine-ef/ode/IceTypes.h new file mode 100644 index 00000000..4ff71b51 --- /dev/null +++ b/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 inline_ const T& TMin (const T& a, const T& b) { return b < a ? b : a; } + template inline_ const T& TMax (const T& a, const T& b) { return a < b ? b : a; } + template inline_ void TSetMin (T& a, const T& b) { if(a>b) a = b; } + template inline_ void TSetMax (T& a, const T& b) { if(a> 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<> 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 inline_ void TSwap(Type& a, Type& b) { const Type c = a; a = b; b = c; } + template inline_ Type TClamp(const Type& x, const Type& lo, const Type& hi) { return ((xhi) ? hi : x); } + + template inline_ void TSort(Type& a, Type& b) + { + if(a>b) TSwap(a, b); + } + + template 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__ diff --git a/src/external/open_dynamics_engine-ef/ode/LICENSE_BSD.TXT b/src/external/open_dynamics_engine-ef/ode/LICENSE_BSD.TXT new file mode 100644 index 00000000..112f6a27 --- /dev/null +++ b/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. diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_AABBCollider.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_AABBCollider.cpp new file mode 100644 index 00000000..dd665251 --- /dev/null +++ b/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;imCenterCoeff; + 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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_AABBCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_AABBCollider.h new file mode 100644 index 00000000..3911e957 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_AABBTree.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_AABBTree.cpp new file mode 100644 index 00000000..d8156595 --- /dev/null +++ b/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;iGetSplittingValue(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;iGetSplittingValue(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;iGetSplittingValue(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]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]]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;imNbPrimitives;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); +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_AABBTree.h b/src/external/open_dynamics_engine-ef/ode/OPC_AABBTree.h new file mode 100644 index 00000000..ee2533db --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_BaseModel.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_BaseModel.cpp new file mode 100644 index 00000000..24b5d16a --- /dev/null +++ b/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); +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_BaseModel.h b/src/external/open_dynamics_engine-ef/ode/OPC_BaseModel.h new file mode 100644 index 00000000..0057acdf --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_BoxBoxOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_BoxBoxOverlap.h new file mode 100644 index 00000000..757a17dd --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_BoxPruning.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_BoxPruning.cpp new file mode 100644 index 00000000..417e9165 --- /dev/null +++ b/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;iGetMin(Axis0); + for(udword i=0;iGetMin(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(RunningAddress1GetMax(Axis0)) + { + if(array0[Index0]->Intersect(*array1[Index1], Axis1)) + { + if(array0[Index0]->Intersect(*array1[Index1], Axis2)) + { + pairs.AddPair(Index0, Index1); + } + } + } + } + + //// + + while(RunningAddress0GetMax(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;iGetMin(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(RunningAddressGetMax(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;iGetMin(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(RunningAddressGetMax(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;iIntersect(*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;iIntersect(*array[j])) pairs.AddPair(i, j); + } + } + return true; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_BoxPruning.h b/src/external/open_dynamics_engine-ef/ode/OPC_BoxPruning.h new file mode 100644 index 00000000..460c5261 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Collider.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_Collider.cpp new file mode 100644 index 00000000..dd26e0d0 --- /dev/null +++ b/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() +{ +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Collider.h b/src/external/open_dynamics_engine-ef/ode/OPC_Collider.h new file mode 100644 index 00000000..678b5820 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Common.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_Common.cpp new file mode 100644 index 00000000..4379eb34 --- /dev/null +++ b/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; diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Common.h b/src/external/open_dynamics_engine-ef/ode/OPC_Common.h new file mode 100644 index 00000000..f1349903 --- /dev/null +++ b/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)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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_HybridModel.h b/src/external/open_dynamics_engine-ef/ode/OPC_HybridModel.h new file mode 100644 index 00000000..c7eb59d4 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_IceHook.h b/src/external/open_dynamics_engine-ef/ode/OPC_IceHook.h new file mode 100644 index 00000000..01d6e621 --- /dev/null +++ b/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 + #include + #include + #include + #include + #include + + #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; diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_LSSAABBOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_LSSAABBOverlap.h new file mode 100644 index 00000000..5cc50b58 --- /dev/null +++ b/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(s2Add(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;imCenterCoeff; + 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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_LSSCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_LSSCollider.h new file mode 100644 index 00000000..8b1f1d29 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_LSSTriOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_LSSTriOverlap.h new file mode 100644 index 00000000..f1d17e4a --- /dev/null +++ b/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 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(fSqrDist0GetTriangle(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= 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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Model.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_Model.cpp new file mode 100644 index 00000000..8fdec9e1 --- /dev/null +++ b/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(); +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Model.h b/src/external/open_dynamics_engine-ef/ode/OPC_Model.h new file mode 100644 index 00000000..98dee560 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_OBBCollider.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_OBBCollider.cpp new file mode 100644 index 00000000..d3bf19bc --- /dev/null +++ b/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 || fmB0.y || fmB0.z || f 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;imCenterCoeff; + 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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_OBBCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_OBBCollider.h new file mode 100644 index 00000000..9ebd72cc --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_OptimizedTree.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_OptimizedTree.cpp new file mode 100644 index 00000000..aa573e8d --- /dev/null +++ b/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;iCMax.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<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;iIsLeaf()) + { + _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;iHasPosLeaf()) _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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_OptimizedTree.h b/src/external/open_dynamics_engine-ef/ode/OPC_OptimizedTree.h new file mode 100644 index 00000000..f8e7498a --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Picking.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_Picking.cpp new file mode 100644 index 00000000..6b6f82b1 --- /dev/null +++ b/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.mDistancemDistance) *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 diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Picking.h b/src/external/open_dynamics_engine-ef/ode/OPC_Picking.h new file mode 100644 index 00000000..d22fa38a --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_PlanesAABBOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_PlanesAABBOverlap.h new file mode 100644 index 00000000..5d7576e5 --- /dev/null +++ b/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<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<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;iHasSingleNode()) + { + 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< 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< 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<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<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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_RayAABBOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_RayAABBOverlap.h new file mode 100644 index 00000000..a8162bf0 --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_RayCollider.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_RayCollider.cpp new file mode 100644 index 00000000..dc28b982 --- /dev/null +++ b/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(mStabbedFaces->GetFaces()); \ + if(Current && mStabbedFace.mDistancemDistance) \ + { \ + *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)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.mDistanceAddFace(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); + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_RayCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_RayCollider.h new file mode 100644 index 00000000..fe65d77e --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_RayTriOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_RayTriOverlap.h new file mode 100644 index 00000000..7fe37c98 --- /dev/null +++ b/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 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 && det1.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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Settings.h b/src/external/open_dynamics_engine-ef/ode/OPC_Settings.h new file mode 100644 index 00000000..1841a2bc --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_SphereAABBOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_SphereAABBOverlap.h new file mode 100644 index 00000000..2278bc01 --- /dev/null +++ b/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]Max[i]) + if(mCenter[i]>Max) + { + float s = mCenter[i] - Max; + d += s*s; + } + } + } +#endif + return d <= mRadius2; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_SphereCollider.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_SphereCollider.cpp new file mode 100644 index 00000000..39a4bfab --- /dev/null +++ b/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;imCenterCoeff; + 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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_SphereCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_SphereCollider.h new file mode 100644 index 00000000..6a191d00 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_SphereTriOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_SphereTriOverlap.h new file mode 100644 index 00000000..77e59f37 --- /dev/null +++ b/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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Stdafx.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_Stdafx.cpp new file mode 100644 index 00000000..a4e20edc --- /dev/null +++ b/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" diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_Stdafx.h b/src/external/open_dynamics_engine-ef/ode/OPC_Stdafx.h new file mode 100644 index 00000000..f6468a61 --- /dev/null +++ b/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_) diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_SweepAndPrune.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_SweepAndPrune.cpp new file mode 100644 index 00000000..89e3ffef --- /dev/null +++ b/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;imID = 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) 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;imIDmID); + Current = Current->mNext; + } + } +} + +void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const +{ + if(!callback) return; + + // ### Ugly and slow + for(udword i=0;imIDmID, 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;iGetMin(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>1; + + ASSERT(BoxIndexValue = 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;iid0; + 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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_SweepAndPrune.h b/src/external/open_dynamics_engine-ef/ode/OPC_SweepAndPrune.h new file mode 100644 index 00000000..2cbbb7e6 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_TreeBuilders.cpp b/src/external/open_dynamics_engine-ef/ode/OPC_TreeBuilders.cpp new file mode 100644 index 00000000..f34a4b68 --- /dev/null +++ b/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;iGetTriangle(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;iGetTriangle(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;iHasLeafNodes()!=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()); + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_TreeCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_TreeCollider.h new file mode 100644 index 00000000..31b8cc66 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_TriBoxOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_TriBoxOverlap.h new file mode 100644 index 00000000..b3a9bdee --- /dev/null +++ b/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(x1max) max=x1; \ + if(x2max) 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; +} diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_TriTriOverlap.h b/src/external/open_dynamics_engine-ef/ode/OPC_TriTriOverlap.h new file mode 100644 index 00000000..1c0b81a6 --- /dev/null +++ b/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|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)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)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]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) diff --git a/src/external/open_dynamics_engine-ef/ode/OPC_VolumeCollider.h b/src/external/open_dynamics_engine-ef/ode/OPC_VolumeCollider.h new file mode 100644 index 00000000..c0b812e7 --- /dev/null +++ b/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__ diff --git a/src/external/open_dynamics_engine-ef/ode/Opcode.cpp b/src/external/open_dynamics_engine-ef/ode/Opcode.cpp new file mode 100644 index 00000000..e52e4aae --- /dev/null +++ b/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 diff --git a/src/external/open_dynamics_engine-ef/ode/ode.cpp b/src/external/open_dynamics_engine-ef/ode/ode.cpp new file mode 100644 index 00000000..848216d5 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode.cpp @@ -0,0 +1,1827 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +// this source file is mostly concerned with the data structures, not the +// numerics. + +#include "ode/ode_objects_private.h" +#include "ode/ode.h" +#include "ode/ode_joint.h" +#include "ode/ode_math.h" +#include "ode/ode_matrix.h" +#include "ode/ode_step.h" +#include "ode/ode_quickstep.h" +#include "ode/ode_util.h" +#include "ode/ode_memory.h" +#include "ode/ode_error.h" + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// utility + + +//////////////////// ERIC TEST STUFFF + +#if VALUE_TESTING +FILE *f = NULL; + +bool testLogging = false; + +void odeTestEnableLogging() +{ + testLogging = true; + //fprintf(f,"log test\n"); +} + +void odeTestDisableLogging() +{ + testLogging = false; + //fprintf(f,"log test end\n"); +} + +#define PRINT_ARRAY(a,b) {for (int i = 0; i < b; i++) fprintf(f," %f",a[i]); fprintf(f,"\n");} + +//#define PRINT_DBL_HEX(x) {int *poo = (int*)&(x); fprintf(f,"0x%X%X\n",poo[0],poo[1]);} +void printBody(dxBody* b, int index) +{ + fprintf(f,"body %d\n", index); + fprintf(f," flags: %d\n",b->flags); + //fprintf(f," geomID: %x\n",b->geom); + fprintf(f," mass.mass: %x\n",b->mass.mass); //can check more + fprintf(f," mass.c: "); PRINT_ARRAY(b->mass.c,4); + fprintf(f," mass.I: "); PRINT_ARRAY(b->mass.I,12); + fprintf(f," invI: "); PRINT_ARRAY(b->invI,12); + fprintf(f," invMass: %f ",b->invMass); + fprintf(f," "); PRINT_DBL_HEX(b->invMass); + fprintf(f," pos: %f %f %f\n",b->pos[0],b->pos[1],b->pos[2]); + fprintf(f," p1"); PRINT_DBL_HEX(b->pos[0]); + fprintf(f," p2"); PRINT_DBL_HEX(b->pos[1]); + fprintf(f," p3"); PRINT_DBL_HEX(b->pos[2]); + fprintf(f," quat: %f %f %f %f\n",b->q[0],b->q[1],b->q[2],b->q[3]); + fprintf(f," q0"); PRINT_DBL_HEX(b->q[0]); + fprintf(f," q1"); PRINT_DBL_HEX(b->q[1]); + fprintf(f," q2"); PRINT_DBL_HEX(b->q[2]); + fprintf(f," q3"); PRINT_DBL_HEX(b->q[3]); + fprintf(f," rotMat: "); PRINT_ARRAY(b->R,12); + fprintf(f," lvel: %f %f %f\n",b->lvel[0],b->lvel[1],b->lvel[2]); + fprintf(f," lvel1"); PRINT_DBL_HEX(b->lvel[0]); + fprintf(f," lvel2"); PRINT_DBL_HEX(b->lvel[1]); + fprintf(f," lvel3"); PRINT_DBL_HEX(b->lvel[2]); + fprintf(f," avel: %f %f %f\n",b->avel[0],b->avel[1],b->avel[2]); + fprintf(f," avel1"); PRINT_DBL_HEX(b->avel[0]); + fprintf(f," avel2"); PRINT_DBL_HEX(b->avel[1]); + fprintf(f," avel3"); PRINT_DBL_HEX(b->avel[2]); + fprintf(f," facc: %f %f %f\n",b->facc[0],b->facc[1],b->facc[2]); + fprintf(f," facc1"); PRINT_DBL_HEX(b->facc[0]); + fprintf(f," facc2"); PRINT_DBL_HEX(b->facc[1]); + fprintf(f," facc3"); PRINT_DBL_HEX(b->facc[2]); + fprintf(f," tacc: %f %f %f\n",b->tacc[0],b->tacc[1],b->tacc[2]); + fprintf(f," tacc1"); PRINT_DBL_HEX(b->tacc[0]); + fprintf(f," tacc2"); PRINT_DBL_HEX(b->tacc[1]); + fprintf(f," tacc3"); PRINT_DBL_HEX(b->tacc[2]); + fprintf(f," finite_rot_axis: %f %f %f\n",b->finite_rot_axis[0], + b->finite_rot_axis[1], + b->finite_rot_axis[2]); +} + +void printJoint(dxJoint* j, int index) +{ + fprintf(f,"joint %d\n",index); + + //fprintf(f," fps: %d\n",j->fps); + + dxJoint::Info2 i2; + //dxJoint::Info1 i1; + + //fprintf(f," vtable: %x\n",j->vtable); + fprintf(f," vtable.size: %d\n",j->vtable->size); + //fprintf(f," vtable.getInfo1_fn %x\n",j->vtable->getInfo1); + //fprintf(f," vtable.getInfo2_fn %x\n",j->vtable->getInfo2); + +// j->lambda[0] = 0.0; +// j->lambda[1] = 0.0; +// j->lambda[2] = 0.0; +// j->lambda[3] = 0.0; +// j->lambda[4] = 0.0; +// j->lambda[5] = 0.0; + + fprintf(f," lambda: "); PRINT_ARRAY(j->lambda,6); + + //j->vtable->getInfo1(j,&i1); + //j->vtable->getInfo2(j,&i2); + + //fprintf(f," vtable: %x\n",j->vtable); + +} + +FILE *printODEState(dxWorld *w, int fileNum) +{ + + //double test = 1.0/24.563; + //int *t2 = (int*)&test; + + //printf("test is %f\n",test); + //printf("hex it is 0x%X%X\n",t2[0],t2[1]); + char fname[256]; + snprintf(fname,sizeof(fname),"/home/ecfroeml/Desktop/test%d.txt",fileNum); + + f = fopen(fname,"w"); + if (!f){ + printf("couldnt open file: %s\n", fname); + fflush(stdout); + // cout << "couldnt open file: " << fname << endl; + return NULL; + } + + dxBody *b; + dxJoint *j; + int n = 0; + for (b=w->firstbody; b; b=(dxBody*)b->next){ + n++; + printBody(b,n - 1); + } + if (w->nb != n) dDebug (0,"body count incorrect"); + //cout << "bodyCount: " << n << endl; + n = 0; + for (j=w->firstjoint; j; j=(dxJoint*)j->next){ + n++; + printJoint(j, n - 1); + } + + return f; +} +#endif + +/////////////////// END ERIC TEST STUFF + +static inline void initObject (dObject *obj, dxWorld *w) +{ + obj->world = w; + obj->next = 0; + obj->tome = 0; + obj->userdata = 0; + obj->tag = 0; +} + + +// add an object `obj' to the list who's head pointer is pointed to by `first'. + +static inline void addObjectToList (dObject *obj, dObject **first) +{ + obj->next = *first; + obj->tome = first; + if (*first) (*first)->tome = &obj->next; + (*first) = obj; +} + + +// remove the object from the linked list + +static inline void removeObjectFromList (dObject *obj) +{ + if (obj->next) obj->next->tome = obj->tome; + *(obj->tome) = obj->next; + // safeguard + obj->next = 0; + obj->tome = 0; +} + + +// remove the joint from neighbour lists of all connected bodies + +static void removeJointReferencesFromAttachedBodies (dxJoint *j) +{ + for (int i=0; i<2; i++) { + dxBody *body = j->node[i].body; + if (body) { + dxJointNode *n = body->firstjoint; + dxJointNode *last = 0; + while (n) { + if (n->joint == j) { + if (last) last->next = n->next; + else body->firstjoint = n->next; + break; + } + last = n; + n = n->next; + } + } + } + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; +} + +//**************************************************************************** +// debugging + +// see if an object list loops on itself (if so, it's bad). + +static int listHasLoops (dObject *first) +{ + if (first==0 || first->next==0) return 0; + dObject *a=first,*b=first->next; + int skip=0; + while (b) { + if (a==b) return 1; + b = b->next; + if (skip) a = a->next; + skip ^= 1; + } + return 0; +} + + +// check the validity of the world data structures + +static void checkWorld (dxWorld *w) +{ + dxBody *b; + dxJoint *j; + + // check there are no loops + if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops"); + if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops"); + + // check lists are well formed (check `tome' pointers) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + if (b->next && b->next->tome != &b->next) + dDebug (0,"bad tome pointer in body list"); + } + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->next && j->next->tome != &j->next) + dDebug (0,"bad tome pointer in joint list"); + } + + // check counts + int n = 0; + for (b=w->firstbody; b; b=(dxBody*)b->next){ + n++; + //printBody(b,n - 1); + } + if (w->nb != n) dDebug (0,"body count incorrect"); + //cout << "bodyCount: " << n << endl; + n = 0; + for (j=w->firstjoint; j; j=(dxJoint*)j->next){ + n++; + //printJoint(j, n - 1); + } + if (w->nj != n) dDebug (0,"joint count incorrect"); + //cout << "jointCount: " << n << endl; + + //fclose(f); + //} + + // set all tag values to a known value + static int count = 0; + count++; + for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count; + + // check all body/joint world pointers are ok + for (b=w->firstbody; b; b=(dxBody*)b->next)if (b->world != w) + dDebug (0,"bad world pointer in body list"); + for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w) + dDebug (0,"bad world pointer in joint list"); + + /* + // check for half-connected joints - actually now these are valid + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body || j->node[1].body) { + if (!(j->node[0].body && j->node[1].body)) + dDebug (0,"half connected joint found"); + } + } + */ + + // check that every joint node appears in the joint lists of both bodies it + // attaches + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + for (int i=0; i<2; i++) { + if (j->node[i].body) { + int ok = 0; + for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) { + if (n->joint == j) ok = 1; + } + if (ok==0) dDebug (0,"joint not in joint list of attached body"); + } + } + } + + // check all body joint lists (correct body ptrs) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (&n->joint->node[0] == n) { + if (n->joint->node[1].body != b) + dDebug (0,"bad body pointer in joint node of body list (1)"); + } + else { + if (n->joint->node[0].body != b) + dDebug (0,"bad body pointer in joint node of body list (2)"); + } + if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body"); + } + } + + // check all body pointers in joints, check they are distinct + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body && (j->node[0].body == j->node[1].body)) + dDebug (0,"non-distinct body pointers in joint"); + if ((j->node[0].body && j->node[0].body->tag != count) || + (j->node[1].body && j->node[1].body->tag != count)) + dDebug (0,"bad body pointer in joint"); + } +} + + +void dWorldCheck (dxWorld *w) +{ + checkWorld (w); +} + +//**************************************************************************** +// body + +dxBody *dBodyCreate (dxWorld *w) +{ + dAASSERT (w); + dxBody *b = new dxBody; + initObject (b,w); + b->firstjoint = 0; + b->flags = 0; + b->geom = 0; + dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0); + dSetZero (b->invI,4*3); + b->invI[0] = 1; + b->invI[5] = 1; + b->invI[10] = 1; + b->invMass = 1; + dSetZero (b->pos,4); + dSetZero (b->q,4); + b->q[0] = 1; + dRSetIdentity (b->R); + dSetZero (b->lvel,4); + dSetZero (b->avel,4); + dSetZero (b->facc,4); + dSetZero (b->tacc,4); + dSetZero (b->finite_rot_axis,4); + addObjectToList (b,(dObject **) &w->firstbody); + w->nb++; + + // set auto-disable parameters + dBodySetAutoDisableDefaults (b); // must do this after adding to world + b->adis_stepsleft = b->adis.idle_steps; + b->adis_timeleft = b->adis.idle_time; + + return b; +} + + +void dBodyDestroy (dxBody *b) +{ + dAASSERT (b); + + // all geoms that link to this body must be notified that the body is about + // to disappear. note that the call to dGeomSetBody(geom,0) will result in + // dGeomGetBodyNext() returning 0 for the body, so we must get the next body + // before setting the body to 0. + dxGeom *next_geom = 0; + for (dxGeom *geom = b->geom; geom; geom = next_geom) { + next_geom = dGeomGetBodyNext (geom); + dGeomSetBody (geom,0); + } + + // detach all neighbouring joints, then delete this body. + dxJointNode *n = b->firstjoint; + while (n) { + // sneaky trick to speed up removal of joint references (black magic) + n->joint->node[(n == n->joint->node)].body = 0; + + dxJointNode *next = n->next; + n->next = 0; + removeJointReferencesFromAttachedBodies (n->joint); + n = next; + } + removeObjectFromList (b); + b->world->nb--; + delete b; +} + + +void dBodySetData (dBodyID b, void *data) +{ + dAASSERT (b); + b->userdata = data; +} + + +void *dBodyGetData (dBodyID b) +{ + dAASSERT (b); + return b->userdata; +} + + +void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->pos[0] = x; + b->pos[1] = y; + b->pos[2] = z; + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetRotation (dBodyID b, const dMatrix3 R) +{ + dAASSERT (b && R); + dQuaternion q; + dRtoQ (R,q); + dNormalize4 (q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetQuaternion (dBodyID b, const dQuaternion q) +{ + dAASSERT (b && q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + //dNormalize4 (b->q); + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->lvel[0] = x; + b->lvel[1] = y; + b->lvel[2] = z; +} + + +void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->avel[0] = x; + b->avel[1] = y; + b->avel[2] = z; +} + + +const dReal * dBodyGetPosition (dBodyID b) +{ + dAASSERT (b); + return b->pos; +} + + +const dReal * dBodyGetRotation (dBodyID b) +{ + dAASSERT (b); + return b->R; +} + + +const dReal * dBodyGetQuaternion (dBodyID b) +{ + dAASSERT (b); + return b->q; +} + + +const dReal * dBodyGetLinearVel (dBodyID b) +{ + dAASSERT (b); + return b->lvel; +} + + +const dReal * dBodyGetAngularVel (dBodyID b) +{ + dAASSERT (b); + return b->avel; +} + + +void dBodySetMass (dBodyID b, const dMass *mass) +{ + dAASSERT (b && mass); + memcpy (&b->mass,mass,sizeof(dMass)); + if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) { + dDEBUGMSG ("inertia must be positive definite"); + dRSetIdentity (b->invI); + } + b->invMass = dRecip(b->mass.mass); +} + + +void dBodyGetMass (dBodyID b, dMass *mass) +{ + dAASSERT (b && mass); + memcpy (mass,&b->mass,sizeof(dMass)); +} + +#ifndef dNODEBUG +#define ERICF_MAX_FORCE_TEST 9999.0 +#define ERICF_MAX_TORQUE_TEST 9999.0 +static void printCrazyForce(float fx, float fy, float fz){ + printf("CRAAAZY FORCE BEING APPLIED!!!: %f %f %f\n",fx,fy,fz); +} +static void printCrazyTorque(float tx, float ty, float tz){ + printf("CRAAAZY TORQUE BEING APPLIED!!!: %f %f %f\n",tx,ty,tz); +} +#endif + +void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_FORCE_TEST || dFabs(fy) > ERICF_MAX_FORCE_TEST || dFabs(fz) > ERICF_MAX_FORCE_TEST){ + printCrazyForce(fx,fy,fz); + } +# endif + + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; +} + + +void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_TORQUE_TEST || dFabs(fy) > ERICF_MAX_TORQUE_TEST || dFabs(fz) > ERICF_MAX_TORQUE_TEST){ + printCrazyTorque(fx,fy,fz); + } +# endif + dAASSERT (b); + b->tacc[0] += fx; + b->tacc[1] += fy; + b->tacc[2] += fz; +} + + +void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_FORCE_TEST || dFabs(fy) > ERICF_MAX_FORCE_TEST || dFabs(fz) > ERICF_MAX_FORCE_TEST){ + printCrazyForce(fx,fy,fz); + } +# endif + + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->R,t1); + b->facc[0] += t2[0]; + b->facc[1] += t2[1]; + b->facc[2] += t2[2]; +} + + +void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_TORQUE_TEST || dFabs(fy) > ERICF_MAX_TORQUE_TEST || dFabs(fz) > ERICF_MAX_TORQUE_TEST){ + printCrazyTorque(fx,fy,fz); + } +# endif + + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->R,t1); + b->tacc[0] += t2[0]; + b->tacc[1] += t2[1]; + b->tacc[2] += t2[2]; +} + + +void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_FORCE_TEST || dFabs(fy) > ERICF_MAX_FORCE_TEST || dFabs(fz) > ERICF_MAX_FORCE_TEST){ + printCrazyForce(fx,fy,fz); + } +# endif + + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; + dVector3 f,q; + f[0] = fx; + f[1] = fy; + f[2] = fz; + q[0] = px - b->pos[0]; + q[1] = py - b->pos[1]; + q[2] = pz - b->pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_FORCE_TEST || dFabs(fy) > ERICF_MAX_FORCE_TEST || dFabs(fz) > ERICF_MAX_FORCE_TEST){ + printCrazyForce(fx,fy,fz); + } +# endif + + dAASSERT (b); + dVector3 prel,f,p; + f[0] = fx; + f[1] = fy; + f[2] = fz; + f[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_FORCE_TEST || dFabs(fy) > ERICF_MAX_FORCE_TEST || dFabs(fz) > ERICF_MAX_FORCE_TEST){ + printCrazyForce(fx,fy,fz); + } +# endif + + dAASSERT (b); + dVector3 frel,f; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + dMULTIPLY0_331 (f,b->R,frel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dVector3 q; + q[0] = px - b->pos[0]; + q[1] = py - b->pos[1]; + q[2] = pz - b->pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + // ericf tweak +# ifndef dNODEBUG + if (dFabs(fx) > ERICF_MAX_FORCE_TEST || dFabs(fy) > ERICF_MAX_FORCE_TEST || dFabs(fz) > ERICF_MAX_FORCE_TEST){ + printCrazyForce(fx,fy,fz); + } +# endif + + dAASSERT (b); + dVector3 frel,prel,f,p; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (f,b->R,frel); + dMULTIPLY0_331 (p,b->R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +const dReal * dBodyGetForce (dBodyID b) +{ + dAASSERT (b); + return b->facc; +} + + +const dReal * dBodyGetTorque (dBodyID b) +{ + dAASSERT (b); + return b->tacc; +} + + +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->facc[0] = x; + b->facc[1] = y; + b->facc[2] = z; +} + + +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->tacc[0] = x; + b->tacc[1] = y; + b->tacc[2] = z; +} + + +void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + result[0] = p[0] + b->pos[0]; + result[1] = p[1] + b->pos[1]; + result[2] = p[2] + b->pos[2]; +} + + +void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px - b->pos[0]; + p[1] = py - b->pos[1]; + p[2] = pz - b->pos[2]; + p[3] = 0; + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel; + prel[0] = px - b->pos[0]; + prel[1] = py - b->pos[1]; + prel[2] = pz - b->pos[2]; + prel[3] = 0; + dMULTIPLY1_331 (result,b->R,prel); +} + + +void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY0_331 (result,b->R,p); +} + + +void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY1_331 (result,b->R,p); +} + + +void dBodySetFiniteRotationMode (dBodyID b, int mode) +{ + dAASSERT (b); + b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis); + if (mode) { + b->flags |= dxBodyFlagFiniteRotation; + if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 || + b->finite_rot_axis[2] != 0) { + b->flags |= dxBodyFlagFiniteRotationAxis; + } + } +} + + +void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->finite_rot_axis[0] = x; + b->finite_rot_axis[1] = y; + b->finite_rot_axis[2] = z; + if (x != 0 || y != 0 || z != 0) { + dNormalize3 (b->finite_rot_axis); + b->flags |= dxBodyFlagFiniteRotationAxis; + } + else { + b->flags &= ~dxBodyFlagFiniteRotationAxis; + } +} + + +int dBodyGetFiniteRotationMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyFlagFiniteRotation) != 0); +} + + +void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result) +{ + dAASSERT (b); + result[0] = b->finite_rot_axis[0]; + result[1] = b->finite_rot_axis[1]; + result[2] = b->finite_rot_axis[2]; +} + + +int dBodyGetNumJoints (dBodyID b) +{ + dAASSERT (b); + int count=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, count++); + return count; +} + + +dJointID dBodyGetJoint (dBodyID b, int index) +{ + dAASSERT (b); + int i=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) { + if (i == index) return n->joint; + } + return 0; +} + + +void dBodyEnable (dBodyID b) +{ + dAASSERT (b); + b->flags &= ~dxBodyDisabled; + b->adis_stepsleft = b->adis.idle_steps; + b->adis_timeleft = b->adis.idle_time; +} + + +void dBodyDisable (dBodyID b) +{ + dAASSERT (b); + b->flags |= dxBodyDisabled; +} + + +int dBodyIsEnabled (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyDisabled) == 0); +} + + +void dBodySetGravityMode (dBodyID b, int mode) +{ + dAASSERT (b); + if (mode) b->flags &= ~dxBodyNoGravity; + else b->flags |= dxBodyNoGravity; +} + + +int dBodyGetGravityMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyNoGravity) == 0); +} + + +// body auto-disable functions + +dReal dBodyGetAutoDisableLinearThreshold (dBodyID b) +{ + dAASSERT(b); + return dSqrt (b->adis.linear_threshold); +} + + +void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_threshold) +{ + dAASSERT(b); + b->adis.linear_threshold = linear_threshold * linear_threshold; +} + + +dReal dBodyGetAutoDisableAngularThreshold (dBodyID b) +{ + dAASSERT(b); + return dSqrt (b->adis.angular_threshold); +} + + +void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_threshold) +{ + dAASSERT(b); + b->adis.angular_threshold = angular_threshold * angular_threshold; +} + + +int dBodyGetAutoDisableSteps (dBodyID b) +{ + dAASSERT(b); + return b->adis.idle_steps; +} + + +void dBodySetAutoDisableSteps (dBodyID b, int steps) +{ + dAASSERT(b); + b->adis.idle_steps = steps; +} + + +dReal dBodyGetAutoDisableTime (dBodyID b) +{ + dAASSERT(b); + return b->adis.idle_time; +} + + +void dBodySetAutoDisableTime (dBodyID b, dReal time) +{ + dAASSERT(b); + b->adis.idle_time = time; +} + + +int dBodyGetAutoDisableFlag (dBodyID b) +{ + dAASSERT(b); + return ((b->flags & dxBodyAutoDisable) != 0); +} + + +void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable) +{ + dAASSERT(b); + if (!do_auto_disable) b->flags &= ~dxBodyAutoDisable; + else b->flags |= dxBodyAutoDisable; +} + + +void dBodySetAutoDisableDefaults (dBodyID b) +{ + dAASSERT(b); + dWorldID w = b->world; + dAASSERT(w); + b->adis = w->adis; + dBodySetAutoDisableFlag (b, w->adis_flag); +} + +//**************************************************************************** +// joints + +static void dJointInit (dxWorld *w, dxJoint *j) +{ + dIASSERT (w && j); + initObject (j,w); + j->vtable = 0; + j->flags = 0; + j->node[0].joint = j; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].joint = j; + j->node[1].body = 0; + j->node[1].next = 0; + dSetZero (j->lambda,6); + + // ericf temp + # ifndef dNODEBUG + j->jFixed = (dxJointFixed*)j; + j->jContact = (dxJointContact*)j; + j->jMotor = (dxJointAMotor*)j; + j->jFixedEricf = (JointFixedEF*)j; + #endif + + addObjectToList (j,(dObject **) &w->firstjoint); + w->nj++; +} + + +static dxJoint *createJoint (dWorldID w, dJointGroupID group, + dxJoint::Vtable *vtable) +{ + dIASSERT (w && vtable); + dxJoint *j; + if (group) { + j = (dxJoint*) group->stack.alloc (vtable->size); + group->num++; + } + else j = (dxJoint*) dAlloc (vtable->size); + dJointInit (w,j); + j->vtable = vtable; + if (group) j->flags |= dJOINT_INGROUP; + if (vtable->init) vtable->init (j); + j->feedback = 0; + return j; +} + + +dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dball_vtable); +} + + +dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge_vtable); +} + + +dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dslider_vtable); +} + + +dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group, + const dContact *c) +{ + dAASSERT (w && c); + dxJointContact *j = (dxJointContact *) + createJoint (w,group,&__dcontact_vtable); + j->contact = *c; + return j; +} + + +dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge2_vtable); +} + + +dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__duniversal_vtable); +} + + +dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dfixed_vtable); +} + + +dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dnull_vtable); +} + + +dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__damotor_vtable); +} + + +void dJointDestroy (dxJoint *j) +{ + dAASSERT (j); + if (j->flags & dJOINT_INGROUP) return; + removeJointReferencesFromAttachedBodies (j); + removeObjectFromList (j); + j->world->nj--; + dFree (j,j->vtable->size); +} + + +dJointGroupID dJointGroupCreate (int max_size) +{ + // not any more ... dUASSERT (max_size > 0,"max size must be > 0"); + dxJointGroup *group = new dxJointGroup; + group->num = 0; + return group; +} + + +void dJointGroupDestroy (dJointGroupID group) +{ + dAASSERT (group); + dJointGroupEmpty (group); + delete group; +} + + +void dJointGroupEmpty (dJointGroupID group) +{ + // the joints in this group are detached starting from the most recently + // added (at the top of the stack). this helps ensure that the various + // linked lists are not traversed too much, as the joints will hopefully + // be at the start of those lists. + // if any group joints have their world pointer set to 0, their world was + // previously destroyed. no special handling is required for these joints. + + dAASSERT (group); + int i; + dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*)); + dxJoint *j = (dxJoint*) group->stack.rewind(); + for (i=0; i < group->num; i++) { + jlist[i] = j; + j = (dxJoint*) (group->stack.next (j->vtable->size)); + } + for (i=group->num-1; i >= 0; i--) { + if (jlist[i]->world) { + removeJointReferencesFromAttachedBodies (jlist[i]); + removeObjectFromList (jlist[i]); + jlist[i]->world->nj--; + } + } + group->num = 0; + group->stack.freeAll(); +} + + +void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2) +{ + // check arguments + dUASSERT (joint,"bad joint argument"); + dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2"); + //dxWorld *world = joint->world; + dUASSERT ( (!body1 || body1->world == joint->world) && + (!body2 || body2->world == joint->world), + "joint and bodies must be in same world"); + + // check if the joint can not be attached to just one body + dUASSERT (!((joint->flags & dJOINT_TWOBODIES) && + ((body1 != 0) ^ (body2 != 0))), + "joint can not be attached to just one body"); + + // remove any existing body attachments + if (joint->node[0].body || joint->node[1].body) { + removeJointReferencesFromAttachedBodies (joint); + } + + // if a body is zero, make sure that it is body2, so 0 --> node[1].body + if (body1==0) { + body1 = body2; + body2 = 0; + joint->flags |= dJOINT_REVERSE; + } + else { + joint->flags &= (~dJOINT_REVERSE); + } + + // attach to new bodies + joint->node[0].body = body1; + joint->node[1].body = body2; + if (body1) { + joint->node[1].next = body1->firstjoint; + body1->firstjoint = &joint->node[1]; + } + else joint->node[1].next = 0; + if (body2) { + joint->node[0].next = body2->firstjoint; + body2->firstjoint = &joint->node[0]; + } + else { + joint->node[0].next = 0; + } +} + + +void dJointSetData (dxJoint *joint, void *data) +{ + dAASSERT (joint); + joint->userdata = data; +} + + +void *dJointGetData (dxJoint *joint) +{ + dAASSERT (joint); + return joint->userdata; +} + + +int dJointGetType (dxJoint *joint) +{ + dAASSERT (joint); + return joint->vtable->typenum; +} + + +dBodyID dJointGetBody (dxJoint *joint, int index) +{ + dAASSERT (joint); + if (index == 0 || index == 1) { + if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body; + else return joint->node[index].body; + } + else return 0; +} + + +void dJointSetFeedback (dxJoint *joint, dJointFeedback *f) +{ + dAASSERT (joint); + joint->feedback = f; +} + + +dJointFeedback *dJointGetFeedback (dxJoint *joint) +{ + dAASSERT (joint); + return joint->feedback; +} + + + +dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2) +{ + dAASSERT (in_b1 || in_b2); + + dBodyID b1, b2; + + if (in_b1 == 0) { + b1 = in_b2; + b2 = in_b1; + } + else { + b1 = in_b1; + b2 = in_b2; + } + + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return n->joint; + } + + return 0; +} + + + +int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list) +{ + dAASSERT (in_b1 || in_b2); + + + dBodyID b1, b2; + + if (in_b1 == 0) { + b1 = in_b2; + b2 = in_b1; + } + else { + b1 = in_b1; + b2 = in_b2; + } + + // look through b1's neighbour list for b2 + int numConnectingJoints = 0; + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) + out_list[numConnectingJoints++] = n->joint; + } + + return numConnectingJoints; +} + + +int dAreConnected (dBodyID b1, dBodyID b2) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return 1; + } + return 0; +} + + +int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1; + } + return 0; +} + +int dWorldGetBodyCount(dxWorld *w) +{ + int count = 0; + dxBody *b = w->firstbody; + while (b) { + count++; + b = (dxBody*)b->next; + } + return count; +} + + +//**************************************************************************** +// world + +dxWorld * dWorldCreate() +{ + dxWorld *w = new dxWorld; + w->firstbody = 0; + w->firstjoint = 0; + w->nb = 0; + w->nj = 0; + dSetZero (w->gravity,4); + w->global_erp = REAL(0.2); +#if defined(dSINGLE) + w->global_cfm = 1e-5f; +#elif defined(dDOUBLE) + w->global_cfm = 1e-10; +#else + #error dSINGLE or dDOUBLE must be defined +#endif + + w->adis.linear_threshold = REAL(0.001)*REAL(0.001); // (magnitude squared) + w->adis.angular_threshold = REAL(0.001)*REAL(0.001); // (magnitude squared) + w->adis.idle_steps = 10; + w->adis.idle_time = 0; + w->adis_flag = 0; + + w->qs.num_iterations = 20; + w->qs.w = REAL(1.3); + + w->contactp.max_vel = dInfinity; + w->contactp.min_depth = 0; + + return w; +} + + +void dWorldDestroy (dxWorld *w) +{ + // delete all bodies and joints + dAASSERT (w); + dxBody *nextb, *b = w->firstbody; + while (b) { + nextb = (dxBody*) b->next; + delete b; + b = nextb; + } + dxJoint *nextj, *j = w->firstjoint; + while (j) { + nextj = (dxJoint*)j->next; + if (j->flags & dJOINT_INGROUP) { + // the joint is part of a group, so "deactivate" it instead + j->world = 0; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; + dMessage (0,"warning: destroying world containing grouped joints"); + } + else { + dFree (j,j->vtable->size); + } + j = nextj; + } + delete w; +} + + +void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z) +{ + dAASSERT (w); + w->gravity[0] = x; + w->gravity[1] = y; + w->gravity[2] = z; +} + + +void dWorldGetGravity (dWorldID w, dVector3 g) +{ + dAASSERT (w); + g[0] = w->gravity[0]; + g[1] = w->gravity[1]; + g[2] = w->gravity[2]; +} + + +void dWorldSetERP (dWorldID w, dReal erp) +{ + dAASSERT (w); + w->global_erp = erp; +} + + +dReal dWorldGetERP (dWorldID w) +{ + dAASSERT (w); + return w->global_erp; +} + + +void dWorldSetCFM (dWorldID w, dReal cfm) +{ + dAASSERT (w); + w->global_cfm = cfm; +} + + +dReal dWorldGetCFM (dWorldID w) +{ + dAASSERT (w); + return w->global_cfm; +} + + +void dWorldStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + dxProcessIslands (w,stepsize,&dInternalStepIsland); +} + +void dWorldQuickStep (dWorldID w, dReal stepsize) { + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + dxProcessIslands (w,stepsize,&dxQuickStepper); +} + +int dWorldGetQuickStepWarmStartingDataSize(dWorldID w) { + return 6 * w->nj; +} + +void dWorldGetQuickStepWarmStartingData(dWorldID w, dReal *data) { + dReal *a = data; + dxJoint *j; + for (j=w->firstjoint; j; j=(dxJoint*)j->next){ + for (int i = 0; i < 6; i++){ + *a = j->lambda[i]; + a++; + } + } +} + +void dWorldSetQuickStepWarmStartingData(dWorldID w, dReal *data) +{ + dReal *a = data; + dxJoint *j; + for (j=w->firstjoint; j; j=(dxJoint*)j->next){ + for (int i = 0; i < 6; i++){ + j->lambda[i] = *a; + a++; + } + } +} + + + + +void dWorldImpulseToForce (dWorldID w, dReal stepsize, + dReal ix, dReal iy, dReal iz, + dVector3 force) +{ + dAASSERT (w); + stepsize = dRecip(stepsize); + force[0] = stepsize * ix; + force[1] = stepsize * iy; + force[2] = stepsize * iz; + // @@@ force[3] = 0; +} + + +// world auto-disable functions + +dReal dWorldGetAutoDisableLinearThreshold (dWorldID w) +{ + dAASSERT(w); + return dSqrt (w->adis.linear_threshold); +} + + +void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_threshold) +{ + dAASSERT(w); + w->adis.linear_threshold = linear_threshold * linear_threshold; +} + + +dReal dWorldGetAutoDisableAngularThreshold (dWorldID w) +{ + dAASSERT(w); + return dSqrt (w->adis.angular_threshold); +} + + +void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_threshold) +{ + dAASSERT(w); + w->adis.angular_threshold = angular_threshold * angular_threshold; +} + + +int dWorldGetAutoDisableSteps (dWorldID w) +{ + dAASSERT(w); + return w->adis.idle_steps; +} + + +void dWorldSetAutoDisableSteps (dWorldID w, int steps) +{ + dAASSERT(w); + w->adis.idle_steps = steps; +} + + +dReal dWorldGetAutoDisableTime (dWorldID w) +{ + dAASSERT(w); + return w->adis.idle_time; +} + + +void dWorldSetAutoDisableTime (dWorldID w, dReal time) +{ + dAASSERT(w); + w->adis.idle_time = time; +} + + +int dWorldGetAutoDisableFlag (dWorldID w) +{ + dAASSERT(w); + return w->adis_flag; +} + + +void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable) +{ + dAASSERT(w); + w->adis_flag = (do_auto_disable != 0); +} + + +void dWorldSetQuickStepNumIterations (dWorldID w, int num) +{ + dAASSERT(w); + w->qs.num_iterations = num; +} + + +int dWorldGetQuickStepNumIterations (dWorldID w) +{ + dAASSERT(w); + return w->qs.num_iterations; +} + + +void dWorldSetQuickStepW (dWorldID w, dReal param) +{ + dAASSERT(w); + w->qs.w = param; +} + + +dReal dWorldGetQuickStepW (dWorldID w) +{ + dAASSERT(w); + return w->qs.w; +} + + +void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel) +{ + dAASSERT(w); + w->contactp.max_vel = vel; +} + + +dReal dWorldGetContactMaxCorrectingVel (dWorldID w) +{ + dAASSERT(w); + return w->contactp.max_vel; +} + + +void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth) +{ + dAASSERT(w); + w->contactp.min_depth = depth; +} + + +dReal dWorldGetContactSurfaceLayer (dWorldID w) +{ + dAASSERT(w); + return w->contactp.min_depth; +} + +//**************************************************************************** +// testing + +#define NUM 100 + +#define DO(x) + + +extern "C" void dTestDataStructures() +{ + int i; + DO(printf ("testDynamicsStuff()\n")); + + dBodyID body [NUM]; + int nb = 0; + dJointID joint [NUM]; + int nj = 0; + + for (i=0; i 0.5) { + DO(printf ("creating body\n")); + body[nb] = dBodyCreate (w); + DO(printf ("\t--> %p\n",body[nb])); + nb++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj < NUM && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + DO(printf ("creating joint, attaching to %p,%p\n",b1,b2)); + joint[nj] = dJointCreateBall (w,0); + DO(printf ("\t-->%p\n",joint[nj])); + checkWorld (w); + dJointAttach (joint[nj],b1,b2); + nj++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nj > 0 && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + int k = dRand() % nj; + DO(printf ("reattaching joint %p\n",joint[k])); + dJointAttach (joint[k],b1,b2); + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nb > 0 && dRandReal() > 0.5) { + int k = dRand() % nb; + DO(printf ("destroying body %p\n",body[k])); + dBodyDestroy (body[k]); + checkWorld (w); + for (; k < (NUM-1); k++) body[k] = body[k+1]; + nb--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj > 0 && dRandReal() > 0.5) { + int k = dRand() % nj; + DO(printf ("destroying joint %p\n",joint[k])); + dJointDestroy (joint[k]); + checkWorld (w); + for (; k < (NUM-1); k++) joint[k] = joint[k+1]; + nj--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + + /* + printf ("creating world\n"); + dWorldID w = dWorldCreate(); + checkWorld (w); + printf ("creating body\n"); + dBodyID b1 = dBodyCreate (w); + checkWorld (w); + printf ("creating body\n"); + dBodyID b2 = dBodyCreate (w); + checkWorld (w); + printf ("creating joint\n"); + dJointID j = dJointCreateBall (w); + checkWorld (w); + printf ("attaching joint\n"); + dJointAttach (j,b1,b2); + checkWorld (w); + printf ("destroying joint\n"); + dJointDestroy (j); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b1); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b2); + checkWorld (w); + printf ("destroying world\n"); + dWorldDestroy (w); + */ +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode.h b/src/external/open_dynamics_engine-ef/ode/ode.h new file mode 100644 index 00000000..a7257e1c --- /dev/null +++ b/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 diff --git a/src/external/open_dynamics_engine-ef/ode/ode_Opcode.h b/src/external/open_dynamics_engine-ef/ode/ode_Opcode.h new file mode 100644 index 00000000..3ac519e2 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_Opcode.h @@ -0,0 +1,113 @@ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* + * OPCODE - Optimized Collision Detection + * Copyright (C) 2001 Pierre Terdiman + * Homepage: http://www.codercorner.com/Opcode.htm + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Main file for Opcode.dll. + * \file Opcode.h + * \author Pierre Terdiman + * \date March, 20, 2001 + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Include Guard +#ifndef __OPCODE_H__ +#define __OPCODE_H__ + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Things to help us compile on non-windows platforms + +#if defined(__APPLE__) || defined(__MACOSX__) +#if __APPLE_CC__ < 1495 +#define sqrtf sqrt +#define sinf sin +#define cosf cos +#define acosf acos +#define asinf sinf +#endif +#endif + +#ifndef _MSC_VER + +#ifndef __int64 +#define __int64 long long int +#endif +#define __stdcall /* */ + +#endif + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Compilation messages +#ifdef _MSC_VER + #if defined(OPCODE_EXPORTS) + // #pragma message("Compiling OPCODE") + #elif !defined(OPCODE_EXPORTS) + // #pragma message("Using OPCODE") + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Automatic linking + #ifndef BAN_OPCODE_AUTOLINK + #ifdef _DEBUG + //#pragma comment(lib, "Opcode_D.lib") + #else + //#pragma comment(lib, "Opcode.lib") + #endif + #endif + #endif +#endif + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Preprocessor +#ifndef ICE_NO_DLL + #ifdef OPCODE_EXPORTS + #define OPCODE_API// __declspec(dllexport) + #else + #define OPCODE_API// __declspec(dllimport) + #endif +#else + #define OPCODE_API +#endif + + #include "ode/OPC_IceHook.h" + + namespace Opcode + { + // Bulk-of-the-work + #include "ode/OPC_Settings.h" + #include "ode/OPC_Common.h" + #include "ode/OPC_MeshInterface.h" + // Builders + #include "ode/OPC_TreeBuilders.h" + // Trees + #include "ode/OPC_AABBTree.h" + #include "ode/OPC_OptimizedTree.h" + // Models + #include "ode/OPC_BaseModel.h" + #include "ode/OPC_Model.h" + #include "ode/OPC_HybridModel.h" + // Colliders + #include "ode/OPC_Collider.h" + #include "ode/OPC_VolumeCollider.h" + #include "ode/OPC_TreeCollider.h" + #include "ode/OPC_RayCollider.h" + #include "ode/OPC_SphereCollider.h" + #include "ode/OPC_OBBCollider.h" + #include "ode/OPC_AABBCollider.h" + #include "ode/OPC_LSSCollider.h" + #include "ode/OPC_PlanesCollider.h" + // Usages + #include "ode/OPC_Picking.h" + // Sweep-and-prune + #include "ode/OPC_BoxPruning.h" + #include "ode/OPC_SweepAndPrune.h" + + FUNCTION OPCODE_API bool InitOpcode(); + FUNCTION OPCODE_API bool CloseOpcode(); + } + +#endif // __OPCODE_H__ diff --git a/src/external/open_dynamics_engine-ef/ode/ode_array.cpp b/src/external/open_dynamics_engine-ef/ode/ode_array.cpp new file mode 100644 index 00000000..16dc9abf --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_array.cpp @@ -0,0 +1,80 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_config.h" +#include "ode/ode_memory.h" +#include "ode/ode_error.h" +#include "ode/ode_array.h" + + +static inline int roundUpToPowerOfTwo (int x) +{ + int i = 1; + while (i < x) i <<= 1; + return i; +} + + +void dArrayBase::_freeAll (int sizeofT) +{ + if (_data) { + if (_data == this+1) return; // if constructLocalArray() was called + dFree (_data,_anum * sizeofT); + } +} + + +void dArrayBase::_setSize (int newsize, int sizeofT) +{ + if (newsize < 0) return; + if (newsize > _anum) { + if (_data == this+1) { + // this is a no-no, because constructLocalArray() was called + dDebug (0,"setSize() out of space in LOCAL array"); + } + int newanum = roundUpToPowerOfTwo (newsize); + if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT); + else _data = dAlloc (newanum*sizeofT); + _anum = newanum; + } + _size = newsize; +} + + +void * dArrayBase::operator new (size_t size) +{ + return dAlloc (size); +} + + +void dArrayBase::operator delete (void *ptr, size_t size) +{ + dFree (ptr,size); +} + + +void dArrayBase::constructLocalArray (int __anum) +{ + _size = 0; + _anum = __anum; + _data = this+1; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_array.h b/src/external/open_dynamics_engine-ef/ode/ode_array.h new file mode 100644 index 00000000..7835b631 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_array.h @@ -0,0 +1,135 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* this comes from the `reuse' library. copy any changes back to the source. + * + * Variable sized array template. The array is always stored in a contiguous + * chunk. The array can be resized. A size increase will cause more memory + * to be allocated, and may result in relocation of the array memory. + * A size decrease has no effect on the memory allocation. + * + * Array elements with constructors or destructors are not supported! + * But if you must have such elements, here's what to know/do: + * - Bitwise copy is used when copying whole arrays. + * - When copying individual items (via push(), insert() etc) the `=' + * (equals) operator is used. Thus you should define this operator to do + * a bitwise copy. You should probably also define the copy constructor. + */ + + +#ifndef _ODE_ARRAY_H_ +#define _ODE_ARRAY_H_ + +#include "ode/ode_config.h" + + +// this base class has no constructors or destructor, for your convenience. + +class dArrayBase { +protected: + int _size; // number of elements in `data' + int _anum; // allocated number of elements in `data' + void *_data; // array data + + void _freeAll (int sizeofT); + void _setSize (int newsize, int sizeofT); + // set the array size to `newsize', allocating more memory if necessary. + // if newsize>_anum and is a power of two then this is guaranteed to + // set _size and _anum to newsize. + +public: + // not: dArrayBase () { _size=0; _anum=0; _data=0; } + + int size() const { return _size; } + int allocatedSize() const { return _anum; } + void * operator new (size_t size); + void operator delete (void *ptr, size_t size); + + void constructor() { _size=0; _anum=0; _data=0; } + // if this structure is allocated with malloc() instead of new, you can + // call this to set it up. + + void constructLocalArray (int __anum); + // this helper function allows non-reallocating arrays to be constructed + // on the stack (or in the heap if necessary). this is something of a + // kludge and should be used with extreme care. this function acts like + // a constructor - it is called on uninitialized memory that will hold the + // Array structure and the data. __anum is the number of elements that + // are allocated. the memory MUST be allocated with size: + // sizeof(ArrayBase) + __anum*sizeof(T) + // arrays allocated this way will never try to reallocate or free the + // memory - that's your job. +}; + + +template class dArray : public dArrayBase { +public: + void equals (const dArray &x) { + setSize (x.size()); + memcpy (_data,x._data,x._size * sizeof(T)); + } + + dArray () { constructor(); } + dArray (const dArray &x) { constructor(); equals (x); } + ~dArray () { _freeAll(sizeof(T)); } + void setSize (int newsize) { _setSize (newsize,sizeof(T)); } + T *data() const { return (T*) _data; } + T & operator[] (int i) const { return ((T*)_data)[i]; } + void operator = (const dArray &x) { equals (x); } + + void push (const T item) { + if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); + memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T)); + } + + void swap (dArray &x) { + int tmp1; + void *tmp2; + tmp1=_size; _size=x._size; x._size=tmp1; + tmp1=_anum; _anum=x._anum; x._anum=tmp1; + tmp2=_data; _data=x._data; x._data=tmp2; + } + + // insert the item at the position `i'. if i<0 then add the item to the + // start, if i >= size then add the item to the end of the array. + void insert (int i, const T item) { + if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); + if (i >= (_size-1)) i = _size-1; // add to end + else { + if (i < 0) i=0; // add to start + int n = _size-1-i; + if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T)); + } + ((T*)_data)[i] = item; + } + + void remove (int i) { + if (i >= 0 && i < _size) { // passing this test guarantees size>0 + int n = _size-1-i; + if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T)); + _size--; + } + } +}; + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision.h b/src/external/open_dynamics_engine-ef/ode/ode_collision.h new file mode 100644 index 00000000..46b07161 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision.h @@ -0,0 +1,197 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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_COLLISION_H_ +#define _ODE_COLLISION_H_ + +#include "ode/ode_common.h" +#include "ode/ode_collision_space.h" +#include "ode/ode_contact.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ************************************************************************ */ +/* general functions */ + +void dGeomDestroy (dGeomID); +void dGeomSetData (dGeomID, void *); +void *dGeomGetData (dGeomID); +void dGeomSetBody (dGeomID, dBodyID); +dBodyID dGeomGetBody (dGeomID); +void dGeomSetPosition (dGeomID, dReal x, dReal y, dReal z); +void dGeomSetRotation (dGeomID, const dMatrix3 R); +void dGeomSetQuaternion (dGeomID, const dQuaternion); +const dReal * dGeomGetPosition (dGeomID); +const dReal * dGeomGetRotation (dGeomID); +void dGeomGetRelPointPos (dGeomID g, dReal px, dReal py, dReal pz,dVector3 result); +void dGeomGetQuaternion (dGeomID, dQuaternion result); +void dGeomGetAABB (dGeomID, dReal aabb[6]); +int dGeomIsSpace (dGeomID); +dSpaceID dGeomGetSpace (dGeomID); +int dGeomGetClass (dGeomID); +void dGeomSetCategoryBits (dGeomID, unsigned long bits); +void dGeomSetCollideBits (dGeomID, unsigned long bits); +unsigned long dGeomGetCategoryBits (dGeomID); +unsigned long dGeomGetCollideBits (dGeomID); +void dGeomEnable (dGeomID); +void dGeomDisable (dGeomID); +int dGeomIsEnabled (dGeomID); + +/* ************************************************************************ */ +/* collision detection */ + +int dCollide (dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, + int skip); +void dSpaceCollide (dSpaceID space, void *data, dNearCallback *callback); +void dSpaceCollide2 (dGeomID o1, dGeomID o2, void *data, + dNearCallback *callback); + +/* ************************************************************************ */ +/* standard classes */ + +/* the maximum number of user classes that are supported */ +enum { + dMaxUserClasses = 4 +}; + +/* class numbers - each geometry object needs a unique number */ +enum { + dSphereClass = 0, + dBoxClass, + dCCylinderClass, + dCylinderClass, + dPlaneClass, + dRayClass, + dGeomTransformClass, + dTriMeshClass, + + dFirstSpaceClass, + dSimpleSpaceClass = dFirstSpaceClass, + dHashSpaceClass, + dSweepAndPruneSpaceClass, // SAP + dQuadTreeSpaceClass, + dLastSpaceClass = dQuadTreeSpaceClass, + + dFirstUserClass, + dLastUserClass = dFirstUserClass + dMaxUserClasses - 1, + dGeomNumClasses +}; + + +dGeomID dCreateSphere (dSpaceID space, dReal radius); +void dGeomSphereSetRadius (dGeomID sphere, dReal radius); +dReal dGeomSphereGetRadius (dGeomID sphere); +dReal dGeomSpherePointDepth (dGeomID sphere, dReal x, dReal y, dReal z); + +dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz); +void dGeomBoxSetLengths (dGeomID box, dReal lx, dReal ly, dReal lz); +void dGeomBoxGetLengths (dGeomID box, dVector3 result); +dReal dGeomBoxPointDepth (dGeomID box, dReal x, dReal y, dReal z); + +dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); +void dGeomPlaneSetParams (dGeomID plane, dReal a, dReal b, dReal c, dReal d); +void dGeomPlaneGetParams (dGeomID plane, dVector4 result); +dReal dGeomPlanePointDepth (dGeomID plane, dReal x, dReal y, dReal z); + +dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length); +void dGeomCCylinderSetParams (dGeomID ccylinder, dReal radius, dReal length); +void dGeomCCylinderGetParams (dGeomID ccylinder, dReal *radius, dReal *length); +dReal dGeomCCylinderPointDepth (dGeomID ccylinder, dReal x, dReal y, dReal z); + +//new cylinder code +dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length); +void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length); +void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length); +// end new cylinder code + +dGeomID dCreateRay (dSpaceID space, dReal length); +void dGeomRaySetLength (dGeomID ray, dReal length); +dReal dGeomRayGetLength (dGeomID ray); +void dGeomRaySet (dGeomID ray, dReal px, dReal py, dReal pz, + dReal dx, dReal dy, dReal dz); +void dGeomRayGet (dGeomID ray, dVector3 start, dVector3 dir); + +/* + * Set/get ray flags that influence ray collision detection. + * These flags are currently only noticed by the trimesh collider, because + * they can make a major differences there. + */ +void dGeomRaySetParams (dGeomID g, int FirstContact, int BackfaceCull); +void dGeomRayGetParams (dGeomID g, int *FirstContact, int *BackfaceCull); +void dGeomRaySetClosestHit (dGeomID g, int closestHit); +int dGeomRayGetClosestHit (dGeomID g); + +#include "ode/ode_collision_trimesh.h" + +dGeomID dCreateGeomTransform (dSpaceID space); +void dGeomTransformSetGeom (dGeomID g, dGeomID obj); +dGeomID dGeomTransformGetGeom (dGeomID g); +void dGeomTransformSetCleanup (dGeomID g, int mode); +int dGeomTransformGetCleanup (dGeomID g); +void dGeomTransformSetInfo (dGeomID g, int mode); +int dGeomTransformGetInfo (dGeomID g); + +/* ************************************************************************ */ +/* utility functions */ + +void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2, + const dVector3 b1, const dVector3 b2, + dVector3 cp1, dVector3 cp2); + +int dBoxTouchesBox (const dVector3 _p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 _p2, + const dMatrix3 R2, const dVector3 side2); + +void dInfiniteAABB (dGeomID geom, dReal aabb[6]); +void dCloseODE(void); + +/* ************************************************************************ */ +/* custom classes */ + +typedef void dGetAABBFn (dGeomID, dReal aabb[6]); +typedef int dColliderFn (dGeomID o1, dGeomID o2, + int flags, dContactGeom *contact, int skip); +typedef dColliderFn * dGetColliderFnFn (int num); +typedef void dGeomDtorFn (dGeomID o); +typedef int dAABBTestFn (dGeomID o1, dGeomID o2, dReal aabb[6]); + +typedef struct dGeomClass { + int bytes; + dGetColliderFnFn *collider; + dGetAABBFn *aabb; + dAABBTestFn *aabb_test; + dGeomDtorFn *dtor; +} dGeomClass; + +int dCreateGeomClass (const dGeomClass *classptr); +void * dGeomGetClassData (dGeomID); +dGeomID dCreateGeom (int classnum); + +/* ************************************************************************ */ + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_box.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_box.cpp new file mode 100644 index 00000000..bf25bdfc --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_box.cpp @@ -0,0 +1,994 @@ +/************************************************************************* +* * +* Open Dynamics Engine, Copyright (C) 2001-2003 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. * +* * +*************************************************************************/ + +/* + * Cylinder-box collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" +#include "ode/ode_collision_kernel.h" + +static const int MAX_CYLBOX_CLIP_POINTS = 16; +static const int nCYLINDER_AXIS = 2; +// Number of segment of cylinder base circle. +// Must be divisible by 4. +static const int nCYLINDER_SEGMENT = 8; + +#define MAX_FLOAT dInfinity + +// Data that passed through the collider's functions +typedef struct _sCylinderBoxData +{ + // cylinder parameters + dMatrix3 mCylinderRot; + dVector3 vCylinderPos; + dVector3 vCylinderAxis; + dReal fCylinderRadius; + dReal fCylinderSize; + dVector3 avCylinderNormals[nCYLINDER_SEGMENT]; + + // box parameters + + dMatrix3 mBoxRot; + dVector3 vBoxPos; + dVector3 vBoxHalfSize; + // box vertices array : 8 vertices + dVector3 avBoxVertices[8]; + + // global collider data + dVector3 vDiff; + dVector3 vNormal; + dReal fBestDepth; + dReal fBestrb; + dReal fBestrc; + int iBestAxis; + + // contact data + dVector3 vEp0, vEp1; + dReal fDepth0, fDepth1; + + // ODE stuff + dGeomID gBox; + dGeomID gCylinder; + dContactGeom* gContact; + int iFlags; + int iSkip; + int nContacts; + +} sCylinderBoxData; + + +// initialize collision data +void _cldInitCylinderBox(sCylinderBoxData& cData) +{ + // get cylinder position, orientation + const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); + dMatrix3Copy(pRotCyc,cData.mCylinderRot); + + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); + dVector3Copy(*pPosCyc,cData.vCylinderPos); + + dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); + + // get cylinder radius and size + dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); + + // get box position, orientation, size + const dReal* pRotBox = dGeomGetRotation(cData.gBox); + dMatrix3Copy(pRotBox,cData.mBoxRot); + const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox); + dVector3Copy(*pPosBox,cData.vBoxPos); + + dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize); + cData.vBoxHalfSize[0] *= REAL(0.5); + cData.vBoxHalfSize[1] *= REAL(0.5); + cData.vBoxHalfSize[2] *= REAL(0.5); + + // vertex 0 + cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2]; + + // vertex 1 + cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2]; + + // vertex 2 + cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2]; + + // vertex 3 + cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2]; + + // vertex 4 + cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2]; + + // vertex 5 + cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0]; + cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2]; + + // vertex 6 + cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1]; + cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2]; + + // vertex 7 + cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0]; + cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1]; + cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2]; + + // temp index + int i = 0; + dVector3 vTempBoxVertices[8]; + // transform vertices in absolute space + for(i=0; i < 8; i++) + { + dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]); + dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]); + } + + // find relative position + dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff); + cData.fBestDepth = MAX_FLOAT; + cData.vNormal[0] = REAL(0.0); + cData.vNormal[1] = REAL(0.0); + cData.vNormal[2] = REAL(0.0); + + // calculate basic angle for nCYLINDER_SEGMENT-gon + dReal fAngle = M_PI/nCYLINDER_SEGMENT; + + // calculate angle increment + dReal fAngleIncrement = fAngle * REAL(2.0); + + // calculate nCYLINDER_SEGMENT-gon points + for(i = 0; i < nCYLINDER_SEGMENT; i++) + { + cData.avCylinderNormals[i][0] = -dCos(fAngle); + cData.avCylinderNormals[i][1] = -dSin(fAngle); + cData.avCylinderNormals[i][2] = 0; + + fAngle += fAngleIncrement; + } + + cData.fBestrb = 0; + cData.fBestrc = 0; + cData.iBestAxis = 0; + cData.nContacts = 0; + +} + +// test for given separating axis +int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis ) +{ + // check length of input normal + dReal fL = dVector3Length(vInputNormal); + // if not long enough + if ( fL < 1e-5f ) + { + // do nothing + return 1; + } + + // otherwise make it unit for sure + dNormalize3(vInputNormal); + + // project box and Cylinder on mAxis + dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal); + + dReal frc; + + if (fdot1 > REAL(1.0)) + { + fdot1 = REAL(1.0); + frc = dFabs(cData.fCylinderSize*REAL(0.5)); + } + + // project box and capsule on iAxis + frc = dFabs( fdot1 * (cData.fCylinderSize*REAL(0.5))) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + + dVector3 vTemp1; + dReal frb = REAL(0.0); + + dMat3GetCol(cData.mBoxRot,0,vTemp1); + frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0]; + + dMat3GetCol(cData.mBoxRot,1,vTemp1); + frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1]; + + dMat3GetCol(cData.mBoxRot,2,vTemp1); + frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2]; + + // project their distance on separating axis + dReal fd = dVector3Dot(cData.vDiff,vInputNormal); + + // if they do not overlap exit, we have no intersection + if ( dFabs(fd) > frc+frb ) + { + return 0; + } + + // get depth + dReal fDepth = - dFabs(fd) + (frc+frb); + + // get maximum depth + if ( fDepth < cData.fBestDepth ) + { + cData.fBestDepth = fDepth; + dVector3Copy(vInputNormal,cData.vNormal); + cData.iBestAxis = iAxis; + cData.fBestrb = frb; + cData.fBestrc = frc; + + // flip normal if interval is wrong faced + if (fd > 0) + { + dVector3Inv(cData.vNormal); + } + } + + return 1; +} + + +// check for separation between box edge and cylinder circle edge +int _cldTestEdgeCircleAxis( sCylinderBoxData& cData, + const dVector3 &vCenterPoint, + const dVector3 &vVx0, const dVector3 &vVx1, + int iAxis ) +{ + // calculate direction of edge + dVector3 vDirEdge; + dVector3Subtract(vVx1,vVx0,vDirEdge); + dNormalize3(vDirEdge); + // starting point of edge + dVector3 vEStart; + dVector3Copy(vVx0,vEStart);; + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2) < 1e-5f) + { + // this can't be separating axis, because edge is parallel to circle plane + return 1; + } + + // find point of intersection between edge line and circle plane + dVector3 vTemp1; + dVector3Subtract(vCenterPoint,vEStart,vTemp1); + dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis); + dVector3 vpnt; + vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2); + vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2); + vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2); + + // find tangent vector on circle with same center (vCenterPoint) that + // touches point of intersection (vpnt) + dVector3 vTangent; + dVector3Subtract(vCenterPoint,vpnt,vTemp1); + dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent); + + // find vector orthogonal both to tangent and edge direction + dVector3 vAxis; + dVector3Cross(vTangent,vDirEdge,vAxis); + + // use that vector as separating axis + return _cldTestAxis( cData, vAxis, iAxis ); +} + +// Test separating axis for collision +int _cldTestSeparatingAxes(sCylinderBoxData& cData) +{ + // reset best axis + cData.fBestDepth = MAX_FLOAT; + cData.iBestAxis = 0; + cData.fBestrb = 0; + cData.fBestrc = 0; + cData.nContacts = 0; + + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = 1e-6f; + + // axis A0 + dMat3GetCol(cData.mBoxRot, 0 , vAxis); + if (!_cldTestAxis( cData, vAxis, 1 )) + { + return 0; + } + + // axis A1 + dMat3GetCol(cData.mBoxRot, 1 , vAxis); + if (!_cldTestAxis( cData, vAxis, 2 )) + { + return 0; + } + + // axis A2 + dMat3GetCol(cData.mBoxRot, 2 , vAxis); + if (!_cldTestAxis( cData, vAxis, 3 )) + { + return 0; + } + + // axis C - Cylinder Axis + //vAxis = vCylinderAxis; + dVector3Copy(cData.vCylinderAxis , vAxis); + if (!_cldTestAxis( cData, vAxis, 4 )) + { + return 0; + } + + // axis CxA0 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 )); + dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 5 )) + { + return 0; + } + } + + // axis CxA1 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 )); + dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 6 )) + { + return 0; + } + } + + // axis CxA2 + //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 )); + dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 7 )) + { + return 0; + } + } + + int i = 0; + dVector3 vTemp1; + dVector3 vTemp2; + // here we check box's vertices axis + for(i=0; i< 8; i++) + { + //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos)); + dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1); + dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2); + //vAxis = ( vCylinderAxis cross vAxis ); + dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis); + if(dVector3Length2( vAxis ) > fEpsilon ) + { + if (!_cldTestAxis( cData, vAxis, 8 + i )) + { + return 0; + } + } + } + + // ************************************ + // this is defined for first 12 axes + // normal of plane that contains top circle of cylinder + // center of top circle of cylinder + dVector3 vcc; + vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + // ************************************ + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) + { + return 0; + } + + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) + { + return 0; + } + + // ************************************ + // this is defined for second 12 axes + // normal of plane that contains bottom circle of cylinder + // center of bottom circle of cylinder + // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5)); + vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + // ************************************ + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38)) + { + return 0; + } + + if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39)) + { + return 0; + } + + return 1; +} + +int _cldClipCylinderToBox(sCylinderBoxData& cData) +{ + + // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal + dVector3 vN; + dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal); + vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1; + vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1; + vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1; + + // normalize that vector + dNormalize3(vN); + + // translate cylinder end points by the vector + dVector3 vCposTrans; + vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius; + vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius; + vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius; + + cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + // transform edge points in box space + cData.vEp0[0] -= cData.vBoxPos[0]; + cData.vEp0[1] -= cData.vBoxPos[1]; + cData.vEp0[2] -= cData.vBoxPos[2]; + + cData.vEp1[0] -= cData.vBoxPos[0]; + cData.vEp1[1] -= cData.vBoxPos[1]; + cData.vEp1[2] -= cData.vBoxPos[2]; + + dVector3 vTemp1; + // clip the edge to box + dVector4 plPlane; + // plane 0 +x + dMat3GetCol(cData.mBoxRot,0,vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 1 +y + dMat3GetCol(cData.mBoxRot,1,vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 2 +z + dMat3GetCol(cData.mBoxRot,2,vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 3 -x + dMat3GetCol(cData.mBoxRot,0,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 4 -y + dMat3GetCol(cData.mBoxRot,1,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // plane 5 -z + dMat3GetCol(cData.mBoxRot,2,vTemp1); + dVector3Inv(vTemp1); + dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); + if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) + { + return 0; + } + + // calculate depths for both contact points + cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal); + cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal); + + // clamp depths to 0 + if(cData.fDepth0<0) + { + cData.fDepth0 = REAL(0.0); + } + + if(cData.fDepth1<0) + { + cData.fDepth1 = REAL(0.0); + } + + // back transform edge points from box to absolute space + cData.vEp0[0] += cData.vBoxPos[0]; + cData.vEp0[1] += cData.vBoxPos[1]; + cData.vEp0[2] += cData.vBoxPos[2]; + + cData.vEp1[0] += cData.vBoxPos[0]; + cData.vEp1[1] += cData.vBoxPos[1]; + cData.vEp1[2] += cData.vBoxPos[2]; + + if (cData.nContacts < (cData.iFlags & NUMC_MASK)){ + dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact0->depth = cData.fDepth0; + dVector3Copy(cData.vNormal,Contact0->normal); + dVector3Copy(cData.vEp0,Contact0->pos); + Contact0->g1 = cData.gCylinder; + Contact0->g2 = cData.gBox; + dVector3Inv(Contact0->normal); + cData.nContacts++; + } + + if (cData.nContacts < (cData.iFlags & NUMC_MASK)){ + dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact1->depth = cData.fDepth1; + dVector3Copy(cData.vNormal,Contact1->normal); + dVector3Copy(cData.vEp1,Contact1->pos); + Contact1->g1 = cData.gCylinder; + Contact1->g2 = cData.gBox; + dVector3Inv(Contact1->normal); + cData.nContacts++; + } + + return 1; +} + + +void _cldClipBoxToCylinder(sCylinderBoxData& cData ) +{ + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + // check which circle from cylinder we take for clipping + if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) ) + { + // get top circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[0] = REAL(0.0); + vCylinderCircleNormal_Rel[1] = REAL(0.0); + vCylinderCircleNormal_Rel[2] = REAL(0.0); + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[0] = REAL(0.0); + vCylinderCircleNormal_Rel[1] = REAL(0.0); + vCylinderCircleNormal_Rel[2] = REAL(0.0); + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + // vNr is normal in Box frame, pointing from Cylinder to Box + dVector3 vNr; + dMatrix3 mBoxInv; + + // Find a way to use quaternion + dMatrix3Inv(cData.mBoxRot,mBoxInv); + dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr); + + dVector3 vAbsNormal; + + vAbsNormal[0] = dFabs( vNr[0] ); + vAbsNormal[1] = dFabs( vNr[1] ); + vAbsNormal[2] = dFabs( vNr[2] ); + + // find which face in box is closest to cylinder + int iB0, iB1, iB2; + + // Different from Croteam's code + if (vAbsNormal[1] > vAbsNormal[0]) + { + // 1 > 0 + if (vAbsNormal[0]> vAbsNormal[2]) + { + // 0 > 2 -> 1 > 0 >2 + iB0 = 1; iB1 = 0; iB2 = 2; + } + else + { + // 2 > 0-> Must compare 1 and 2 + if (vAbsNormal[1] > vAbsNormal[2]) + { + // 1 > 2 -> 1 > 2 > 0 + iB0 = 1; iB1 = 2; iB2 = 0; + } + else + { + // 2 > 1 -> 2 > 1 > 0; + iB0 = 2; iB1 = 1; iB2 = 0; + } + } + } + else + { + // 0 > 1 + if (vAbsNormal[1] > vAbsNormal[2]) + { + // 1 > 2 -> 0 > 1 > 2 + iB0 = 0; iB1 = 1; iB2 = 2; + } + else + { + // 2 > 1 -> Must compare 0 and 2 + if (vAbsNormal[0] > vAbsNormal[2]) + { + // 0 > 2 -> 0 > 2 > 1; + iB0 = 0; iB1 = 2; iB2 = 1; + } + else + { + // 2 > 0 -> 2 > 0 > 1; + iB0 = 2; iB1 = 0; iB2 = 1; + } + } + } + + dVector3 vCenter; + // find center of box polygon + dVector3 vTemp; + if (vNr[iB0] > 0) + { + dMat3GetCol(cData.mBoxRot,iB0,vTemp); + vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0]; + vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1]; + vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2]; + } + else + { + dMat3GetCol(cData.mBoxRot,iB0,vTemp); + vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0]; + vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1]; + vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2]; + } + + // find the vertices of box polygon + dVector3 avPoints[4]; + dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; + dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; + + int i=0; + for(i=0; i= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment %2) + { + for( i=0; i REAL(0.0)) + { + // generate contacts + if (cData.nContacts < (cData.iFlags & NUMC_MASK)){ + dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact0->depth = fTempDepth; + dVector3Copy(cData.vNormal,Contact0->normal); + dVector3Copy(vPoint,Contact0->pos); + Contact0->g1 = cData.gCylinder; + Contact0->g2 = cData.gBox; + dVector3Inv(Contact0->normal); + cData.nContacts++; + } + } + } + } + else + { + for( i=0; i REAL(0.0)) + { + // generate contacts + if (cData.nContacts < (cData.iFlags & NUMC_MASK)){ + dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); + Contact0->depth = fTempDepth; + dVector3Copy(cData.vNormal,Contact0->normal); + dVector3Copy(vPoint,Contact0->pos); + Contact0->g1 = cData.gCylinder; + Contact0->g2 = cData.gBox; + dVector3Inv(Contact0->normal); + cData.nContacts++; + } + } + } + } +} + + +// Cylinder - Box by CroTeam +// Ported by Nguyen Binh +int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + sCylinderBoxData cData; + + // Assign ODE stuff + cData.gCylinder = o1; + cData.gBox = o2; + cData.iFlags = flags; + cData.iSkip = skip; + cData.gContact = contact; + + // initialize collider + _cldInitCylinderBox( cData ); + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes( cData ) ) + { + // if not found do nothing + return 0; + } + + // if best separation axis is not found + if ( cData.iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(0); + // do nothing + return 0; + } + + dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis); + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + // clip cylinder over box + if(!_cldClipCylinderToBox(cData)) + { + return 0; + } + } + else + { + _cldClipBoxToCylinder(cData); + } + + return cData.nContacts; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_plane.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_plane.cpp new file mode 100644 index 00000000..3a30cee7 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_plane.cpp @@ -0,0 +1,180 @@ +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define SQRT3_2 ((dReal)0.86602540378443864676) +#define dLENGTH(a) (dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) )); +#define dOPC(a,op,b,c) \ + (a)[0] = ((b)[0]) op (c); \ + (a)[1] = ((b)[1]) op (c); \ + (a)[2] = ((b)[2]) op (c); +#define dOPE(a,op,b) \ + (a)[0] op ((b)[0]); \ + (a)[1] op ((b)[1]); \ + (a)[2] op ((b)[2]); +#define dOP(a,op,b,c) \ + (a)[0] = ((b)[0]) op ((c)[0]); \ + (a)[1] = ((b)[1]) op ((c)[1]); \ + (a)[2] = ((b)[2]) op ((c)[2]); + + +/* + * There are five cases: no collision, one-point collision when one + edge + * circle intersects the plane, two-point collision when both edge + circles + * intersect the plane, three-point collision when the two edge + circles are + * on opposite sides of the plane, and deep collision when the center + of + * the cylinder has penetrated the plane (ugh). The contact normal is + * always perpendicular to the plane. + */ + +static void generatePlaneContact(dxGeom *geom, dxGeom *plane, + dContactGeom *contact, int skip, + dVector4 pparams, dVector3 point) { + dContactGeom *c = CONTACT(contact,skip); + dReal depth = -dDOT(pparams,point); + c->pos[0] = point[0] + depth*pparams[0]; + c->pos[1] = point[1] + depth*pparams[1]; + c->pos[2] = point[2] + depth*pparams[2]; + c->normal[0] = pparams[0]; + c->normal[1] = pparams[1]; + c->normal[2] = pparams[2]; + c->depth = depth; + c->g1 = geom; + c->g2 = plane; +} + +int dCollideCylinderPlane(dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + int maxContacts = flags&0xFFFF, numContacts = 0; + dReal radius, half_length; + dGeomCylinderGetParams( o1, &radius, &half_length ); + half_length /= 2; + const dReal *pos = dGeomGetPosition( o1 ); + const dReal *rot = dGeomGetRotation( o1 ); + + dVector4 pparams; + dGeomPlaneGetParams( o2, pparams ); + + // Early-out now by colliding against the cylinder's bounding + //sphere? + + dVector3 axis = { rot[2], rot[6], rot[10] }; + dVector3 ctop = { pos[0]+half_length*axis[0], + pos[1]+half_length*axis[1], + pos[2]+half_length*axis[2] }; + dVector3 cbot = { pos[0]-half_length*axis[0], + pos[1]-half_length*axis[1], + pos[2]-half_length*axis[2] }; + + dVector3 cross, rvec; + dCROSS(cross,=,pparams,axis); + dReal projectedRadius = radius * dLENGTH(cross); + dNormalize3(cross); + dCROSS(rvec,=,cross,axis); + dNormalize3(rvec); + dOPC(rvec,*,rvec,radius); + if( dDOT(pparams,rvec) > 0 ) { + dOPE(rvec,=-,rvec); + } + dReal dtop = dDOT(pparams,ctop); + dReal dbot = dDOT(pparams,cbot); + dVector3 point; + + // Has the center penetrated? + if( dDOT(pparams,pos) <= pparams[3] ) { + // Drat. Hopefully this will blast us out of the plane. + if( dtop < dbot ) { dOP(point,+,ctop,rvec); } + else { dOP(point,+,cbot,rvec); } + generatePlaneContact(o1,o2,contact,(numContacts++)*skip, + pparams,point); + } + else { + // Has the top face penetrated? + if( dtop - projectedRadius <= pparams[3] ) { + dOP(point,+,ctop,rvec); + generatePlaneContact(o1,o2,contact,(numContacts++)*skip, + pparams,point); + // Are we allowed to look for more contacts? + if( maxContacts >= 2 ) { + // Has the bottom face penetrated too? + if( dbot - projectedRadius <= pparams[3] ) { + dOP(point,+,cbot,rvec); + + generatePlaneContact(o1,o2,contact,(numContacts++)*skip, + pparams,point); + } + // Has the *whole* top face penetrated? + else if( dtop + projectedRadius <= pparams[3] ) { + if( maxContacts >= 3 ) { + dVector3 rvec2; + dCROSS(rvec2,=,axis,rvec); + dOPC(rvec,/,rvec,2); + dOPC(rvec2,*,rvec2,SQRT3_2); + point[0] = ctop[0]-rvec[0]+rvec2[0]; + point[1] = ctop[1]-rvec[1]+rvec2[1]; + point[2] = ctop[2]-rvec[2]+rvec2[2]; + generatePlaneContact(o1,o2,contact, + (numContacts++)*skip, + pparams,point); + point[0] = ctop[0]-rvec[0]-rvec2[0]; + point[1] = ctop[1]-rvec[1]-rvec2[1]; + point[2] = ctop[2]-rvec[2]-rvec2[2]; + generatePlaneContact(o1,o2,contact, + (numContacts++)*skip, + pparams,point); + } + else { + dOP(point,-,ctop,rvec); + generatePlaneContact(o1,o2,contact, + (numContacts++)*skip, + pparams,point); + } + } + } + } + // Has the bottom face penetrated? + else if( dbot - projectedRadius <= pparams[3] ) { + dOP(point,+,cbot,rvec); + generatePlaneContact(o1,o2,contact,(numContacts++)*skip, + pparams,point); + // Are we allowed to look for more contacts? + if( maxContacts >= 2 ) { + // Has the *whole* bottom face penetrated? + if( dbot + projectedRadius <= pparams[3] ) { + if( maxContacts >= 3 ) { + dVector3 rvec2; + dCROSS(rvec2,=,axis,rvec); + dOPC(rvec,/,rvec,2); + dOPC(rvec2,*,rvec2,SQRT3_2); + point[0] = cbot[0]-rvec[0]+rvec2[0]; + point[1] = cbot[1]-rvec[1]+rvec2[1]; + point[2] = cbot[2]-rvec[2]+rvec2[2]; + generatePlaneContact(o1,o2,contact, + (numContacts++)*skip, + pparams,point); + point[0] = cbot[0]-rvec[0]-rvec2[0]; + point[1] = cbot[1]-rvec[1]-rvec2[1]; + point[2] = cbot[2]-rvec[2]-rvec2[2]; + generatePlaneContact(o1,o2,contact, + (numContacts++)*skip, + pparams,point); + } + else { + dOP(point,-,cbot,rvec); + generatePlaneContact(o1,o2,contact, + (numContacts++)*skip, + pparams,point); + } + } + } + } + } + return numContacts; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_sphere.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_sphere.cpp new file mode 100644 index 00000000..1c55a592 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_sphere.cpp @@ -0,0 +1,580 @@ +/************************************************************************* +* * +* Open Dynamics Engine, Copyright (C) 2001-2003 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. * +* * +*************************************************************************/ + +/* + * Cylinder-sphere collider by Alen Ladavac (Croteam) + * Ported to ODE by Nguyen Binh ( www.coderfarm.com ) + */ + +// NOTES : +// I only try to solve some obvious problems on cylinder-sphere +// If you like to solve all problem when very large sphere drop over +// very small cylinder or vice versa, you will need to re-organize the code. +// I would eventually, try to do it later, when I have more time. +// On this version, I only try to solve the problem when a very large cylinder +// drop over very small sphere. + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" +#include "ode/ode_collision_kernel.h" +#include "ode/ode_objects.h" + +// Axis of Cylinder - ODE use axis Z. +static const int nCYLINDER_AXIS = 2; + +// Method to cure very deep penetration +// When two objects are in very deep penetration, we should exaggerate +// the depth value between them or numerical errors will make one object go +// through the other. +// 0 : do nothing - keep the calculated depth +// 1 : exaggerate calculated depth by multiple of fDepthRecoverRatio times. +// 2 : exaggerate calculated depth by multiple of log of ration between +// heavy object and light object. This method take a little more computing +// power but can be used to solved almost all "drop very large object over +// small object" problems. + +#define _DEPTH_RECOVER_METHOD_ 0 + +#if (_DEPTH_RECOVER_METHOD_ == 1) +static const dReal fDepthRecoverRatio = REAL(2.0); +#endif + +int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, int flags, dContactGeom *contact, int skip) +{ + // get source hull position and orientation + // Number of contacts + int nContacts = 0; + + dQuaternion mQuatCylinder; + dGeomGetQuaternion(gCylinder,mQuatCylinder); + + dVector3 vCylinderPos; + dMatrix3 mCylinderRot; + const dReal* pRotCyc = dGeomGetRotation(gCylinder); + dMatrix3Copy(pRotCyc,mCylinderRot); + + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(gCylinder); + dVector3Copy(*pPosCyc,vCylinderPos); + + // get capsule radius and size + dReal fCylinderRadius; + dReal fCylinderSize; + + dGeomCylinderGetParams(gCylinder,&fCylinderRadius,&fCylinderSize); + + // get destination hull position and radius + dMatrix3 mSphereRot; + dVector3 vSpherePos; + + dReal fSphereRadius = dGeomSphereGetRadius(gSphere); + + const dReal* pSphereRot = dGeomGetRotation(gSphere); + dMatrix3Copy(pSphereRot,mSphereRot); + + const dVector3* pSpherePos = (const dVector3*)dGeomGetPosition(gSphere); + dVector3Copy(*pSpherePos,vSpherePos); + + // transform sphere position in cylinder frame + dVector3 vSpherePosInCylinderFrame; + // temporary variables + dVector3 vTemp; + dVector3 vTemp2; + + // Sphere position relative to Cylinder + dVector3Subtract(vSpherePos,vCylinderPos,vTemp); + + dQuaternion mInvQuatCylinder; + dQuatInv(mQuatCylinder,mInvQuatCylinder); + dQuatTransform(mInvQuatCylinder,vTemp,vSpherePosInCylinderFrame); + + // cylinder boundaries along cylinder axis + dReal fHighCylinderBase = fCylinderSize*0.5f; + dReal fLowCylinderBase = -fCylinderSize*0.5f; + + dReal fDeltaHigh = (vSpherePosInCylinderFrame[nCYLINDER_AXIS] - fHighCylinderBase ); + dReal fDeltaLow = (fLowCylinderBase - vSpherePosInCylinderFrame[nCYLINDER_AXIS] ); + + // check if sphere intersecting with cylindrical part - side part + if( fDeltaHigh <= REAL(0.0) && fDeltaLow <= REAL(0.0)) + { + // This mean the center of sphere lies between high and low base along cylinder axis + // of the cylinder + + // calculate center of sphere on cylindrical axis which is referent for collision + // This circle of cylinder is in the same level with the center of sphere + dVector3 vBodyPoint = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vBodyPoint[nCYLINDER_AXIS] = vSpherePosInCylinderFrame[nCYLINDER_AXIS]; + + // calculate distance between two spheres + dVector3Subtract(vSpherePosInCylinderFrame,vBodyPoint,vTemp ); + dReal fDistance = dVector3Length( vTemp ); + + if ( fDistance <= (fCylinderRadius + fSphereRadius)) + { + dReal fTemp; + dReal fDepth; + dVector3 vPoint; + + // Axis dependent - Should change when you don't use cylinder along z axis + if (dFabs(vSpherePosInCylinderFrame[0]) <= fCylinderRadius + && dFabs(vSpherePosInCylinderFrame[1]) <= fCylinderRadius) + { + // Actually, not side penetrate but very deep top (or) bottom penetrate + // We have to use some trick to solve it. + + // First try to find top or bottom penetrate + dVector3 vCylinderLinearVel = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + dVector3 vSphereLinearVel = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + dBodyID CylinderBody = dGeomGetBody(gCylinder); + dBodyID SphereBody = dGeomGetBody(gSphere); + + // Get linear velocity + if (CylinderBody) + { + const dReal* pCylinderVel = dBodyGetLinearVel(CylinderBody); + vCylinderLinearVel[0] = pCylinderVel[0]; + vCylinderLinearVel[1] = pCylinderVel[1]; + vCylinderLinearVel[2] = pCylinderVel[2]; + } + + if (SphereBody) + { + const dReal* pSphereVel = dBodyGetLinearVel(SphereBody); + vSphereLinearVel[0] = pSphereVel[0]; + vSphereLinearVel[1] = pSphereVel[1]; + vSphereLinearVel[2] = pSphereVel[2]; + } + + dVector3 vSphereVelInCylinderFrame; + dVector3Subtract(vSphereLinearVel,vCylinderLinearVel,vSphereVelInCylinderFrame); + + #if (_DEPTH_RECOVER_METHOD_ == 2) + dReal fRelativeVel = dVector3Length(vSphereVelInCylinderFrame); + #endif + + dNormalize3(vSphereVelInCylinderFrame); + + dVector3 vCylinderAxis; + dMat3GetCol(mCylinderRot,nCYLINDER_AXIS,vCylinderAxis); + dNormalize3(vCylinderAxis); + + dReal fAngle = dVector3Dot(vSphereVelInCylinderFrame,vCylinderAxis); + + // Solve problem when drop very large cylinder over very small sphere + if (fAngle < 0 ) + { + // Top penetrate + // collision normal showing up from top cylinder plane + dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vNormal[nCYLINDER_AXIS] = REAL(-1.0); + + // Transform to cylinder space + dQuatTransform(mQuatCylinder,vNormal,vTemp2); + dNormalize3(vTemp2); + + // set collision point in cylinder frame + dVector3Copy(vSpherePosInCylinderFrame,vPoint); + vPoint[nCYLINDER_AXIS] = fHighCylinderBase; + + // transform in absolute space + dQuatTransform(mQuatCylinder,vPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vPoint); + + // calculate collision depth + dReal fDepth = fSphereRadius - fDeltaHigh; + // Experiment show that we need to exaggerate depth ratio to + // keep the small object from being stuck in other + + #if (_DEPTH_RECOVER_METHOD_ == 1) + // Constant ratio + fDepth *= fDepthRecoverRatio; + #endif // (_DEPTH_RECOVER_METHOD_ == 1) + + // use log of ratio between large object and small object masses + #if (_DEPTH_RECOVER_METHOD_ == 2) + if (CylinderBody && SphereBody) + { + // No static geom -> need to exaggerate + dMass sphereMass; + dBodyGetMass(SphereBody,&sphereMass); + dMass cylinderMass; + dBodyGetMass(CylinderBody,&cylinderMass); + + dReal fRatio1 = cylinderMass.mass/sphereMass.mass; + + if (fRatio1 > REAL(1.0)) + { + fDepth *= fRelativeVel *dSqrt(fRatio1); + } + else + { + fDepth *= fRelativeVel * dSqrt( REAL(1.0) / fRatio1); + } + } + #endif // (_DEPTH_RECOVER_METHOD_ == 2) + + // generate contact + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vTemp2,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + } + + return nContacts; + } + else + { + // Near Bottom + dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vNormal[nCYLINDER_AXIS] = REAL(1.0); + // Transform to cylinder space + dQuatTransform(mQuatCylinder,vNormal,vTemp2); + dNormalize3(vTemp2); + + // set collision point in cylinder frame + dVector3Copy(vSpherePosInCylinderFrame,vPoint); + vPoint[nCYLINDER_AXIS] = fLowCylinderBase; + + // transform in absolute space + dQuatTransform(mQuatCylinder,vPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vPoint); + + // calculate collision depth + dReal fDepth = fSphereRadius - fDeltaLow; + // Experiment show that we need to exaggerate depth ratio to + // keep the small sphere from being stuck + + #if (_DEPTH_RECOVER_METHOD_ == 1) + // Constant ratio + fDepth *= fDepthRecoverRatio; + #endif // (_DEPTH_RECOVER_METHOD_ == 1) + + // use log of ratio between large object and small object masses + #if (_DEPTH_RECOVER_METHOD_ == 2) + if (CylinderBody && SphereBody) + { + // No static geom -> need to exaggerate + dMass sphereMass; + dBodyGetMass(SphereBody,&sphereMass); + dMass cylinderMass; + dBodyGetMass(CylinderBody,&cylinderMass); + + dReal fRatio1 = cylinderMass.mass/sphereMass.mass; + + if (fRatio1 > REAL(1.0)) + { + fDepth *= fRelativeVel *dSqrt(fRatio1); + } + else + { + fDepth *= fRelativeVel * dSqrt( REAL(1.0) / fRatio1); + } + } + #endif // (_DEPTH_RECOVER_METHOD_ == 2) + + + // generate contact + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vTemp2,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + } + + return nContacts; + } + } + + // calculate collision normal + dVector3 vNormal; + dQuatTransform(mQuatCylinder,vBodyPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vTemp2); + dVector3Subtract(vSpherePos,vTemp2,vNormal); + dNormalize3(vNormal); + + // calculate collision point + fTemp = fCylinderRadius-fSphereRadius-fDistance; + + vPoint[0] = vSpherePos[0] + vNormal[0]* fTemp *REAL(0.5); + vPoint[1] = vSpherePos[1] + vNormal[1]* fTemp *REAL(0.5); + vPoint[2] = vSpherePos[2] + vNormal[2]* fTemp *REAL(0.5); + + // calculate penetration depth + fDepth = fCylinderRadius + fSphereRadius-fDistance; + + // generate contact + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vNormal,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + dVector3Inv(Contact->normal); + } + + return nContacts; + } + // check if sphere is intersecting with top or bottom circle of cylinder + } + else + { + // test sphere on top circle of cylinder + if ( fDeltaHigh > REAL(0.0)) + { + // check if sphere is intersecting top plane + if( fDeltaHigh < fSphereRadius ) + { + // calculate center of sphere on cylindrical axis which is referent for collision + dVector3 vBodyPoint = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vBodyPoint[nCYLINDER_AXIS] = vSpherePosInCylinderFrame[nCYLINDER_AXIS]; + + // distance between sphere and cylinder axis + dVector3Subtract(vSpherePosInCylinderFrame,vBodyPoint,vTemp); + dReal fDistance = dVector3Length(vTemp); + + // see if our intersection point is inside top circle + if( fDistance < fCylinderRadius) + { + // collision normal showing up from top cylinder plane + dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vNormal[nCYLINDER_AXIS] = REAL(-1.0); + // Transform to cylinder space + dQuatTransform(mQuatCylinder,vNormal,vTemp2); + dNormalize3(vTemp2); + // set collision point in cylinder frame + dVector3 vPoint; + dVector3Copy(vSpherePosInCylinderFrame,vPoint); + vPoint[nCYLINDER_AXIS] = fHighCylinderBase; + + // transform in absolute space + dQuatTransform(mQuatCylinder,vPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vPoint); + + // calculate collision depth + dReal fDepth = fSphereRadius - fDeltaHigh; + + // generate contact + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vTemp2,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + } + + return nContacts; + } + + // if we got here then we are potentially intersecting the top ring + // of the cylinder + + // define top circle center point on cylinder axis + dVector3 vE0 = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vE0[nCYLINDER_AXIS] = fHighCylinderBase; + + // set direction vector from center to circle edge + dVector3 vDirVector; + dVector3Subtract(vSpherePosInCylinderFrame,vE0,vDirVector); + + // project it onto top plane + vDirVector[nCYLINDER_AXIS] = REAL(0.0); + + // and make it unit vector + dNormalize3(vDirVector); + + // define point on the top circle edge + dVector3 vPoint; + vPoint[0] = vE0[0] + vDirVector[0] * fCylinderRadius; + vPoint[1] = vE0[1] + vDirVector[1] * fCylinderRadius; + vPoint[2] = vE0[2] + vDirVector[2] * fCylinderRadius; + + // calculate distance from edge to sphere + dVector3Subtract(vPoint,vSpherePosInCylinderFrame,vTemp); + dReal fDistEdgeToSphere = dVector3Length(vTemp); + + // if edge/sphere are intersecting + if (fDistEdgeToSphere < fSphereRadius ) + { + // transform in absolute space + dQuatTransform(mQuatCylinder,vPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vPoint); + + // calculate collision normal + dVector3 vNormal; + dVector3Subtract(vPoint,vSpherePos,vNormal); + + // and make it unit vector + dNormalize3(vNormal); + // calculate penetration depth + dReal fDepth = fSphereRadius - fDistEdgeToSphere; + + // generate contact + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vNormal,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + } + + return nContacts; + } + } + + // test sphere on bottom circle of cylinder + } + else + if (vSpherePosInCylinderFrame[nCYLINDER_AXIS] < fLowCylinderBase) + { + + if( fDeltaLow < fSphereRadius ) + { + // calculate center of sphere on cylindrical axis which is referent for collision + dVector3 vBodyPoint = {REAL(0.0),REAL(0.0),REAL(0.0)}; + vBodyPoint[nCYLINDER_AXIS] = vSpherePosInCylinderFrame[nCYLINDER_AXIS]; + + // distance between sphere and cylinder axis + dVector3Subtract(vSpherePosInCylinderFrame,vBodyPoint,vTemp); + dReal fDistance = dVector3Length(vTemp); + + // see if our intersection point is inside bottom circle + if( fDistance < fCylinderRadius ) + { + // collision normal showing up from top cylinder plane + dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)};//(0,-1,0); + vNormal[nCYLINDER_AXIS] = REAL(1.0); + + dQuatTransform(mQuatCylinder,vNormal,vTemp2); + dNormalize3(vTemp2); + + // set collision point in cylinder frame + dVector3 vPoint; + dVector3Copy(vSpherePosInCylinderFrame,vPoint); + vPoint[nCYLINDER_AXIS] = fLowCylinderBase; + + // transform in absolute space + dQuatTransform(mQuatCylinder,vPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vPoint); + + // calculate collision depth + dReal fDepth = fSphereRadius - fDeltaLow; + + // generate contact + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vTemp2,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + } + + return nContacts; + } + + // if we got here then we are potentially intersecting the bottom ring + // of the cylinder + + // define top circle center point on cylinder axis + dVector3 vE0 = {REAL(0.0),REAL(0.0),REAL(0.0)};//(0,fLowCylinderBase,0); + vE0[nCYLINDER_AXIS] = fLowCylinderBase; + + // set direction vector from center to circle edge + dVector3 vDirVector; + dVector3Subtract(vSpherePosInCylinderFrame,vE0,vDirVector); + + // project it onto top plane + vDirVector[nCYLINDER_AXIS] = REAL(0.0); + + // and make it unit vector + dNormalize3(vDirVector); + + // define point on the top circle edge + dVector3 vPoint; + vPoint[0] = vE0[0] + vDirVector[0] * fCylinderRadius; + vPoint[1] = vE0[1] + vDirVector[1] * fCylinderRadius; + vPoint[2] = vE0[2] + vDirVector[2] * fCylinderRadius; + + dVector3Subtract(vPoint,vSpherePosInCylinderFrame,vTemp); + dReal fDistEdgeToSphere = dVector3Length(vTemp); + + // if edge/sphere are intersecting + if (fDistEdgeToSphere < fSphereRadius ) + { + // transform in absolute space + dQuatTransform(mQuatCylinder,vPoint,vTemp); + dVector3Add(vTemp,vCylinderPos,vPoint); + + // calculate collision normal + dVector3 vNormal;// = dVector3(vPoint - vSpherePos); + dVector3Subtract(vPoint,vSpherePos,vNormal); + + // and make it unit vector + dNormalize3(vNormal); + + // calculate penetration depth + dReal fDepth = fSphereRadius - fDistEdgeToSphere; + + if (nContacts < (flags & NUMC_MASK)) + { + dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip ); + Contact->depth = fDepth; + dVector3Copy(vNormal,Contact->normal); + dVector3Copy(vPoint,Contact->pos); + Contact->g1 = gCylinder; + Contact->g2 = gSphere; + nContacts++; + } + + return nContacts; + } + } + } + } + + return nContacts; +} + + diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_trimesh.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_trimesh.cpp new file mode 100644 index 00000000..66775d4b --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_cylinder_trimesh.cpp @@ -0,0 +1,1041 @@ +/************************************************************************* +* * +* Open Dynamics Engine, Copyright (C) 2001-2003 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. * +* * +*************************************************************************/ + +/* + * Cylinder-trimesh collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + + // 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 + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + + +#define MAX_REAL dInfinity +static const int nCYLINDER_AXIS = 2; +static const int nCYLINDER_CIRCLE_SEGMENTS = 8; +static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12; +static const int gMaxLocalContacts = 32; + +#define OPTIMIZE_CONTACTS 1 + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +typedef struct _sCylinderTrimeshColliderData +{ + // cylinder data + dMatrix3 mCylinderRot; + dQuaternion qCylinderRot; + dQuaternion qInvCylinderRot; + dVector3 vCylinderPos; + dVector3 vCylinderAxis; + dReal fCylinderRadius; + dReal fCylinderSize; + dVector3 avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; + + // mesh data + dQuaternion qTrimeshRot; + dQuaternion qInvTrimeshRot; + dMatrix3 mTrimeshRot; + dVector3 vTrimeshPos; + + // global collider data + dVector3 vBestPoint; + dReal fBestDepth; + dReal fBestCenter; + dReal fBestrt; + int iBestAxis; + dVector3 vContactNormal; + dVector3 vNormal; + dVector3 vE0; + dVector3 vE1; + dVector3 vE2; + + // ODE stuff + dGeomID gCylinder; + dxTriMesh* gTrimesh; + dContactGeom* gContact; + int iFlags; + int iSkip; + int nContacts;// = 0; + sLocalContactData gLocalContacts[gMaxLocalContacts]; +} sCylinderTrimeshColliderData; + +// Short type name +typedef sCylinderTrimeshColliderData sData; + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + dVector3Subtract(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + dVector3Subtract(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +inline void _OptimizeLocalContacts(sData& cData) +{ + int nContacts = cData.nContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j])) + { + // If they are seem to be the samed then filtered + // out the least penetrate one + if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i])) + { + cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + cData.gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} + +inline int _ProcessLocalContacts(sData& cData) +{ + if (cData.nContacts == 0) + { + return 0; + } + +#ifdef OPTIMIZE_CONTACTS + if (cData.nContacts > 1) + { + // Can be optimized... + _OptimizeLocalContacts(cData); + } +#endif + + int iContact = 0; + dContactGeom* Contact = 0; + + int nFinalContact = 0; + + for (iContact = 0; iContact < cData.nContacts; iContact ++) + { + if (1 == cData.gLocalContacts[iContact].nFlags) + { + // eric added - dont go over our contact limit + if (iContact >= (cData.iFlags & 0x0ffff)) + break; + Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip); + Contact->depth = cData.gLocalContacts[iContact].fDepth; + dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal); + dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos); + Contact->g1 = cData.gCylinder; + Contact->g2 = cData.gTrimesh; + dVector3Inv(Contact->normal); + + nFinalContact++; + } + } + // debug + //if (nFinalContact != cData.nContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact); + //} + + return nFinalContact; +} + + +bool _cldTestAxis(sData& cData, + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3& vAxis, + int iAxis, + bool bNoFlip = false) +{ + + // calculate length of separating axis vector + dReal fL = dVector3Length(vAxis); + // if not long enough + if ( fL < 1e-5f ) + { + // do nothing + return true; + } + + // otherwise normalize it + vAxis[0] /= fL; + vAxis[1] /= fL; + vAxis[2] /= fL; + + dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis); + // project capsule on vAxis + dReal frc; + + if (fdot1 > REAL(1.0) ) + { + fdot1 = REAL(1.0); + frc = dFabs(cData.fCylinderSize* REAL(0.5)); + } + else + { + frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1) + + cData.fCylinderRadius * dFabs(REAL(1.0)-(fdot1*fdot1)); + } + + dVector3 vV0; + dVector3Subtract(v0,cData.vCylinderPos,vV0); + dVector3 vV1; + dVector3Subtract(v1,cData.vCylinderPos,vV1); + dVector3 vV2; + dVector3Subtract(v2,cData.vCylinderPos,vV2); + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dVector3Dot( vV0 , vAxis ); + afv[1] = dVector3Dot( vV1 , vAxis ); + afv[2] = dVector3Dot( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = -MAX_REAL; + + // for each vertex + for(int i = 0; i < 3; i++) + { + // find minimum + if (afv[i]fMax) + { + fMax = afv[i]; + } + } + + // find capsule's center of interval on axis + dReal fCenter = (fMin+fMax)* REAL(0.5); + // calculate triangles halfinterval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > (frc+fTriangleRadius) ) + { + // exit, we have no intersection + return false; + } + + // calculate depth + dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) ); + + // if greater then best found so far + if ( fDepth < cData.fBestDepth ) + { + // remember depth + cData.fBestDepth = fDepth; + cData.fBestCenter = fCenter; + cData.fBestrt = frc; + dVector3Copy(vAxis,cData.vContactNormal); + cData.iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if ( fCenter< REAL(0.0) && !bNoFlip) + { + dVector3Inv(cData.vContactNormal); + cData.fBestCenter = -fCenter; + } + } + + return true; +} + +// intersection test between edge and circle +bool _cldTestCircleToEdgeAxis(sData& cData, + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) +{ + // calculate direction of edge + dVector3 vkl; + dVector3Subtract( vVx1 , vVx0 , vkl); + dNormalize3(vkl); + // starting point of edge + dVector3 vol; + dVector3Copy(vVx0,vol); + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2)<1e-5f) + { + // this can't be separating axis, because edge is parallel to circle plane + return true; + } + + // find point of intersection between edge line and circle plane + dVector3 vTemp; + dVector3Subtract(vCenterPoint,vol,vTemp); + dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1); + dVector3 vpnt;// = vol + vkl * (fdot1/fdot2); + vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2; + vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2; + vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2; + + // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt) + dVector3 vTangent; + dVector3Subtract(vCenterPoint,vpnt,vTemp); + dVector3Cross(vTemp,vCylinderAxis1,vTangent); + + // find vector orthogonal both to tangent and edge direction + dVector3 vAxis; + dVector3Cross(vTangent,vkl,vAxis); + + // use that vector as separating axis + return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis ); +} + +// helper for less key strokes +// r = ( (v1 - v2) cross v3 ) cross v3 +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + dVector3Subtract(v1,v2,t1); + dVector3Cross(t1,v3,t2); + dVector3Cross(t2,v3,r); +} + +bool _cldTestSeparatingAxes(sData& cData, + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2) +{ + + // calculate edge vectors + dVector3Subtract(v1 ,v0 , cData.vE0); + // cData.vE1 has been calculated before -> so save some cycles here + dVector3Subtract(v0 ,v2 , cData.vE2); + + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); + vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); + vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); + + dVector3 vCp1; + vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); + vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); + vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); + + // reset best axis + cData.iBestAxis = 0; + dVector3 vAxis; + + // axis cData.vNormal + //vAxis = -cData.vNormal; + vAxis[0] = -cData.vNormal[0]; + vAxis[1] = -cData.vNormal[1]; + vAxis[2] = -cData.vNormal[2]; + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true)) + { + return false; + } + + // axis CxE0 + // vAxis = ( cData.vCylinderAxis cross cData.vE0 ); + dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2)) + { + return false; + } + + // axis CxE1 + // vAxis = ( cData.vCylinderAxis cross cData.vE1 ); + dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3)) + { + return false; + } + + // axis CxE2 + // vAxis = ( cData.vCylinderAxis cross cData.vE2 ); + dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis); + if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4)) + { + return false; + } + + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; + _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11)) + { + return false; + } + + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; + _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12)) + { + return false; + } + + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; + _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13)) + { + return FALSE; + } + + // test cylinder axis + // vAxis = cData.vCylinderAxis; + dVector3Copy(cData.vCylinderAxis , vAxis); + if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14)) + { + return false; + } + + // Test top and bottom circle ring of cylinder for separation + dVector3 vccATop; + vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); + vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); + vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); + + dVector3 vccABottom; + vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); + vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); + vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); + + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20)) + { + return false; + } + + return true; +} + +bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) +{ + // translate cylinder + dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal); + dVector3 vN2; + vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp; + vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp; + vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp; + + fTemp = dVector3Length(vN2); + if (fTemp < 1e-5) + { + return false; + } + + // Normalize it + vN2[0] /= fTemp; + vN2[1] /= fTemp; + vN2[2] /= fTemp; + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius; + vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius; + vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint0[1] = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint0[2] = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint1[1] = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); + vCEdgePoint1[2] = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); + + // transform cylinder edge points into triangle space + vCEdgePoint0[0] -= v0[0]; + vCEdgePoint0[1] -= v0[1]; + vCEdgePoint0[2] -= v0[2]; + + vCEdgePoint1[0] -= v0[0]; + vCEdgePoint1[1] -= v0[1]; + vCEdgePoint1[2] -= v0[2]; + + dVector4 plPlane; + dVector3 vPlaneNormal; + + // triangle plane + //plPlane = Plane4f( -cData.vNormal, 0); + vPlaneNormal[0] = -cData.vNormal[0]; + vPlaneNormal[1] = -cData.vNormal[1]; + vPlaneNormal[2] = -cData.vNormal[2]; + dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 0 + //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), 1e-5f); + dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal); + dConstructPlane(vPlaneNormal,1e-5f,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 1 + //dVector3 vTemp = ( cData.vNormal cross cData.vE1 ); + dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal); + fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - 1e-5; + //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-1e-5f)); + dConstructPlane(vPlaneNormal,-fTemp,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 2 + // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), 1e-5f); + dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal); + dConstructPlane(vPlaneNormal,1e-5f,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += v0[0]; + vCEdgePoint0[1] += v0[1]; + vCEdgePoint0[2] += v0[2]; + + vCEdgePoint1[0] += v0[0]; + vCEdgePoint1[1] += v0[1]; + vCEdgePoint1[2] += v0[2]; + + // calculate depths for both contact points + dVector3 vTemp; + dVector3Subtract(vCEdgePoint0,cData.vCylinderPos, vTemp); + dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; + dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp); + dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; + + dReal fDepth0 = cData.fBestDepth - (fRestDepth0); + dReal fDepth1 = cData.fBestDepth - (fRestDepth1); + + // clamp depths to zero + if(fDepth0 < REAL(0.0) ) + { + fDepth0 = REAL(0.0); + } + + if(fDepth1 REAL(0.0)) + { + // get top circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + dVector3 vTemp; + dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot); + // transform triangle points to space of cylinder circle + for(int i=0; i<3; i++) + { + dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); + dQuatTransform(cData.qInvCylinderRot,vTemp,avPoints[i]); + } + + int iTmpCounter1 = 0; + int iTmpCounter2 = 0; + dVector4 plPlane; + + // plane of cylinder that contains circle for intersection + //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); + dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); + dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); + + // Body of base circle of Cylinder + int nCircleSegment = 0; + for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++) + { + dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane); + + if (0 == (nCircleSegment % 2)) + { + dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); + } + else + { + dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); + } + + dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + int i = 0; + if (nCircleSegment %2) + { + for( i=0; i REAL(0.0)) + { + cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; + dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); + dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); + cData.gLocalContacts[cData.nContacts].nFlags = 1; + cData.nContacts++; + } + } + } + else + { + for( i=0; i REAL(0.0)) + { + cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; + dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); + dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); + cData.gLocalContacts[cData.nContacts].nFlags = 1; + cData.nContacts++; + } + } + } +} + +void TestOneTriangleVsCylinder( sData& cData, + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + const bool bDoubleSided) +{ + + // calculate triangle normal + dVector3Subtract( v2 , v1 ,cData.vE1); + dVector3 vTemp; + dVector3Subtract( v0 , v1 ,vTemp); + dVector3Cross(cData.vE1 , vTemp , cData.vNormal ); + + dNormalize3( cData.vNormal); + + // create plane from triangle + //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); + dReal plDistance = -dVector3Dot(v0, cData.vNormal); + dVector4 plTrianglePlane; + dConstructPlane( cData.vNormal,plDistance,plTrianglePlane); + + // calculate sphere distance to plane + dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane); + + // Sphere must be over positive side of triangle + if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + dVector3 vPnt1; + dVector3 vPnt2; + + if (fDistanceCylinderCenterToPlane < REAL(0.0) ) + { + // flip it + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt2); + dVector3Copy(v2 , vPnt1); + } + else + { + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt1); + dVector3Copy(v2 , vPnt2); + } + + cData.fBestDepth = MAX_REAL; + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( cData.iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + dIASSERT(false); + // do nothing + return; + } + + dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis ); + + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2)) + { + return; + } + } + else + { + _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2); + } + +} + +void _InitCylinderTrimeshData(sData& cData) +{ + // get cylinder information + // Rotation + const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); + dMatrix3Copy(pRotCyc,cData.mCylinderRot); + dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot); + + // Position + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); + dVector3Copy(*pPosCyc,cData.vCylinderPos); + // Cylinder axis + dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); + // get cylinder radius and size + dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); + + // get trimesh position and orientation + const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh); + dMatrix3Copy(pRotTris,cData.mTrimeshRot); + dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot); + + // Position + const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh); + dVector3Copy(*pPosTris,cData.vTrimeshPos); + + + // calculate basic angle for 8-gon + dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS; + // calculate angle increment + dReal fAngleIncrement = fAngle*REAL(2.0); + + // calculate plane normals + // axis dependant code + for(int i=0; i_OBBCollider; + + Point cCenter(cData.vCylinderPos[0],cData.vCylinderPos[1],cData.vCylinderPos[2]); + + Point cExtents(cData.fCylinderRadius,cData.fCylinderRadius,cData.fCylinderRadius); + cExtents[nCYLINDER_AXIS] = cData.fCylinderSize * REAL(0.5); + + Matrix3x3 obbRot; + + obbRot[0][0] = cData.mCylinderRot[0]; + obbRot[1][0] = cData.mCylinderRot[1]; + obbRot[2][0] = cData.mCylinderRot[2]; + + obbRot[0][1] = cData.mCylinderRot[4]; + obbRot[1][1] = cData.mCylinderRot[5]; + obbRot[2][1] = cData.mCylinderRot[6]; + + obbRot[0][2] = cData.mCylinderRot[8]; + obbRot[1][2] = cData.mCylinderRot[9]; + obbRot[2][2] = cData.mCylinderRot[10]; + + OBB obbCCylinder(cCenter,cExtents,obbRot); + + Matrix4x4 CCylinderMatrix; + MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CCylinderMatrix); + + Matrix4x4 MeshMatrix; + MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix); + + // TC results + if (cData.gTrimesh->doBoxTC) + { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++) + { + if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder) + { + BoxTC = &cData.gTrimesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC) + { + cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1]; + BoxTC->Geom = cData.gCylinder; + BoxTC->FatCoeff = REAL(1.0); + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCCylinder, cData.gTrimesh->Data->BVTree, null, &MeshMatrix); + } + else + { + Collider.SetTemporalCoherence(false); + //Collider.Collide(dxTriMesh::defaultBoxCache, obbCCylinder, cData.gTrimesh->Data->BVTree, null,&MeshMatrix); + Collider.Collide(cData.gTrimesh->boxCache, obbCCylinder, cData.gTrimesh->Data->BVTree, null,&MeshMatrix); + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + + if (TriCount != 0) + { + if (cData.gTrimesh->ArrayCallback != null) + { + cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount); + } + + //int OutTriCount = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) + { + break; + } + + const int& Triint = Triangles[i]; + if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv); + + // test this triangle + TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false); + } + } + + return _ProcessLocalContacts(cData); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_kernel.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_kernel.cpp new file mode 100644 index 00000000..b16d4d81 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_kernel.cpp @@ -0,0 +1,653 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + +core collision functions and data structures, plus part of the public API +for geometry objects + +*/ + +#include "ode/ode_common.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_objects.h" +#include "ode/ode_collision_kernel.h" +#include "ode/ode_collision_util.h" +#include "ode/ode_collision_std.h" +#include "ode/ode_collision_transform.h" +#include "ode/ode_collision_trimesh_internal.h" + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +//**************************************************************************** +// helper functions for dCollide()ing a space with another geom + +// this struct records the parameters passed to dCollideSpaceGeom() + +struct SpaceGeomColliderData { + int flags; // space left in contacts array + dContactGeom *contact; + int skip; +}; + + +static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2) +{ + SpaceGeomColliderData *d = (SpaceGeomColliderData*) data; + if (d->flags & NUMC_MASK) { + int n = dCollide (o1,o2,d->flags,d->contact,d->skip); + d->contact = CONTACT (d->contact,d->skip*n); + d->flags -= n; + } +} + + +static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + SpaceGeomColliderData data; + data.flags = flags; + data.contact = contact; + data.skip = skip; + dSpaceCollide2 (o1,o2,&data,&space_geom_collider); + return (flags & NUMC_MASK) - (data.flags & NUMC_MASK); +} + +//**************************************************************************** +// dispatcher for the N^2 collider functions + +// function pointers and modes for n^2 class collider functions + +struct dColliderEntry { + dColliderFn *fn; // collider function, 0 = no function available + int reverse; // 1 = reverse o1 and o2 +}; +static dColliderEntry colliders[dGeomNumClasses][dGeomNumClasses]; +static int colliders_initialized = 0; + + +// setCollider() will refuse to write over a collider entry once it has +// been written. + +static void setCollider (int i, int j, dColliderFn *fn) +{ + if (colliders[i][j].fn == 0) { + colliders[i][j].fn = fn; + colliders[i][j].reverse = 0; + } + if (colliders[j][i].fn == 0) { + colliders[j][i].fn = fn; + colliders[j][i].reverse = 1; + } +} + + +static void setAllColliders (int i, dColliderFn *fn) +{ + for (int j=0; jtype >= 0 && o1->type < dGeomNumClasses,"bad o1 class number"); + dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number"); + + // no contacts if both geoms are the same + if (o1 == o2) return 0; + + // no contacts if both geoms on the same body, and the body is not 0 + if (o1->body == o2->body && o1->body) return 0; + + dColliderEntry *ce = &colliders[o1->type][o2->type]; + int count = 0; + if (ce->fn) { + if (ce->reverse) { + count = (*ce->fn) (o2,o1,flags,contact,skip); + for (int i=0; inormal[0] = -c->normal[0]; + c->normal[1] = -c->normal[1]; + c->normal[2] = -c->normal[2]; + dxGeom *tmp = c->g1; + c->g1 = c->g2; + c->g2 = tmp; + } + } + else { + count = (*ce->fn) (o1,o2,flags,contact,skip); + } + } + return count; +} + +//**************************************************************************** +// dxGeom + +dxGeom::dxGeom (dSpaceID _space, int is_placeable) +{ + initColliders(); + + // setup body vars. invalid type of -1 must be changed by the constructor. + type = -1; + gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED; + if (is_placeable) gflags |= GEOM_PLACEABLE; + data = 0; + body = 0; + body_next = 0; + if (is_placeable) { + dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR)); + pos = pr->pos; + R = pr->R; + dSetZero (pos,4); + dRSetIdentity (R); + } + else { + pos = 0; + R = 0; + } + + // setup space vars + next = 0; + tome = 0; + parent_space = 0; + dSetZero (aabb,6); + category_bits = ~0; + collide_bits = ~0; + + // put this geom in a space if required + if (_space) dSpaceAdd (_space,this); +} + + +dxGeom::~dxGeom() +{ + if (parent_space) dSpaceRemove (parent_space,this); + if ((gflags & GEOM_PLACEABLE) && !body) dFree (pos,sizeof(dxPosR)); + bodyRemove(); +} + + +int dxGeom::AABBTest (dxGeom *o, dReal aabb[6]) +{ + return 1; +} + + +void dxGeom::bodyRemove() +{ + if (body) { + // delete this geom from body list + dxGeom **last = &body->geom, *g = body->geom; + while (g) { + if (g == this) { + *last = g->body_next; + break; + } + last = &g->body_next; + g = g->body_next; + } + body = 0; + body_next = 0; + } +} + +//**************************************************************************** +// misc + +dxGeom *dGeomGetBodyNext (dxGeom *geom) +{ + return geom->body_next; +} + +//**************************************************************************** +// public API for geometry objects + +#define CHECK_NOT_LOCKED(space) \ + dUASSERT (!(space && space->lock_count), \ + "invalid operation for geom in locked space"); + + +void dGeomDestroy (dxGeom *g) +{ + dAASSERT (g); + delete g; +} + + +void dGeomSetData (dxGeom *g, void *data) +{ + dAASSERT (g); + g->data = data; +} + + +void *dGeomGetData (dxGeom *g) +{ + dAASSERT (g); + return g->data; +} + + +void dGeomSetBody (dxGeom *g, dxBody *b) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + + if (b) { + if (!g->body) dFree (g->pos,sizeof(dxPosR)); + g->pos = b->pos; + g->R = b->R; + dGeomMoved (g); + if (g->body != b) { + g->bodyRemove(); + g->bodyAdd (b); + } + } + else { + if (g->body) { + dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR)); + g->pos = pr->pos; + g->R = pr->R; + memcpy (g->pos,g->body->pos,sizeof(dVector3)); + memcpy (g->R,g->body->R,sizeof(dMatrix3)); + g->bodyRemove(); + } + // dGeomMoved() should not be called if the body is being set to 0, as the + // new position of the geom is set to the old position of the body, so the + // effective position of the geom remains unchanged. + } +} + + +dBodyID dGeomGetBody (dxGeom *g) +{ + dAASSERT (g); + return g->body; +} + + +void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetPosition (g->body,x,y,z); + } + else { + g->pos[0] = x; + g->pos[1] = y; + g->pos[2] = z; + dGeomMoved (g); + } +} + + +void dGeomSetRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g && R); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetRotation (g->body,R); + } + else { + memcpy (g->R,R,sizeof(dMatrix3)); + dGeomMoved (g); + } +} + + +void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat) +{ + dAASSERT (g && quat); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + CHECK_NOT_LOCKED (g->parent_space); + if (g->body) { + // this will call dGeomMoved (g), so we don't have to + dBodySetQuaternion (g->body,quat); + } + else { + dQtoR (quat, g->R); + dGeomMoved (g); + } +} + + +const dReal * dGeomGetPosition (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + return g->pos; +} + +void dGeomGetRelPointPos (dGeomID g, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,g->R,prel); + result[0] = p[0] + g->pos[0]; + result[1] = p[1] + g->pos[1]; + result[2] = p[2] + g->pos[2]; +} + + +const dReal * dGeomGetRotation (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + return g->R; +} + + +void dGeomGetQuaternion (dxGeom *g, dQuaternion quat) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + if (g->body) { + const dReal * body_quat = dBodyGetQuaternion (g->body); + quat[0] = body_quat[0]; + quat[1] = body_quat[1]; + quat[2] = body_quat[2]; + quat[3] = body_quat[3]; + } + else { + dRtoQ (g->R, quat); + } +} + + +void dGeomGetAABB (dxGeom *g, dReal aabb[6]) +{ + dAASSERT (g); + dAASSERT (aabb); + g->recomputeAABB(); + memcpy (aabb,g->aabb,6 * sizeof(dReal)); +} + + +int dGeomIsSpace (dxGeom *g) +{ + dAASSERT (g); + return IS_SPACE(g); +} + + +dSpaceID dGeomGetSpace (dxGeom *g) +{ + dAASSERT (g); + return g->parent_space; +} + + +int dGeomGetClass (dxGeom *g) +{ + dAASSERT (g); + return g->type; +} + + +void dGeomSetCategoryBits (dxGeom *g, unsigned long bits) +{ + dAASSERT (g); + CHECK_NOT_LOCKED (g->parent_space); + g->category_bits = bits; +} + + +void dGeomSetCollideBits (dxGeom *g, unsigned long bits) +{ + dAASSERT (g); + CHECK_NOT_LOCKED (g->parent_space); + g->collide_bits = bits; +} + + +unsigned long dGeomGetCategoryBits (dxGeom *g) +{ + dAASSERT (g); + return g->category_bits; +} + + +unsigned long dGeomGetCollideBits (dxGeom *g) +{ + dAASSERT (g); + return g->collide_bits; +} + + +void dGeomEnable (dxGeom *g) +{ + dAASSERT (g); + g->gflags |= GEOM_ENABLED; +} + +void dGeomDisable (dxGeom *g) +{ + dAASSERT (g); + g->gflags &= ~GEOM_ENABLED; +} + +int dGeomIsEnabled (dxGeom *g) +{ + dAASSERT (g); + return (g->gflags & GEOM_ENABLED) != 0; +} + + +//**************************************************************************** +// C interface that lets the user make new classes. this interface is a lot +// more cumbersome than C++ subclassing, which is what is used internally +// in ODE. this API is mainly to support legacy code. + +static int num_user_classes = 0; +static dGeomClass user_classes [dMaxUserClasses]; + + +struct dxUserGeom : public dxGeom { + void *user_data; + + dxUserGeom (int class_num); + ~dxUserGeom(); + void computeAABB(); + int AABBTest (dxGeom *o, dReal aabb[6]); +}; + + +dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1) +{ + type = class_num; + int size = user_classes[type-dFirstUserClass].bytes; + user_data = dAlloc (size); + memset (user_data,0,size); +} + + +dxUserGeom::~dxUserGeom() +{ + dGeomClass *c = &user_classes[type-dFirstUserClass]; + if (c->dtor) c->dtor (this); + dFree (user_data,c->bytes); +} + + +void dxUserGeom::computeAABB() +{ + user_classes[type-dFirstUserClass].aabb (this,aabb); +} + + +int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6]) +{ + dGeomClass *c = &user_classes[type-dFirstUserClass]; + if (c->aabb_test) return c->aabb_test (this,o,aabb); + else return 1; +} + + +static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + // this generic collider function is called the first time that a user class + // tries to collide against something. it will find out the correct collider + // function and then set the colliders array so that the correct function is + // called directly the next time around. + + int t1 = o1->type; // note that o1 is a user geom + int t2 = o2->type; // o2 *may* be a user geom + + // find the collider function to use. if o1 does not know how to collide with + // o2, then o2 might know how to collide with o1 (provided that it is a user + // geom). + dColliderFn *fn = user_classes[t1-dFirstUserClass].collider (t2); + int reverse = 0; + if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) { + fn = user_classes[t2-dFirstUserClass].collider (t1); + reverse = 1; + } + + // set the colliders array so that the correct function is called directly + // the next time around. note that fn can be 0 here if no collider was found, + // which means that dCollide() will always return 0 for this case. + colliders[t1][t2].fn = fn; + colliders[t1][t2].reverse = reverse; + colliders[t2][t1].fn = fn; + colliders[t2][t1].reverse = !reverse; + + // now call the collider function indirectly through dCollide(), so that + // contact reversing is properly handled. + return dCollide (o1,o2,flags,contact,skip); +} + + +int dCreateGeomClass (const dGeomClass *c) +{ + dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class"); + + if (num_user_classes >= dMaxUserClasses) { + dDebug (0,"too many user classes, you must increase the limit and " + "recompile ODE"); + } + user_classes[num_user_classes] = *c; + int class_number = num_user_classes + dFirstUserClass; + initColliders(); + setAllColliders (class_number,&dCollideUserGeomWithGeom); + + num_user_classes++; + return class_number; +} + + +void * dGeomGetClassData (dxGeom *g) +{ + dUASSERT (g && g->type >= dFirstUserClass && + g->type <= dLastUserClass,"not a custom class"); + dxUserGeom *user = (dxUserGeom*) g; + return user->user_data; +} + + +dGeomID dCreateGeom (int classnum) +{ + dUASSERT (classnum >= dFirstUserClass && + classnum <= dLastUserClass,"not a custom class"); + return new dxUserGeom (classnum); +} + +//**************************************************************************** +// here is where we deallocate any memory that has been globally +// allocated, or free other global resources. + +void dCloseODE() +{ + colliders_initialized = 0; + num_user_classes = 0; +} + + diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_kernel.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_kernel.h new file mode 100644 index 00000000..d52f28e8 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_kernel.h @@ -0,0 +1,202 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + +internal data structures and functions for collision detection. + +*/ + +#ifndef _ODE_COLLISION_KERNEL_H_ +#define _ODE_COLLISION_KERNEL_H_ + +#include "ode/ode_common.h" +#include "ode/ode_contact.h" +#include "ode/ode_collision.h" +#include "ode/ode_objects_private.h" + +//**************************************************************************** +// constants and macros + +// mask for the number-of-contacts field in the dCollide() flags parameter +#define NUMC_MASK (0xffff) + +#define IS_SPACE(geom) \ + ((geom)->type >= dFirstSpaceClass && (geom)->type <= dLastSpaceClass) + +//**************************************************************************** +// geometry object base class + +// position vector and rotation matrix for geometry objects that are not +// connected to bodies. + +struct dxPosR { + dVector3 pos; + dMatrix3 R; +}; + + +// geom flags. +// +// GEOM_DIRTY means that the space data structures for this geom are +// potentially not up to date. NOTE THAT all space parents of a dirty geom +// are themselves dirty. this is an invariant that must be enforced. +// +// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date. +// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might +// recalculate its own AABB but does not know how to update the space data +// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY. +// the valid combinations are: 0, GEOM_DIRTY, GEOM_DIRTY|GEOM_AABB_BAD. + +enum { + GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown + GEOM_AABB_BAD = 2, // geom's AABB is not valid + GEOM_PLACEABLE = 4, // geom is placeable + GEOM_ENABLED = 8, // geom is enabled + + // Ray specific + RAY_FIRSTCONTACT = 0x10000, + RAY_BACKFACECULL = 0x20000, + RAY_CLOSEST_HIT = 0x40000 +}; + + +// geometry object base class. pos and R will either point to a separately +// allocated buffer (if body is 0 - pos points to the dxPosR object) or to +// the pos and R of the body (if body nonzero). +// a dGeomID is a pointer to this object. + +struct dxGeom : public dBase { + int type; // geom type number, set by subclass constructor + int gflags; // flags used by geom and space + void *data; // user-defined data pointer + dBodyID body; // dynamics body associated with this object (if any) + dxGeom *body_next; // next geom in body's linked list of associated geoms + dReal *pos; // pointer to object's position vector + dReal *R; // pointer to object's rotation matrix + + // information used by spaces + dxGeom *next; // next geom in linked list of geoms + dxGeom **tome; // linked list backpointer + dxSpace *parent_space;// the space this geom is contained in, 0 if none + dReal aabb[6]; // cached AABB for this space + unsigned long category_bits,collide_bits; + + dxGeom (dSpaceID _space, int is_placeable); + virtual ~dxGeom(); + + virtual void computeAABB()=0; + // compute the AABB for this object and put it in aabb. this function + // always performs a fresh computation, it does not inspect the + // GEOM_AABB_BAD flag. + + virtual int AABBTest (dxGeom *o, dReal aabb[6]); + // test whether the given AABB object intersects with this object, return + // 1=yes, 0=no. this is used as an early-exit test in the space collision + // functions. the default implementation returns 1, which is the correct + // behavior if no more detailed implementation can be provided. + + // utility functions + + // compute the AABB only if it is not current. this function manipulates + // the GEOM_AABB_BAD flag. + + void recomputeAABB() { + if (gflags & GEOM_AABB_BAD) { + computeAABB(); + gflags &= ~GEOM_AABB_BAD; + } + } + + // add and remove this geom from a linked list maintained by a space. + + void spaceAdd (dxGeom **first_ptr) { + next = *first_ptr; + tome = first_ptr; + if (*first_ptr) (*first_ptr)->tome = &next; + *first_ptr = this; + } + void spaceRemove() { + if (next) next->tome = tome; + *tome = next; + } + + // add and remove this geom from a linked list maintained by a body. + + void bodyAdd (dxBody *b) { + body = b; + body_next = b->geom; + b->geom = this; + } + void bodyRemove(); +}; + +//**************************************************************************** +// the base space class +// +// the contained geoms are divided into two kinds: clean and dirty. +// the clean geoms have not moved since they were put in the list, +// and their AABBs are valid. the dirty geoms have changed position, and +// their AABBs are may not be valid. the two types are distinguished by the +// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list. + +struct dxSpace : public dxGeom { + int count; // number of geoms in this space + dxGeom *first; // first geom in list + int cleanup; // cleanup mode, 1=destroy geoms on exit + + // cached state for getGeom() + int current_index; // only valid if current_geom != 0 + dxGeom *current_geom; // if 0 then there is no information + + // locking stuff. the space is locked when it is currently traversing its + // internal data structures, e.g. in collide() and collide2(). operations + // that modify the contents of the space are not permitted when the space + // is locked. + int lock_count; + + dxSpace (dSpaceID _space); + ~dxSpace(); + + void computeAABB(); + + void setCleanup (int mode); + int getCleanup(); + int query (dxGeom *geom); + int getNumGeoms(); + virtual dxGeom *getGeom (int i); + + virtual void add (dxGeom *); + virtual void remove (dxGeom *); + virtual void dirty (dxGeom *); + + virtual void cleanGeoms()=0; + // turn all dirty geoms into clean geoms by computing their AABBs and any + // other space data structures that are required. this should clear the + // GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms. + + virtual void collide (void *data, dNearCallback *callback)=0; + virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0; +}; + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_quadtreespace.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_quadtreespace.cpp new file mode 100644 index 00000000..435406e0 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_quadtreespace.cpp @@ -0,0 +1,583 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +// QuadTreeSpace by Erwin de Vries. + +#include "ode/ode_common.h" +#include "ode/ode_matrix.h" +#include "ode/ode_collision_space.h" +#include "ode/ode_collision.h" +#include "ode/ode_collision_kernel.h" + +#include "ode/ode_collision_space_internal.h" + + +#define AXIS0 0 +#define AXIS1 1 +#define UP 2 + +//#define DRAWBLOCKS + +const int SPLITAXIS = 2; +const int SPLITS = SPLITAXIS * SPLITAXIS; + +#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED + +class Block{ +public: + dReal MinX, MaxX; + dReal MinZ, MaxZ; + + dGeomID First; + int GeomCount; + + Block* Parent; + Block* Children; + + void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks); + + void Collide(void* UserData, dNearCallback* Callback); + void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback); + + void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback); + + void AddObject(dGeomID Object); + void DelObject(dGeomID Object); + void Traverse(dGeomID Object); + + bool Inside(const dReal* AABB); + + Block* GetBlock(const dReal* AABB); + Block* GetBlockChild(const dReal* AABB); +}; + + +#ifdef DRAWBLOCKS +#include "ode/ode_drawstuff.h" + +static void DrawBlock(Block* Block){ + dVector3 v[8]; + v[0][AXIS0] = Block->MinX; + v[0][UP] = REAL(-1.0); + v[0][AXIS1] = Block->MinZ; + + v[1][AXIS0] = Block->MinX; + v[1][UP] = REAL(-1.0); + v[1][AXIS1] = Block->MaxZ; + + v[2][AXIS0] = Block->MaxX; + v[2][UP] = REAL(-1.0); + v[2][AXIS1] = Block->MinZ; + + v[3][AXIS0] = Block->MaxX; + v[3][UP] = REAL(-1.0); + v[3][AXIS1] = Block->MaxZ; + + v[4][AXIS0] = Block->MinX; + v[4][UP] = REAL(1.0); + v[4][AXIS1] = Block->MinZ; + + v[5][AXIS0] = Block->MinX; + v[5][UP] = REAL(1.0); + v[5][AXIS1] = Block->MaxZ; + + v[6][AXIS0] = Block->MaxX; + v[6][UP] = REAL(1.0); + v[6][AXIS1] = Block->MinZ; + + v[7][AXIS0] = Block->MaxX; + v[7][UP] = REAL(1.0); + v[7][AXIS1] = Block->MaxZ; + + // Bottom + dsDrawLine(v[0], v[1]); + dsDrawLine(v[1], v[3]); + dsDrawLine(v[3], v[2]); + dsDrawLine(v[2], v[0]); + + // Top + dsDrawLine(v[4], v[5]); + dsDrawLine(v[5], v[7]); + dsDrawLine(v[7], v[6]); + dsDrawLine(v[6], v[4]); + + // Sides + dsDrawLine(v[0], v[4]); + dsDrawLine(v[1], v[5]); + dsDrawLine(v[2], v[6]); + dsDrawLine(v[3], v[7]); +} +#endif //DRAWBLOCKS + + +void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){ + GeomCount = 0; + First = 0; + + MinX = Center[AXIS0] - Extents[AXIS0]; + MaxX = Center[AXIS0] + Extents[AXIS0]; + + MinZ = Center[AXIS1] - Extents[AXIS1]; + MaxZ = Center[AXIS1] + Extents[AXIS1]; + + this->Parent = Parent; + if (Depth > 0){ + Children = Blocks; + Blocks += SPLITS; + + dVector3 ChildExtents; + ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS; + ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS; + ChildExtents[UP] = Extents[UP]; + + for (int i = 0; i < SPLITAXIS; i++){ + for (int j = 0; j < SPLITAXIS; j++){ + int Index = i * SPLITAXIS + j; + + dVector3 ChildCenter; + ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2); + ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2); + ChildCenter[UP] = Center[UP]; + + Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks); + } + } + } + else Children = 0; +} + +void Block::Collide(void* UserData, dNearCallback* Callback){ +#ifdef DRAWBLOCKS + DrawBlock(this); +#endif + // Collide the local list + dxGeom* g = First; + while (g){ + if (GEOM_ENABLED(g)){ + Collide(g, g->next, UserData, Callback); + } + g = g->next; + } + + // Recurse for children + if (Children){ + for (int i = 0; i < SPLITS; i++){ + if (Children[i].GeomCount <= 1){ // Early out + continue; + } + Children[i].Collide(UserData, Callback); + } + } +} + +void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){ +#ifdef DRAWBLOCKS + DrawBlock(this); +#endif + // Collide against local list + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next; + } + + // Collide against children + if (Children){ + for (int i = 0; i < SPLITS; i++){ + // Early out for empty blocks + if (Children[i].GeomCount == 0){ + continue; + } + + // Does the geom's AABB collide with the block? + // Dont do AABB tests for single geom blocks. + if (Children[i].GeomCount == 1 && Children[i].First){ + // + } + else if (true){ + if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX || + g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX || + g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ || + g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue; + } + Children[i].Collide(g1, Children[i].First, UserData, Callback); + } + } +} + +void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){ + // Collide against local list + dxGeom* g2 = First; + while (g2){ + if (GEOM_ENABLED(g2)){ + collideAABBs (g1, g2, UserData, Callback); + } + g2 = g2->next; + } +} + +void Block::AddObject(dGeomID Object){ + // Add the geom + Object->next = First; + First = Object; + Object->tome = (dxGeom**)this; + + // Now traverse upwards to tell that we have a geom + Block* Block = this; + do{ + Block->GeomCount++; + Block = Block->Parent; + } + while (Block); +} + +void Block::DelObject(dGeomID Object){ + // Del the geom + dxGeom* g = First; + dxGeom* Last = 0; + while (g){ + if (g == Object){ + if (Last){ + Last->next = g->next; + } + else First = g->next; + + break; + } + Last = g; + g = g->next; + } + + Object->tome = 0; + + // Now traverse upwards to tell that we have lost a geom + Block* Block = this; + do{ + Block->GeomCount--; + Block = Block->Parent; + } + while (Block); +} + +void Block::Traverse(dGeomID Object){ + Block* NewBlock = GetBlock(Object->aabb); + + if (NewBlock != this){ + // Remove the geom from the old block and add it to the new block. + // This could be more optimal, but the loss should be very small. + DelObject(Object); + NewBlock->AddObject(Object); + } +} + +bool Block::Inside(const dReal* AABB){ + return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ; +} + +Block* Block::GetBlock(const dReal* AABB){ + if (Inside(AABB)){ + return GetBlockChild(AABB); // Child or this will have a good block + } + else if (Parent){ + return Parent->GetBlock(AABB); // Parent has a good block + } + else return this; // We are at the root, so we have little choice +} + +Block* Block::GetBlockChild(const dReal* AABB){ + if (Children){ + for (int i = 0; i < SPLITS; i++){ + if (Children[i].Inside(AABB)){ + return Children[i].GetBlockChild(AABB); // Child will have good block + } + } + } + return this; // This is the best block +} + +//**************************************************************************** +// quadtree space + +struct dxQuadTreeSpace : public dxSpace{ + Block* Blocks; // Blocks[0] is the root + + dArray DirtyList; + + dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth); + ~dxQuadTreeSpace(); + + dxGeom* getGeom(int i); + + void add(dxGeom* g); + void remove(dxGeom* g); + void dirty(dxGeom* g); + + void computeAABB(); + + void cleanGeoms(); + void collide(void* UserData, dNearCallback* Callback); + void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback); + + // Temp data + Block* CurrentBlock; // Only used while enumerating + int* CurrentChild; // Only used while enumerating + int CurrentLevel; // Only used while enumerating + dxGeom* CurrentObject; // Only used while enumerating + int CurrentIndex; +}; + +dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){ + type = dQuadTreeSpaceClass; + + int BlockCount = 0; + for (int i = 0; i <= Depth; i++){ + BlockCount += (int)pow(dReal(SPLITS), i); + } + + Blocks = (Block*)dAlloc(BlockCount * sizeof(Block)); + Block* Blocks = this->Blocks + 1; // This pointer gets modified! + + this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks); + + CurrentBlock = 0; + CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int)); + CurrentLevel = 0; + CurrentObject = 0; + CurrentIndex = -1; + + // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + +dxQuadTreeSpace::~dxQuadTreeSpace(){ + int Depth = 0; + Block* Current = &Blocks[0]; + while (Current){ + Depth++; + Current = Current->Children; + } + + int BlockCount = 0; + for (int i = 0; i < Depth; i++){ + BlockCount += (int)pow(dReal(SPLITS), i); + } + + dFree(Blocks, BlockCount * sizeof(Block)); + dFree(CurrentChild, (Depth + 1) * sizeof(int)); +} + +dxGeom* dxQuadTreeSpace::getGeom(int Index){ + dUASSERT(Index >= 0 && Index < count, "index out of range"); + + //@@@ + dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented"); + + return 0; + + // This doesnt work + + /*if (CurrentIndex == Index){ + // Loop through all objects in the local list +CHILDRECURSE: + if (CurrentObject){ + dGeomID g = CurrentObject; + CurrentObject = CurrentObject->next; + CurrentIndex++; + +#ifdef DRAWBLOCKS + DrawBlock(CurrentBlock); +#endif //DRAWBLOCKS + return g; + } + else{ + // Now lets loop through our children. Starting at index 0. + if (CurrentBlock->Children){ + CurrentChild[CurrentLevel] = 0; +PARENTRECURSE: + for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){ + if (CurrentBlock->Children[i].GeomCount == 0){ + continue; + } + CurrentBlock = &CurrentBlock->Children[i]; + CurrentObject = CurrentBlock->First; + + i++; + + CurrentLevel++; + goto CHILDRECURSE; + } + } + } + + // Now lets go back to the parent so it can continue processing its other children. + if (CurrentBlock->Parent){ + CurrentBlock = CurrentBlock->Parent; + CurrentLevel--; + goto PARENTRECURSE; + } + } + else{ + CurrentBlock = &Blocks[0]; + CurrentLevel = 0; + CurrentObject = CurrentObject; + CurrentIndex = 0; + + // Other states are already set + CurrentObject = CurrentBlock->First; + } + + + if (current_geom && current_index == Index - 1){ + //current_geom = current_geom->next; // next + current_index = Index; + return current_geom; + } + else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow + getGeom(i); + }*/ + + return 0; +} + +void dxQuadTreeSpace::add(dxGeom* g){ + CHECK_NOT_LOCKED (this); + dAASSERT(g); + dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space"); + + g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + DirtyList.push(g); + + // add + g->parent_space = this; + Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block + count++; + + // enumerator has been invalidated + current_geom = 0; + + dGeomMoved(this); +} + +void dxQuadTreeSpace::remove(dxGeom* g){ + CHECK_NOT_LOCKED(this); + dAASSERT(g); + dUASSERT(g->parent_space == this,"object is not in this space"); + + // remove + ((Block*)g->tome)->DelObject(g); + count--; + + for (int i = 0; i < DirtyList.size(); i++){ + if (DirtyList[i] == g){ + DirtyList.remove(i); + break; + } + } + + // safeguard + g->next = 0; + g->tome = 0; + g->parent_space = 0; + + // enumerator has been invalidated + current_geom = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved(this); +} + +void dxQuadTreeSpace::dirty(dxGeom* g){ + DirtyList.push(g); +} + +void dxQuadTreeSpace::computeAABB(){ + // +} + +void dxQuadTreeSpace::cleanGeoms(){ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + + for (int i = 0; i < DirtyList.size(); i++){ + dxGeom* g = DirtyList[i]; + if (IS_SPACE(g)){ + ((dxSpace*)g)->cleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + + ((Block*)g->tome)->Traverse(g); + } + DirtyList.setSize(0); + + lock_count--; +} + +void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){ + dAASSERT(Callback); + + lock_count++; + cleanGeoms(); + + Blocks[0].Collide(UserData, Callback); + + lock_count--; +} + +void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){ + dAASSERT(g1 && Callback); + + lock_count++; + cleanGeoms(); + g1->recomputeAABB(); + + if (g1->parent_space == this){ + // The block the geom is in + Block* CurrentBlock = (Block*)g1->tome; + + // Collide against block and its children + CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback); + + // Collide against parents + while (true){ + CurrentBlock = CurrentBlock->Parent; + if (!CurrentBlock){ + break; + } + CurrentBlock->CollideLocal(g1, UserData, Callback); + } + } + else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback); + + lock_count--; +} + +dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){ + return new dxQuadTreeSpace(space, Center, Extents, Depth); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_sapspace.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_sapspace.cpp new file mode 100644 index 00000000..9e0081f3 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_sapspace.cpp @@ -0,0 +1,487 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + * Sweep and Prune adaptation/tweaks for ODE by Aras Pranckevicius. + * Original code: + * OPCODE - Optimized Collision Detection + * Copyright (C) 2001 Pierre Terdiman + * Homepage: http://www.codercorner.com/Opcode.htm + * + * This version does complete radix sort, not "classical" SAP. So, we + * have no temporal coherence, but are able to handle any movement + * velocities equally well. + */ + + // 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 + +#include "ode/ode_common.h" +#include "ode/ode_matrix.h" +#include "ode/ode_collision_space.h" +#include "ode/ode_collision.h" +#include "ode/ode_collision_kernel.h" + +#include "ode/ode_collision_space_internal.h" + +#include "ode/ode_Opcode.h" + + +// -------------------------------------------------------------------------- +// Box pruning code +// -------------------------------------------------------------------------- + +// InsertionSort has better coherence, RadixSort is better for one-shot queries. +#define PRUNING_SORTER RadixSort +//#define PRUNING_SORTER InsertionSort + +// Global pruning sorter for coherence +static PRUNING_SORTER* gCompletePruningSorter = 0; + +static inline PRUNING_SORTER* get_pruning_sorter() +{ + if( !gCompletePruningSorter ) + gCompletePruningSorter = new PRUNING_SORTER; + return gCompletePruningSorter; +} +// TBD: who should call this? +void release_pruning_sorters() +{ + if( gCompletePruningSorter ) { + delete gCompletePruningSorter; + gCompletePruningSorter = 0; + } +} + +/** + * Complete box pruning. + * Returns a list of overlapping pairs of boxes, each box of the pair + * belongs to the same set. + * NOTE: code uses floats instead of dReals because Opcode's radix sort + * is optimized for floats :) + * + * @param count [in] number of boxes. + * @param geoms [in] geoms of boxes. + * @param pairs [out] array of overlapping pairs. + * @param axes [in] projection order (0,2,1 is often best). + * @return true If success. + */ +static bool complete_box_pruning( int count, const dxGeom** geoms, Pairs& pairs, const Axes& axes ) +{ + // Checks + if (!count || !geoms) + return false; + + // Catch axes + udword Axis0 = axes.mAxis0; + udword Axis1 = axes.mAxis1; + udword Axis2 = axes.mAxis2; + + // Axis indices into geom's aabb are: min=idx, max=idx+1 + udword ax0idx = Axis0*2; + udword ax1idx = Axis1*2; + udword ax2idx = Axis2*2; + + // Allocate some temporary data + // TBD: persistent allocation between queries? + float* PosList = new float[count+1]; + + // 1) Build main list using the primary axis + for( int i = 0; i < count; ++i ) + PosList[i] = (float)geoms[i]->aabb[ax0idx]; + PosList[count++] = MAX_FLOAT; + + // 2) Sort the list + PRUNING_SORTER* RS = get_pruning_sorter(); + const udword* Sorted = RS->Sort(PosList, count).GetRanks(); + + // 3) Prune the list + const udword* const LastSorted = &Sorted[count]; + const udword* RunningAddress = Sorted; + udword Index0, Index1; + while( RunningAddress < LastSorted && Sorted < LastSorted ) { + Index0 = *Sorted++; + + while( PosList[*RunningAddress++] < PosList[Index0] ) { + // empty, the loop just advances RunningAddress + } + + if( RunningAddress < LastSorted ) { + const udword* RunningAddress2 = RunningAddress; + + float idx0ax0max = (float)geoms[Index0]->aabb[ax0idx+1]; + float idx0ax1max = (float)geoms[Index0]->aabb[ax1idx+1]; + float idx0ax2max = (float)geoms[Index0]->aabb[ax2idx+1]; + while( PosList[Index1 = *RunningAddress2++] <= idx0ax0max ) { +// if(Index0!=Index1) +// { + const dReal* aabb0 = geoms[Index0]->aabb; + const dReal* aabb1 = geoms[Index1]->aabb; + if( idx0ax1max < (float)aabb1[ax1idx] || (float)aabb1[ax1idx+1] < (float)aabb0[ax1idx] ) { + // no intersection + } else { + if( idx0ax2max < (float)aabb1[ax2idx] || (float)aabb1[ax2idx+1] < (float)aabb0[ax2idx] ) { + // no intersection + } else { + // yes! :) + pairs.AddPair( Index0, Index1 ); + } + } +// } + } + } + } + DELETEARRAY(PosList); + return true; +} + + +// -------------------------------------------------------------------------- +// SAP space code +// -------------------------------------------------------------------------- + +#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED) + +/* + * A bit of repetitive work - similar to collideAABBs, but doesn't check + * if AABBs intersect (because SAP returns pairs with overlapping AABBs). + */ +static void collideGeomsNoAABBs( dxGeom *g1, dxGeom *g2, void *data, dNearCallback *callback ) +{ + dIASSERT( (g1->gflags & GEOM_AABB_BAD)==0 ); + dIASSERT( (g2->gflags & GEOM_AABB_BAD)==0 ); + + // no contacts if both geoms on the same body, and the body is not 0 + if (g1->body == g2->body && g1->body) return; + + // test if the category and collide bitfields match + if ( ((g1->category_bits & g2->collide_bits) || + (g2->category_bits & g1->collide_bits)) == 0) { + return; + } + + dReal *bounds1 = g1->aabb; + dReal *bounds2 = g2->aabb; + + // check if either object is able to prove that it doesn't intersect the + // AABB of the other + if (g1->AABBTest (g2,bounds2) == 0) return; + if (g2->AABBTest (g1,bounds1) == 0) return; + + // the objects might actually intersect - call the space callback function + callback (data,g1,g2); +}; + + +// -------------------------------------------------------------------------- +// SAP space + +// Kind of HACK: +// We abuse 'next' and 'tome' members of dxGeom to store indices +// into dirty/geom lists. +#define GEOM_SET_DIRTY_IDX(g,idx) { g->next = (dxGeom*)(size_t)(idx); } +#define GEOM_SET_GEOM_IDX(g,idx) { g->tome = (dxGeom**)(size_t)(idx); } +#define GEOM_GET_DIRTY_IDX(g) ((size_t)g->next) +#define GEOM_GET_GEOM_IDX(g) ((size_t)g->tome) +#define GEOM_INVALID_IDX (-1) + + +struct dxSAPSpace : public dxSpace { + typedef dArray TGeomPtrArray; + + // We have two lists (arrays of pointers) to dirty and clean + // geoms. Each geom knows it's index into the corresponding list + // (see macros above). + TGeomPtrArray DirtyList; // dirty geoms + TGeomPtrArray GeomList; // clean geoms + + // For SAP, we ultimately separate "normal" geoms and the ones that have + // infinite AABBs. No point doing SAP on infinite ones (and it doesn't handle + // infinite geoms anyway). + TGeomPtrArray TmpGeomList; // temporary for normal geoms + TGeomPtrArray TmpInfGeomList; // temporary for geoms with infinite AABBs + + // Our sorting axes. + Axes SortAxes; + + + dxSAPSpace( dSpaceID _space, AxisOrder sortAxes ); + virtual ~dxSAPSpace(); + + // dxSpace + virtual dxGeom* getGeom(int i); + virtual void add(dxGeom* g); + virtual void remove(dxGeom* g); + virtual void dirty(dxGeom* g); + virtual void computeAABB(); + virtual void cleanGeoms(); + virtual void collide (void *data, dNearCallback *callback); + // TBD: not implemented yet + virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + + +dxSAPSpace::dxSAPSpace( dSpaceID _space, AxisOrder sortAxes ) +: dxSpace( _space ), + SortAxes( sortAxes ) +{ + type = dSweepAndPruneSpaceClass; + + // Init AABB to infinity + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + +dxSAPSpace::~dxSAPSpace() +{ +} + +dxGeom* dxSAPSpace::getGeom( int i ) +{ + dUASSERT( i >= 0 && i < count, "index out of range" ); + int dirtySize = DirtyList.size(); + if( i < dirtySize ) + return DirtyList[i]; + else + return GeomList[i-dirtySize]; +} + +void dxSAPSpace::add( dxGeom* g ) +{ + CHECK_NOT_LOCKED (this); + dAASSERT(g); + dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space"); + + g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + + // add to dirty list + GEOM_SET_DIRTY_IDX( g, DirtyList.size() ); + GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX ); + DirtyList.push( g ); + + g->parent_space = this; + this->count++; + + dGeomMoved(this); +} + + +void dxSAPSpace::remove( dxGeom* g ) +{ + CHECK_NOT_LOCKED(this); + dAASSERT(g); + dUASSERT(g->parent_space == this,"object is not in this space"); + + // remove + int dirtyIdx = GEOM_GET_DIRTY_IDX(g); + int geomIdx = GEOM_GET_GEOM_IDX(g); + // must be in one list, not in both + dUASSERT( + dirtyIdx==GEOM_INVALID_IDX && geomIdx>=0 && geomIdx=0 && dirtyIdxparent_space = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved(this); +} + +void dxSAPSpace::dirty( dxGeom* g ) +{ + dAASSERT(g); + dUASSERT(g->parent_space == this,"object is not in this space"); + + // check if already dirtied + int dirtyIdx = GEOM_GET_DIRTY_IDX(g); + if( dirtyIdx != GEOM_INVALID_IDX ) + return; + + int geomIdx = GEOM_GET_GEOM_IDX(g); + dUASSERT( geomIdx>=0 && geomIdxcleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + // remove from dirty list, add to geom list + GEOM_SET_DIRTY_IDX( g, GEOM_INVALID_IDX ); + GEOM_SET_GEOM_IDX( g, geomSize + i ); + GeomList[geomSize+i] = g; + } + // clear dirty list + DirtyList.setSize( 0 ); + + lock_count--; +} + + +void dxSAPSpace::collide (void *data, dNearCallback *callback) +{ + dAASSERT (callback); + + lock_count++; + + cleanGeoms(); + + // by now all geoms are in GeomList, and DirtyList must be empty + int geomSize = GeomList.size(); + dUASSERT( geomSize == count, "geom counts messed up" ); + + // separate all geoms into infinite AABBs and normal AABBs + TmpGeomList.setSize(0); + TmpInfGeomList.setSize(0); + int axis0max = SortAxes.mAxis0*2+1; + for( int i = 0; i < geomSize; ++i ) { + dxGeom* g =GeomList[i]; + if( !GEOM_ENABLED(g) ) // skip disabled ones + continue; + const dReal& amax = g->aabb[axis0max]; + if( amax == dInfinity ) // HACK? probably not... + TmpInfGeomList.push( g ); + else + TmpGeomList.push( g ); + } + + // do SAP on normal AABBs + Pairs overlapBoxes; + //bool isok = complete_box_pruning( TmpGeomList.size(), (const dxGeom**)TmpGeomList.data(), overlapBoxes, SortAxes ); + complete_box_pruning( TmpGeomList.size(), (const dxGeom**)TmpGeomList.data(), overlapBoxes, SortAxes ); + + // collide overlapping + udword overlapCount = overlapBoxes.GetNbPairs(); + for( udword j = 0; j < overlapCount; ++j ) { + const Pair* pair = overlapBoxes.GetPair(j); + dxGeom* g1 = TmpGeomList[pair->id0]; + dxGeom* g2 = TmpGeomList[pair->id1]; + collideGeomsNoAABBs( g1, g2, data, callback ); + } + + int infSize = TmpInfGeomList.size(); + int normSize = TmpGeomList.size(); + int m, n; + for( m = 0; m < infSize; ++m ) { + dxGeom* g1 = TmpInfGeomList[m]; + // collide infinite ones + for( n = m+1; n < infSize; ++n ) { + dxGeom* g2 = TmpInfGeomList[n]; + collideGeomsNoAABBs( g1, g2, data, callback ); + } + // collide infinite ones with normal ones + for( n = 0; n < normSize; ++n ) { + dxGeom* g2 = TmpGeomList[n]; + collideGeomsNoAABBs( g1, g2, data, callback ); + } + } + + lock_count--; +} + + +void dxSAPSpace::collide2( void *data, dxGeom *geom, dNearCallback *callback ) +{ + // TBD + /* + dAASSERT (geom && callback); + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + if (GEOM_ENABLED(g)){ + collideAABBs (g,geom,data,callback); + } + } + + lock_count--; + */ +} + +dSpaceID dSweepAndPruneSpaceCreate(dxSpace* space,int sortAxes) { + return new dxSAPSpace( space, (AxisOrder)sortAxes ); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_space.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_space.cpp new file mode 100644 index 00000000..e37f29e4 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_space.cpp @@ -0,0 +1,970 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + +spaces + +*/ + +#include "ode/ode_common.h" +#include "ode/ode_matrix.h" +#include "ode/ode_collision_space.h" +#include "ode/ode_collision.h" +#include "ode/ode_collision_kernel.h" +#include +#include "ode/ode_util.h" +#include "ballistica/ballistica.h" + +#include "ode/ode_collision_space_internal.h" + +// in my current setup, ldexp seems to be messed up +// when we compile with hardware floats.. +#if BA_OSTYPE_ANDROID +#define ldexp __builtin_ldexpf +#endif + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + + +// ericf addition - super simple memory buffer +// class _Buffer{ +// public: +// void allocate(unsigned long size){ +// if (_allocated and _ptr) free(_ptr); +// _ptr = malloc(size); +// dIASSERT(_ptr); +// _allocated = true; +// } +// ~_Buffer(){ +// if (_allocated and _ptr) free(_ptr); +// _ptr = NULL; +// } +// void* getPtr() const {return _ptr;} +// private: +// void* _ptr = nullptr; +// bool _allocated = false; +// }; + +// ericf addition +// #define TRIXY_ALLOCA(name, type, n) _Buffer name ## BUF; \ +// if (n > 1000) { \ +// name ## BUF.allocate((n)); \ +// name = (type*)name ## BUF.getPtr();\ +// } else { \ +// name = (type*)ALLOCA((n)); \ +// dIASSERT(name); \ +// } + + + +using namespace std; + +void collideAABBs (dxGeom *g1, dxGeom *g2, void *data, dNearCallback *callback) +{ + dIASSERT((g1->gflags & GEOM_AABB_BAD)==0); + dIASSERT((g2->gflags & GEOM_AABB_BAD)==0); + + // no contacts if both geoms on the same body, and the body is not 0 + if (g1->body == g2->body && g1->body) return; + + // test if the category and collide bitfields match + if ( ((g1->category_bits & g2->collide_bits) || + (g2->category_bits & g1->collide_bits)) == 0) { + return; + } + + // if the bounding boxes are disjoint then don't do anything + dReal *bounds1 = g1->aabb; + dReal *bounds2 = g2->aabb; + if (bounds1[0] > bounds2[1] || + bounds1[1] < bounds2[0] || + bounds1[2] > bounds2[3] || + bounds1[3] < bounds2[2] || + bounds1[4] > bounds2[5] || + bounds1[5] < bounds2[4]) { + return; + } + + // check if either object is able to prove that it doesn't intersect the + // AABB of the other + if (g1->AABBTest (g2,bounds2) == 0) return; + if (g2->AABBTest (g1,bounds1) == 0) return; + + // the objects might actually intersect - call the space callback function + callback (data,g1,g2); +} + + +//**************************************************************************** +// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags +// and moving it to the front of the space's list. all the parents of a +// dirty geom also become dirty. + +void dGeomMoved (dxGeom *geom) +{ + dAASSERT (geom); + + // from the bottom of the space heirarchy up, process all clean geoms + // turning them into dirty geoms. + dxSpace *parent = geom->parent_space; + + while (parent && (geom->gflags & GEOM_DIRTY)==0) { + CHECK_NOT_LOCKED (parent); + geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + parent->dirty (geom); + geom = parent; + parent = parent->parent_space; + } + + // all the remaining dirty geoms must have their AABB_BAD flags set, to + // ensure that their AABBs get recomputed + while (geom) { + geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + CHECK_NOT_LOCKED (geom->parent_space); + geom = geom->parent_space; + } +} + +// ericf tweak - we don't use this functionality +//#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED) +#define GEOM_ENABLED(g) (true) + +//**************************************************************************** +// dxSpace + +dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0) +{ + count = 0; + first = 0; + cleanup = 1; + current_index = 0; + current_geom = 0; + lock_count = 0; +} + + +dxSpace::~dxSpace() +{ + CHECK_NOT_LOCKED (this); + if (cleanup) { + // note that destroying each geom will call remove() + dxGeom *g,*n; + for (g = first; g; g=n) { + n = g->next; + dGeomDestroy (g); + } + } + else { + dxGeom *g,*n; + for (g = first; g; g=n) { + n = g->next; + remove (g); + } + } +} + + +void dxSpace::computeAABB() +{ + if (first) { + int i; + dReal a[6]; + a[0] = dInfinity; + a[1] = -dInfinity; + a[2] = dInfinity; + a[3] = -dInfinity; + a[4] = dInfinity; + a[5] = -dInfinity; + for (dxGeom *g=first; g; g=g->next) { + g->recomputeAABB(); + for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i]; + for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i]; + } + memcpy(aabb,a,6*sizeof(dReal)); + } + else { + dSetZero (aabb,6); + } +} + + +void dxSpace::setCleanup (int mode) +{ + cleanup = (mode != 0); +} + + +int dxSpace::getCleanup() +{ + return cleanup; +} + + +int dxSpace::query (dxGeom *geom) +{ + dAASSERT (geom); + return (geom->parent_space == this); +} + + +int dxSpace::getNumGeoms() +{ + return count; +} + + +// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1 + +dxGeom *dxSpace::getGeom (int i) +{ + dUASSERT (i >= 0 && i < count,"index out of range"); + if (current_geom && current_index == i-1) { + current_geom = current_geom->next; + current_index = i; + return current_geom; + } + else { + dxGeom *g=first; + for (int j=0; jnext; else return 0; + } + current_geom = g; + current_index = i; + return g; + } +} + + +void dxSpace::add (dxGeom *geom) +{ + CHECK_NOT_LOCKED (this); + dAASSERT (geom); + dUASSERT (geom->parent_space == 0 && geom->next == 0, + "geom is already in a space"); + + // add + geom->parent_space = this; + geom->spaceAdd (&first); + count++; + + // enumerator has been invalidated + current_geom = 0; + + // new geoms are added to the front of the list and are always + // considered to be dirty. as a consequence, this space and all its + // parents are dirty too. + geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; + dGeomMoved (this); +} + + +void dxSpace::remove (dxGeom *geom) +{ + CHECK_NOT_LOCKED (this); + dAASSERT (geom); + dUASSERT (geom->parent_space == this,"object is not in this space"); + + // remove + geom->spaceRemove(); + count--; + + // safeguard + geom->next = 0; + geom->tome = 0; + geom->parent_space = 0; + + // enumerator has been invalidated + current_geom = 0; + + // the bounding box of this space (and that of all the parents) may have + // changed as a consequence of the removal. + dGeomMoved (this); +} + + +void dxSpace::dirty (dxGeom *geom) +{ +// geom->spaceRemove(); +// geom->spaceAdd (&first); +} + +//**************************************************************************** +// simple space - reports all n^2 object intersections + +struct dxSimpleSpace : public dxSpace { + dxSimpleSpace (dSpaceID _space); + void cleanGeoms(); + void collide (void *data, dNearCallback *callback); + void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + +dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space) +{ + type = dSimpleSpaceClass; +} + + +void dxSimpleSpace::cleanGeoms() +{ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; +// for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) { + for (dxGeom *g=first; g; g=g->next) { + if (g->gflags & GEOM_DIRTY){ + if (IS_SPACE(g)) { + ((dxSpace*)g)->cleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + } + } + lock_count--; +} + + +void dxSimpleSpace::collide (void *data, dNearCallback *callback) +{ +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"simplespace colliding\n"); + } +#endif + + dAASSERT (callback); + + lock_count++; + cleanGeoms(); + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"listing space geoms:\n"); + for (dxGeom *g1=first; g1; g1=g1->next) { + fprintf(f," space geomID: %d\n",g1->indexTest); + } + } +#endif + + // intersect all bounding boxes + for (dxGeom *g1=first; g1; g1=g1->next) { + //if (GEOM_ENABLED(g1)){ + for (dxGeom *g2=g1->next; g2; g2=g2->next) { + // if (GEOM_ENABLED(g2)){ + collideAABBs (g1,g2,data,callback); + // } + } + // } + } + + lock_count--; +} + + +void dxSimpleSpace::collide2 (void *data, dxGeom *geom, + dNearCallback *callback) +{ + dAASSERT (geom && callback); + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + if (GEOM_ENABLED(g)){ + collideAABBs (g,geom,data,callback); + } + } + + lock_count--; +} + +//**************************************************************************** +// utility stuff for hash table space + +// kind of silly, but oh well... +#ifndef MAXINT +#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1)) +#endif + + +// prime[i] is the largest prime smaller than 2^i +#define NUM_PRIMES 31 +static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L, + 1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L, + 524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L, + 67108859L,134217689L,268435399L,536870909L,1073741789L}; + + +// an axis aligned bounding box in the hash table +struct dxAABB { + dxAABB *next; // next in the list of all AABBs + int level; // the level this is stored in (cell size = 2^level) + int dbounds[6]; // AABB bounds, discretized to cell size + dxGeom *geom; // corresponding geometry object (AABB stored here) + int index; // index of this AABB, starting from 0 +}; + + +// a hash table node that represents an AABB that intersects a particular cell +// at a particular level +struct Node { + Node *next; // next node in hash table collision list, 0 if none + int x,y,z; // cell position in space, discretized to cell size + dxAABB *aabb; // axis aligned bounding box that intersects this cell +}; + + +// return the `level' of an AABB. the AABB will be put into cells at this +// level - the cell size will be 2^level. the level is chosen to be the +// smallest value such that the AABB occupies no more than 8 cells, regardless +// of its placement. this means that: +// size/2 < q <= size +// where q is the maximum AABB dimension. + +static int findLevel (dReal bounds[6]) +{ + if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity || + bounds[2] <= -dInfinity || bounds[3] >= dInfinity || + bounds[4] <= -dInfinity || bounds[5] >= dInfinity) { + return MAXINT; + } + + // compute q + dReal q,q2; + q = bounds[1] - bounds[0]; // x bounds + q2 = bounds[3] - bounds[2]; // y bounds + if (q2 > q) q = q2; + q2 = bounds[5] - bounds[4]; // z bounds + if (q2 > q) q = q2; + + // find level such that 0.5 * 2^level < q <= 2^level + int level; + frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp) + return level; +} + + +// find a virtual memory address for a cell at the given level and x,y,z +// position. +// @@@ currently this is not very sophisticated, e.g. the scaling +// factors could be better designed to avoid collisions, and they should +// probably depend on the hash table physical size. + +static unsigned long getVirtualAddress (int level, int x, int y, int z) +{ + return level*1000 + x*100 + y*10 + z; +} + +//**************************************************************************** +// hash space + +struct dxHashSpace : public dxSpace { + int global_minlevel; // smallest hash table level to put AABBs in + int global_maxlevel; // objects that need a level larger than this will be + // put in a "big objects" list instead of a hash table + + dxHashSpace (dSpaceID _space); + void setLevels (int minlevel, int maxlevel); + void getLevels (int *minlevel, int *maxlevel); + void cleanGeoms(); + void collide (void *data, dNearCallback *callback); + void collide2 (void *data, dxGeom *geom, dNearCallback *callback); +}; + + +dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space) +{ + type = dHashSpaceClass; + global_minlevel = -3; + global_maxlevel = 10; +} + + +void dxHashSpace::setLevels (int minlevel, int maxlevel) +{ + dAASSERT (minlevel <= maxlevel); + global_minlevel = minlevel; + global_maxlevel = maxlevel; +} + + +void dxHashSpace::getLevels (int *minlevel, int *maxlevel) +{ + if (minlevel) *minlevel = global_minlevel; + if (maxlevel) *maxlevel = global_maxlevel; +} + + +void dxHashSpace::cleanGeoms() +{ + // compute the AABBs of all dirty geoms, and clear the dirty flags + lock_count++; + for (dxGeom *g=first; g; g=g->next) { + if (g->gflags & GEOM_DIRTY) + { + if (IS_SPACE(g)) { + ((dxSpace*)g)->cleanGeoms(); + } + g->recomputeAABB(); + g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); + } + } + lock_count--; +} + +//extern double ldexp(double, int) __NDK_FPABI_MATH__; +//extern double ldexp(double x, int exponent); + +void dxHashSpace::collide (void *data, dNearCallback *callback) +{ + + unsigned char *allocated1 = NULL; + unsigned char *allocated2 = NULL; + + dAASSERT(callback); + dxGeom *geom; + dxAABB *aabb; + + // ericf tweak - i was overflowing. + // FIXME: isn't long still 32 bit in some/all cases?.... + //int i,maxlevel; + long i, maxlevel; + + // 0 or 1 geoms can't collide with anything + if (count < 2) return; + + lock_count++; + cleanGeoms(); + + // create a list of auxiliary information for all geom axis aligned bounding + // boxes. set the level for all AABBs. put AABBs larger than the space's + // global_maxlevel in the big_boxes list, check everything else against + // that list at the end. for AABBs that are not too big, record the maximum + // level that we need. + + int n = 0; // number of AABBs in main list + dxAABB *first_aabb = 0; // list of AABBs in hash table + dxAABB *big_boxes = 0; // list of AABBs too big for hash table + maxlevel = global_minlevel - 1; + for (geom = first; geom; geom=geom->next) { + // if (!GEOM_ENABLED(geom)){ + // continue; + // } + // ericf note: TRIXY_ALLOCA will never actually allocate because + // this is always a small bit of memory; right?.. should test. + // TRIXY_ALLOCA(aabb, dxAABB, sizeof(dxAABB)); + dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB)); + aabb->geom = geom; + // compute level, but prevent cells from getting too small + int level = findLevel (geom->aabb); + if (level < global_minlevel) level = global_minlevel; + if (level <= global_maxlevel) { + // aabb goes in main list + aabb->next = first_aabb; + first_aabb = aabb; + aabb->level = level; + if (level > maxlevel) maxlevel = level; + // cellsize = 2^level + dReal cellsize = (dReal) ldexp (1.0,level); + // discretize AABB position to cell size + for (i=0; i < 6; i++) aabb->dbounds[i] = (int) + floor (geom->aabb[i]/cellsize); + // set AABB index + aabb->index = n; + n++; + } else { + // aabb is too big, put it in the big_boxes list. we don't care + // about setting level, dbounds, index, or the maxlevel + aabb->next = big_boxes; + big_boxes = aabb; + } + } + + // for `n' objects, an n*n array of bits is used to record if those objects + // have been intersection-tested against each other yet. this array can + // grow large with high n, but oh well... + long tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits + unsigned char *tested; + + // dont allocate on the stack for substantial sizes... + if (n*tested_rowsize < 5000){ + tested = (unsigned char *) alloca (n * tested_rowsize); + } else { + tested = (unsigned char *) malloc (n * tested_rowsize); + allocated1 = tested; + } + memset (tested,0,n * tested_rowsize); + + // create a hash table to store all AABBs. each AABB may take up to 8 cells. + // we use chaining to resolve collisions, but we use a relatively large + // table to reduce the chance of collisions. + + // compute hash table size sz to be a prime > 8*n + for (i=0; i= (8*n)) break; + } + if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless + long sz = prime[i]; + + // allocate and initialize hash table node pointers + Node **table; + if (sizeof(Node*)*sz < 8000){ + table = (Node **) ALLOCA (sizeof(Node*) * sz); + } else { + table = (Node **) malloc(sizeof(Node*) * sz); + allocated2 = (unsigned char*)table; + } + dIASSERT (table != NULL); + + for (i=0; i 500){ + int need = 0; + // tally up what we'll need.. perhaps there's a way to determine this + // without iterating?.. + for (aabb=first_aabb; aabb; aabb=aabb->next) { + int *dbounds = aabb->dbounds; + for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) { + for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) { + for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) { + need++; + } + } + } + } + nodeBuffer = (Node*)malloc(need*sizeof(Node)); + dIASSERT(nodeBuffer); + Node *ni = nodeBuffer; + + // add each AABB to the hash table (may need to add it to up to 8 cells) + for (aabb=first_aabb; aabb; aabb=aabb->next) { + int *dbounds = aabb->dbounds; + for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) { + for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) { + for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz; + // add a new node to the hash table + Node *node = ni++; + node->x = xi; + node->y = yi; + node->z = zi; + node->aabb = aabb; + node->next = table[hi]; + table[hi] = node; + } + } + } + } + } else { + // old alloca-usin' code: + // add each AABB to the hash table (may need to add it to up to 8 cells) + for (aabb=first_aabb; aabb; aabb=aabb->next) { + int *dbounds = aabb->dbounds; + for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) { + for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) { + for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz; + // add a new node to the hash table + Node *node = (Node*) alloca (sizeof (Node)); + node->x = xi; + node->y = yi; + node->z = zi; + node->aabb = aabb; + node->next = table[hi]; + table[hi] = node; + } + } + } + } + } + + // now that all AABBs are loaded into the hash table, we do the actual + // collision detection. for all AABBs, check for other AABBs in the + // same cells for collisions, and then check for other AABBs in all + // intersecting higher level cells. + + int db[6]; // discrete bounds at current level + for (aabb=first_aabb; aabb; aabb=aabb->next) { + // we are searching for collisions with aabb + for (i=0; i<6; i++) db[i] = aabb->dbounds[i]; + for (int level = aabb->level; level <= maxlevel; level++) { + for (int xi = db[0]; xi <= db[1]; xi++) { + for (int yi = db[2]; yi <= db[3]; yi++) { + for (int zi = db[4]; zi <= db[5]; zi++) { + // get the hash index + unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; + // search all nodes at this index + Node *node; + for (node = table[hi]; node; node=node->next) { + // node points to an AABB that may intersect aabb + if (node->aabb == aabb) continue; + if (node->aabb->level == level && + node->x == xi && node->y == yi && node->z == zi) { + // see if aabb and node->aabb have already been tested + // against each other + unsigned char mask; + if (aabb->index <= node->aabb->index) { + i = ((long)aabb->index * tested_rowsize)+(node->aabb->index >> 3); + mask = 1 << (node->aabb->index & 7); + } else { + i = ((long)node->aabb->index * tested_rowsize)+(aabb->index >> 3); + mask = 1 << (aabb->index & 7); + } + dIASSERT (i >= 0 && i < (tested_rowsize*n)); + if ((tested[i] & mask)==0) { + collideAABBs (aabb->geom,node->aabb->geom,data,callback); + } + tested[i] |= mask; + } + } + } + } + } + // get the discrete bounds for the next level up + for (i=0; i<6; i++) db[i] >>= 1; + } + } + + // every AABB in the normal list must now be intersected against every + // AABB in the big_boxes list. so let's hope there are not too many objects + // in the big_boxes list. + for (aabb=first_aabb; aabb; aabb=aabb->next) { + for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) { + collideAABBs (aabb->geom,aabb2->geom,data,callback); + } + } + + // intersected all AABBs in the big_boxes list together + for (aabb=big_boxes; aabb; aabb=aabb->next) { + for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) { + collideAABBs (aabb->geom,aabb2->geom,data,callback); + } + } + + lock_count--; + + // ericf addition: if we allocated using malloc instead of alloca + // (to avoid overflowing the stack), free it here.. + if (allocated1) { + free(allocated1); + } + if (allocated2) { + free(allocated2); + } + if (nodeBuffer) { + free(nodeBuffer); + } +} + + +void dxHashSpace::collide2 (void *data, dxGeom *geom, + dNearCallback *callback) +{ + dAASSERT (geom && callback); + + // this could take advantage of the hash structure to avoid + // O(n2) complexity, but it does not yet. + + lock_count++; + cleanGeoms(); + geom->recomputeAABB(); + + // intersect bounding boxes + for (dxGeom *g=first; g; g=g->next) { + collideAABBs (g,geom,data,callback); + } + + lock_count--; +} + +//**************************************************************************** +// space functions + +dxSpace *dSimpleSpaceCreate (dxSpace *space) +{ + return new dxSimpleSpace (space); +} + + +dxSpace *dHashSpaceCreate (dxSpace *space) +{ + return new dxHashSpace (space); +} + + +void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel) +{ + dAASSERT (space); + dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel"); + dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space"); + dxHashSpace *hspace = (dxHashSpace*) space; + hspace->setLevels (minlevel,maxlevel); +} + + +void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel) +{ + dAASSERT (space); + dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space"); + dxHashSpace *hspace = (dxHashSpace*) space; + hspace->getLevels (minlevel,maxlevel); +} + + +void dSpaceDestroy (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + dGeomDestroy (space); +} + + +void dSpaceSetCleanup (dxSpace *space, int mode) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->setCleanup (mode); +} + + +int dSpaceGetCleanup (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getCleanup(); +} + + +void dSpaceAdd (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + CHECK_NOT_LOCKED (space); + space->add (g); +} + + +void dSpaceRemove (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + CHECK_NOT_LOCKED (space); + space->remove (g); +} + + +int dSpaceQuery (dxSpace *space, dxGeom *g) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->query (g); +} + +void dSpaceClean (dxSpace *space){ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + + space->cleanGeoms(); +} + +int dSpaceGetNumGeoms (dxSpace *space) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getNumGeoms(); +} + + +dGeomID dSpaceGetGeom (dxSpace *space, int i) +{ + dAASSERT (space); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + return space->getGeom (i); +} + + +void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback) +{ + dAASSERT (space && callback); + dUASSERT (dGeomIsSpace(space),"argument not a space"); + space->collide (data,callback); +} + + +void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data, + dNearCallback *callback) +{ + dAASSERT (g1 && g2 && callback); + dxSpace *s1,*s2; + + // see if either geom is a space + if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0; + if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0; + + // handle the four space/geom cases + if (s1) { + if (s2) { + // g1 and g2 are spaces. + if (s1==s2) { + // collide a space with itself --> interior collision + s1->collide (data,callback); + } + else { + // iterate through the space that has the fewest geoms, calling + // collide2 in the other space for each one. + if (s1->count < s2->count) { + for (dxGeom *g = s1->first; g; g=g->next) { + s2->collide2 (data,g,callback); + } + } + else { + for (dxGeom *g = s2->first; g; g=g->next) { + s1->collide2 (data,g,callback); + } + } + } + } + else { + // g1 is a space, g2 is a geom + s1->collide2 (data,g2,callback); + } + } + else { + if (s2) { + // g1 is a geom, g2 is a space + s2->collide2 (data,g1,callback); + } + else { + // g1 and g2 are geoms, call the callback directly + callback (data,g1,g2); + } + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_space.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_space.h new file mode 100644 index 00000000..a71f30af --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_space.h @@ -0,0 +1,76 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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_COLLISION_SPACE_H_ +#define _ODE_COLLISION_SPACE_H_ + +#include "ode/ode_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct dContactGeom; + +typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); + + +dSpaceID dSimpleSpaceCreate (dSpaceID space); +dSpaceID dHashSpaceCreate (dSpaceID space); +dSpaceID dQuadTreeSpaceCreate (dSpaceID space, dVector3 Center, dVector3 Extents, int Depth); + + +// SAP +// Order XZY or ZXY usually works best, if your Y is up. +// Don't know how to express axis orders... defines are ok? +// must match PointComponent/AxisOrder from Opcode/Ice/IceAxes.h! +#define dSAP_AXES_XYZ ((0)|(1<<2)|(2<<4)) +#define dSAP_AXES_XZY ((0)|(2<<2)|(1<<4)) +#define dSAP_AXES_YXZ ((1)|(0<<2)|(2<<4)) +#define dSAP_AXES_YZX ((1)|(2<<2)|(0<<4)) +#define dSAP_AXES_ZXY ((2)|(0<<2)|(1<<4)) +#define dSAP_AXES_ZYX ((2)|(1<<2)|(0<<4)) +dSpaceID dSweepAndPruneSpaceCreate (dSpaceID space, int axisorder); + + + +void dSpaceDestroy (dSpaceID); + +void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel); +void dHashSpaceGetLevels (dSpaceID space, int *minlevel, int *maxlevel); + +void dSpaceSetCleanup (dSpaceID space, int mode); +int dSpaceGetCleanup (dSpaceID space); + +void dSpaceAdd (dSpaceID, dGeomID); +void dSpaceRemove (dSpaceID, dGeomID); +int dSpaceQuery (dSpaceID, dGeomID); +void dSpaceClean (dSpaceID); +int dSpaceGetNumGeoms (dSpaceID); +dGeomID dSpaceGetGeom (dSpaceID, int i); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_space_internal.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_space_internal.h new file mode 100644 index 00000000..a5f3c4c2 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_space_internal.h @@ -0,0 +1,86 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + +stuff common to all spaces + +*/ + +#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_ +#define _ODE_COLLISION_SPACE_INTERNAL_H_ + +#define ALLOCA(x) dALLOCA16(x) + +#define CHECK_NOT_LOCKED(space) \ + dUASSERT ((space)==0 || (space)->lock_count==0, \ + "invalid operation for locked space"); + + +// collide two geoms together. for the hash table space, this is +// called if the two AABBs inhabit the same hash table cells. +// this only calls the callback function if the AABBs actually +// intersect. if a geom has an AABB test function, that is called to +// provide a further refinement of the intersection. +// +// NOTE: this assumes that the geom AABBs are valid on entry +// and that both geoms are enabled. + +void collideAABBs (dxGeom *g1, dxGeom *g2, + void *data, dNearCallback *callback); + +//moved this to collision_space.cpp so we dont get unused warnings +/* { */ +/* dIASSERT((g1->gflags & GEOM_AABB_BAD)==0); */ +/* dIASSERT((g2->gflags & GEOM_AABB_BAD)==0); */ + +/* // no contacts if both geoms on the same body, and the body is not 0 */ +/* if (g1->body == g2->body && g1->body) return; */ + +/* // test if the category and collide bitfields match */ +/* if ( ((g1->category_bits & g2->collide_bits) || */ +/* (g2->category_bits & g1->collide_bits)) == 0) { */ +/* return; */ +/* } */ + +/* // if the bounding boxes are disjoint then don't do anything */ +/* dReal *bounds1 = g1->aabb; */ +/* dReal *bounds2 = g2->aabb; */ +/* if (bounds1[0] > bounds2[1] || */ +/* bounds1[1] < bounds2[0] || */ +/* bounds1[2] > bounds2[3] || */ +/* bounds1[3] < bounds2[2] || */ +/* bounds1[4] > bounds2[5] || */ +/* bounds1[5] < bounds2[4]) { */ +/* return; */ +/* } */ + +/* // check if either object is able to prove that it doesn't intersect the */ +/* // AABB of the other */ +/* if (g1->AABBTest (g2,bounds2) == 0) return; */ +/* if (g2->AABBTest (g1,bounds1) == 0) return; */ + +/* // the objects might actually intersect - call the space callback function */ +/* callback (data,g1,g2); */ +/* } */ + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_std.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_std.cpp new file mode 100644 index 00000000..62673f66 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_std.cpp @@ -0,0 +1,2081 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + +standard ODE geometry primitives: public API and pairwise collision functions. + +the rule is that only the low level primitive collision functions should set +dContactGeom::g1 and dContactGeom::g2. + +*/ + +#include "ode/ode_common.h" +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_kernel.h" +#include "ode/ode_collision_std.h" +#include "ode/ode_collision_util.h" + +// 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 + +//**************************************************************************** +// the basic geometry objects + +struct dxSphere : public dxGeom { + dReal radius; // sphere radius + dxSphere (dSpaceID space, dReal _radius); + void computeAABB(); +}; + + +struct dxBox : public dxGeom { + dVector3 side; // side lengths (x,y,z) + dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz); + void computeAABB(); +}; + + +struct dxCCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + +struct dxCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + +struct dxPlane : public dxGeom { + dReal p[4]; + dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); + void computeAABB(); +}; + + +struct dxRay : public dxGeom { + dReal length; + dxRay (dSpaceID space, dReal _length); + void computeAABB(); +}; + +//**************************************************************************** +// sphere public API + +dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1) +{ + dAASSERT (_radius > 0); + type = dSphereClass; + radius = _radius; +} + + +void dxSphere::computeAABB() +{ + aabb[0] = pos[0] - radius; + aabb[1] = pos[0] + radius; + aabb[2] = pos[1] - radius; + aabb[3] = pos[1] + radius; + aabb[4] = pos[2] - radius; + aabb[5] = pos[2] + radius; +} + + +dGeomID dCreateSphere (dSpaceID space, dReal radius) +{ + return new dxSphere (space,radius); +} + + +void dGeomSphereSetRadius (dGeomID g, dReal radius) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dAASSERT (radius > 0); + dxSphere *s = (dxSphere*) g; + s->radius = radius; + dGeomMoved (g); +} + + +dReal dGeomSphereGetRadius (dGeomID g) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dxSphere *s = (dxSphere*) g; + return s->radius; +} + + +dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dSphereClass,"argument not a sphere"); + dxSphere *s = (dxSphere*) g; + return s->radius - dSqrt ((x-s->pos[0])*(x-s->pos[0]) + + (y-s->pos[1])*(y-s->pos[1]) + + (z-s->pos[2])*(z-s->pos[2])); +} + +//**************************************************************************** +// box public API + +dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1) +{ + dAASSERT (lx >= 0 && ly >= 0 && lz >= 0); + type = dBoxClass; + side[0] = lx; + side[1] = ly; + side[2] = lz; +} + + +void dxBox::computeAABB() +{ + dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) + + dFabs (R[1] * side[1]) + dFabs (R[2] * side[2])); + dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) + + dFabs (R[5] * side[1]) + dFabs (R[6] * side[2])); + dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) + + dFabs (R[9] * side[1]) + dFabs (R[10] * side[2])); + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz) +{ + return new dxBox (space,lx,ly,lz); +} + + +void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dAASSERT (lx > 0 && ly > 0 && lz > 0); + dxBox *b = (dxBox*) g; + b->side[0] = lx; + b->side[1] = ly; + b->side[2] = lz; + dGeomMoved (g); +} + + +void dGeomBoxGetLengths (dGeomID g, dVector3 result) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dxBox *b = (dxBox*) g; + result[0] = b->side[0]; + result[1] = b->side[1]; + result[2] = b->side[2]; +} + + +dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dBoxClass,"argument not a box"); + dxBox *b = (dxBox*) g; + + // Set p = (x,y,z) relative to box center + // + // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2) + + dVector3 p,q; + + p[0] = x - b->pos[0]; + p[1] = y - b->pos[1]; + p[2] = z - b->pos[2]; + + // Rotate p into box's coordinate frame, so we can + // treat the OBB as an AABB + + dMULTIPLY1_331 (q,b->R,p); + + // Record distance from point to each successive box side, and see + // if the point is inside all six sides + + dReal dist[6]; + int i; + + bool inside = true; + + for (i=0; i < 3; i++) { + dReal side = b->side[i] * REAL(0.5); + + dist[i ] = side - q[i]; + dist[i+3] = side + q[i]; + + if ((dist[i] < 0) || (dist[i+3] < 0)) { + inside = false; + } + } + + // If point is inside the box, the depth is the smallest positive distance + // to any side + + if (inside) { + dReal smallest_dist = (dReal) (unsigned) -1; + + for (i=0; i < 6; i++) { + if (dist[i] < smallest_dist) smallest_dist = dist[i]; + } + + return smallest_dist; + } + + // Otherwise, if point is outside the box, the depth is the largest + // distance to any side. This is an approximation to the 'proper' + // solution (the proper solution may be larger in some cases). + + dReal largest_dist = 0; + + for (i=0; i < 6; i++) { + if (dist[i] > largest_dist) largest_dist = dist[i]; + } + + return -largest_dist; +} + +//**************************************************************************** +// capped cylinder public API + +dxCCylinder::dxCCylinder (dSpaceID space, dReal _radius, dReal _length) : + dxGeom (space,1) +{ + dAASSERT (_radius > 0 && _length > 0); + type = dCCylinderClass; + radius = _radius; + lz = _length; +} + + +void dxCCylinder::computeAABB() +{ + dReal xrange = dFabs(R[2] * lz) * REAL(0.5) + radius; + dReal yrange = dFabs(R[6] * lz) * REAL(0.5) + radius; + dReal zrange = dFabs(R[10] * lz) * REAL(0.5) + radius; + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length) +{ + return new dxCCylinder (space,radius,length); +} + + +void dGeomCCylinderSetParams (dGeomID g, dReal radius, dReal length) +{ + dUASSERT (g && g->type == dCCylinderClass,"argument not a ccylinder"); + dAASSERT (radius > 0 && length > 0); + dxCCylinder *c = (dxCCylinder*) g; + c->radius = radius; + c->lz = length; + dGeomMoved (g); +} + + +void dGeomCCylinderGetParams (dGeomID g, dReal *radius, dReal *length) +{ + dUASSERT (g && g->type == dCCylinderClass,"argument not a ccylinder"); + dxCCylinder *c = (dxCCylinder*) g; + *radius = c->radius; + *length = c->lz; +} + + +dReal dGeomCCylinderPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dCCylinderClass,"argument not a ccylinder"); + dxCCylinder *c = (dxCCylinder*) g; + dVector3 a; + a[0] = x - c->pos[0]; + a[1] = y - c->pos[1]; + a[2] = z - c->pos[2]; + dReal beta = dDOT14(a,c->R+2); + dReal lz2 = c->lz*REAL(0.5); + if (beta < -lz2) beta = -lz2; + else if (beta > lz2) beta = lz2; + a[0] = c->pos[0] + beta*c->R[0*4+2]; + a[1] = c->pos[1] + beta*c->R[1*4+2]; + a[2] = c->pos[2] + beta*c->R[2*4+2]; + return c->radius - + dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2])); +} + +//**************************************************************************** +// plane public API + +static void make_sure_plane_normal_has_unit_length (dxPlane *g) +{ + dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2]; + if (l > 0) { + l = dRecipSqrt(l); + g->p[0] *= l; + g->p[1] *= l; + g->p[2] *= l; + g->p[3] *= l; + } + else { + g->p[0] = 1; + g->p[1] = 0; + g->p[2] = 0; + g->p[3] = 0; + } +} + + +dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) : + dxGeom (space,0) +{ + type = dPlaneClass; + p[0] = a; + p[1] = b; + p[2] = c; + p[3] = d; + make_sure_plane_normal_has_unit_length (this); +} + + +void dxPlane::computeAABB() +{ + // @@@ planes that have normal vectors aligned along an axis can use a + // @@@ less comprehensive (half space) bounding box. + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + + +dGeomID dCreatePlane (dSpaceID space, + dReal a, dReal b, dReal c, dReal d) +{ + return new dxPlane (space,a,b,c,d); +} + + +void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + p->p[0] = a; + p->p[1] = b; + p->p[2] = c; + p->p[3] = d; + make_sure_plane_normal_has_unit_length (p); + dGeomMoved (g); +} + + +void dGeomPlaneGetParams (dGeomID g, dVector4 result) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + result[0] = p->p[0]; + result[1] = p->p[1]; + result[2] = p->p[2]; + result[3] = p->p[3]; +} + + +dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); + dxPlane *p = (dxPlane*) g; + return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z; +} + +//**************************************************************************** +// ray public API + +dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1) +{ + type = dRayClass; + length = _length; +} + + +void dxRay::computeAABB() +{ + dVector3 e; + e[0] = pos[0] + R[0*4+2]*length; + e[1] = pos[1] + R[1*4+2]*length; + e[2] = pos[2] + R[2*4+2]*length; + + if (pos[0] < e[0]){ + aabb[0] = pos[0]; + aabb[1] = e[0]; + } + else{ + aabb[0] = e[0]; + aabb[1] = pos[0]; + } + + if (pos[1] < e[1]){ + aabb[2] = pos[1]; + aabb[3] = e[1]; + } + else{ + aabb[2] = e[1]; + aabb[3] = pos[1]; + } + + if (pos[2] < e[2]){ + aabb[4] = pos[2]; + aabb[5] = e[2]; + } + else{ + aabb[4] = e[2]; + aabb[5] = pos[2]; + } +} + + +dGeomID dCreateRay (dSpaceID space, dReal length) +{ + return new dxRay (space,length); +} + + +void dGeomRaySetLength (dGeomID g, dReal length) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + r->length = length; + dGeomMoved (g); +} + + +dReal dGeomRayGetLength (dGeomID g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dxRay *r = (dxRay*) g; + return r->length; +} + + +void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, + dReal dx, dReal dy, dReal dz) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + dReal* rot = g->R; + dReal* pos = g->pos; + dVector3 n; + pos[0] = px; + pos[1] = py; + pos[2] = pz; + + n[0] = dx; + n[1] = dy; + n[2] = dz; + dNormalize3(n); + rot[0*4+2] = n[0]; + rot[1*4+2] = n[1]; + rot[2*4+2] = n[2]; + dGeomMoved (g); +} + + +void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + start[0] = g->pos[0]; + start[1] = g->pos[1]; + start[2] = g->pos[2]; + dir[0] = g->R[0*4+2]; + dir[1] = g->R[1*4+2]; + dir[2] = g->R[2*4+2]; +} + + +void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + if (FirstContact){ + g->gflags |= RAY_FIRSTCONTACT; + } + else g->gflags &= ~RAY_FIRSTCONTACT; + + if (BackfaceCull){ + g->gflags |= RAY_BACKFACECULL; + } + else g->gflags &= ~RAY_BACKFACECULL; +} + + +void dGeomRayGetParams (dxGeom *g, int *FirstContact, int *BackfaceCull) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + + (*FirstContact) = ((g->gflags & RAY_FIRSTCONTACT) != 0); + (*BackfaceCull) = ((g->gflags & RAY_BACKFACECULL) != 0); +} + + +void dGeomRaySetClosestHit (dxGeom *g, int closestHit) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + if (closestHit){ + g->gflags |= RAY_CLOSEST_HIT; + } + else g->gflags &= ~RAY_CLOSEST_HIT; +} + + +int dGeomRayGetClosestHit (dxGeom *g) +{ + dUASSERT (g && g->type == dRayClass,"argument not a ray"); + return ((g->gflags & RAY_CLOSEST_HIT) != 0); +} + +//**************************************************************************** +// box-box collision utility + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr=0; + dReal buffer[16]; + dReal *q = p; + dReal *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + dReal *pq = q; + dReal *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + dReal *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } + done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal)); + return nr; +} + + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints (int n, dReal p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + dReal a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = REAL(0.5)*(p[0] + p[2]); + cy = REAL(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + a = dRecip(REAL(3.0)*(a+q)); + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + dReal A[8]; + for (i=0; i M_PI) a -= 2*M_PI; + dReal maxdiff=1e9,diff; +#ifndef dNODEBUG + *iret = i0; // iret is not allowed to keep this value +#endif + for (i=0; i M_PI) diff = 2*M_PI - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#ifndef dNODEBUG + dIASSERT (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. + +int dBoxBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2, + dVector3 normal, dReal *depth, int *return_code, + int maxc, dContactGeom *contact, int skip) +{ + const dReal fudge_factor = REAL(1.05); + dVector3 p,pp,normalC; + const dReal *normalR = 0; + dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*REAL(0.5); + A[1] = side1[1]*REAL(0.5); + A[2] = side1[2]*REAL(0.5); + B[0] = side2[0]*REAL(0.5); + B[1] = side2[1]*REAL(0.5); + B[2] = side2[2]*REAL(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + + Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); + Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); + Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + +#define TST(expr1,expr2,norm,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); + +#undef TST + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + dVector3 pa; + dReal sign; + for (i=0; i<3; i++) pa[i] = p1[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + dVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + dReal alpha,beta; + dVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + *return_code = code; + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + dVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331 (nr,Rb,normal2); + anr[0] = dFabs (nr[0]); + anr[1] = dFabs (nr[1]); + anr[2] = dFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + dVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + dReal quad[8]; // 2D coordinate of incident face (x,y pairs) + dReal c1,c2,m11,m12,m21,m22; + c1 = dDOT14 (center,Ra+code1); + c2 = dDOT14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44 (Ra+code1,Rb+a1); + m12 = dDOT44 (Ra+code1,Rb+a2); + m21 = dDOT44 (Ra+code2,Rb+a1); + m22 = dDOT44 (Ra+code2,Rb+a2); + { + dReal k1 = m11*Sb[a1]; + dReal k2 = m21*Sb[a1]; + dReal k3 = m12*Sb[a2]; + dReal k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + dReal rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + dReal ret[16]; + int n = intersectRectQuad (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + dReal point[3*8]; // penetrating contact points + dReal dep[8]; // depths for those points + dReal det1 = dRecip(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) { + dContactGeom *con = CONTACT(contact,skip*j); + for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; + con->depth = dep[j]; + } + } + else { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + dReal maxdepth = dep[0]; + for (i=1; i maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { + dContactGeom *con = CONTACT(contact,skip*j); + for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + con->depth = dep[iret[j]]; + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +//**************************************************************************** +// pairwise collision functions for standard geom types + +int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dSphereClass); + dxSphere *sphere1 = (dxSphere*) o1; + dxSphere *sphere2 = (dxSphere*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + return dCollideSpheres (o1->pos,sphere1->radius, + o2->pos,sphere2->radius,contact); +} + + +int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + // this is easy. get the sphere center `p' relative to the box, and then clip + // that to the boundary of the box (call that point `q'). if q is on the + // boundary of the box and |p-q| is <= sphere radius, they touch. + // if q is inside the box, the sphere is inside the box, so set a contact + // normal to push the sphere to the closest box face. + + dVector3 l,t,p,q,r; + dReal depth; + int onborder = 0; + + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dBoxClass); + dxSphere *sphere = (dxSphere*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + p[0] = o1->pos[0] - o2->pos[0]; + p[1] = o1->pos[1] - o2->pos[1]; + p[2] = o1->pos[2] - o2->pos[2]; + + l[0] = box->side[0]*REAL(0.5); + t[0] = dDOT14(p,o2->R); + if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; } + if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; } + + l[1] = box->side[1]*REAL(0.5); + t[1] = dDOT14(p,o2->R+1); + if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; } + if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; } + + t[2] = dDOT14(p,o2->R+2); + l[2] = box->side[2]*REAL(0.5); + if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; } + if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; } + + if (!onborder) { + // sphere center inside box. find closest face to `t' + dReal min_distance = l[0] - dFabs(t[0]); + int mini = 0; + for (int i=1; i<3; i++) { + dReal face_distance = l[i] - dFabs(t[i]); + if (face_distance < min_distance) { + min_distance = face_distance; + mini = i; + } + } + // contact position = sphere center + contact->pos[0] = o1->pos[0]; + contact->pos[1] = o1->pos[1]; + contact->pos[2] = o1->pos[2]; + // contact normal points to closest face + dVector3 tmp; + tmp[0] = 0; + tmp[1] = 0; + tmp[2] = 0; + tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0); + dMULTIPLY0_331 (contact->normal,o2->R,tmp); + // contact depth = distance to wall along normal plus radius + contact->depth = min_distance + sphere->radius; + return 1; + } + + t[3] = 0; //@@@ hmmm + dMULTIPLY0_331 (q,o2->R,t); + r[0] = p[0] - q[0]; + r[1] = p[1] - q[1]; + r[2] = p[2] - q[2]; + depth = sphere->radius - dSqrt(dDOT(r,r)); + if (depth < 0) return 0; + contact->pos[0] = q[0] + o2->pos[0]; + contact->pos[1] = q[1] + o2->pos[1]; + contact->pos[2] = q[2] + o2->pos[2]; + contact->normal[0] = r[0]; + contact->normal[1] = r[1]; + contact->normal[2] = r[2]; + dNormalize3 (contact->normal); + contact->depth = depth; + return 1; +} + + +int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dSphereClass); + dIASSERT (o2->type == dPlaneClass); + dxSphere *sphere = (dxSphere*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + dReal k = dDOT (o1->pos,plane->p); + dReal depth = plane->p[3] - k + sphere->radius; + if (depth >= 0) { + contact->normal[0] = plane->p[0]; + contact->normal[1] = plane->p[1]; + contact->normal[2] = plane->p[2]; + contact->pos[0] = o1->pos[0] - plane->p[0] * sphere->radius; + contact->pos[1] = o1->pos[1] - plane->p[1] * sphere->radius; + contact->pos[2] = o1->pos[2] - plane->p[2] * sphere->radius; + contact->depth = depth; + return 1; + } + else return 0; +} + + +int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dBoxClass); + dIASSERT (o2->type == dBoxClass); + dVector3 normal; + dReal depth; + int code; + dxBox *b1 = (dxBox*) o1; + dxBox *b2 = (dxBox*) o2; + int num = dBoxBox (o1->pos,o1->R,b1->side, o2->pos,o2->R,b2->side, + normal,&depth,&code,flags & NUMC_MASK,contact,skip); + for (int i=0; inormal[0] = -normal[0]; + CONTACT(contact,i*skip)->normal[1] = -normal[1]; + CONTACT(contact,i*skip)->normal[2] = -normal[2]; + CONTACT(contact,i*skip)->g1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + return num; +} + + +int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dBoxClass); + dIASSERT (o2->type == dPlaneClass); + dxBox *box = (dxBox*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + int ret = 0; + + //@@@ problem: using 4-vector (plane->p) as 3-vector (normal). + const dReal *R = o1->R; // rotation of box + const dReal *n = plane->p; // normal vector + + // project sides lengths along normal vector, get absolute values + dReal Q1 = dDOT14(n,R+0); + dReal Q2 = dDOT14(n,R+1); + dReal Q3 = dDOT14(n,R+2); + dReal A1 = box->side[0] * Q1; + dReal A2 = box->side[1] * Q2; + dReal A3 = box->side[2] * Q3; + dReal B1 = dFabs(A1); + dReal B2 = dFabs(A2); + dReal B3 = dFabs(A3); + + // early exit test + dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->pos); + if (depth < 0) return 0; + + // find number of contacts requested + int maxc = flags & NUMC_MASK; + if (maxc < 1) maxc = 1; + if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed + + // find deepest point + dVector3 p; + p[0] = o1->pos[0]; + p[1] = o1->pos[1]; + p[2] = o1->pos[2]; +#define FOO(i,op) \ + p[0] op REAL(0.5)*box->side[i] * R[0+i]; \ + p[1] op REAL(0.5)*box->side[i] * R[4+i]; \ + p[2] op REAL(0.5)*box->side[i] * R[8+i]; +#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) } + BAR(0,1); + BAR(1,2); + BAR(2,3); +#undef FOO +#undef BAR + + // the deepest point is the first contact point + contact->pos[0] = p[0]; + contact->pos[1] = p[1]; + contact->pos[2] = p[2]; + contact->normal[0] = n[0]; + contact->normal[1] = n[1]; + contact->normal[2] = n[2]; + contact->depth = depth; + ret = 1; // ret is number of contact points found so far + if (maxc == 1) goto done; + + // get the second and third contact points by starting from `p' and going + // along the two sides with the smallest projected length. + +#define FOO(i,j,op) \ + CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \ + CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \ + CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j]; +#define BAR(ctact,side,sideinc) \ + depth -= B ## sideinc; \ + if (depth < 0) goto done; \ + if (A ## sideinc > 0) { FOO(ctact,side,+) } else { FOO(ctact,side,-) } \ + CONTACT(contact,ctact*skip)->depth = depth; \ + ret++; + + CONTACT(contact,skip)->normal[0] = n[0]; + CONTACT(contact,skip)->normal[1] = n[1]; + CONTACT(contact,skip)->normal[2] = n[2]; + if (maxc == 3) { + CONTACT(contact,2*skip)->normal[0] = n[0]; + CONTACT(contact,2*skip)->normal[1] = n[1]; + CONTACT(contact,2*skip)->normal[2] = n[2]; + } + + if (B1 < B2) { + if (B3 < B1) goto use_side_3; else { + BAR(1,0,1); // use side 1 + if (maxc == 2) goto done; + if (B2 < B3) goto contact2_2; else goto contact2_3; + } + } + else { + if (B3 < B2) { + use_side_3: // use side 3 + BAR(1,2,3); + if (maxc == 2) goto done; + if (B1 < B2) goto contact2_1; else goto contact2_2; + } + else { + BAR(1,1,2); // use side 2 + if (maxc == 2) goto done; + if (B1 < B3) goto contact2_1; else goto contact2_3; + } + } + + contact2_1: BAR(2,0,1); goto done; + contact2_2: BAR(2,1,2); goto done; + contact2_3: BAR(2,2,3); goto done; +#undef FOO +#undef BAR + + done: + for (int i=0; ig1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + return ret; +} + + +int dCollideCCylinderSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dSphereClass); + dxCCylinder *ccyl = (dxCCylinder*) o1; + dxSphere *sphere = (dxSphere*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // find the point on the cylinder axis that is closest to the sphere + dReal alpha = + o1->R[2] * (o2->pos[0] - o1->pos[0]) + + o1->R[6] * (o2->pos[1] - o1->pos[1]) + + o1->R[10] * (o2->pos[2] - o1->pos[2]); + dReal lz2 = ccyl->lz * REAL(0.5); + if (alpha > lz2) alpha = lz2; + if (alpha < -lz2) alpha = -lz2; + + // collide the spheres + dVector3 p; + p[0] = o1->pos[0] + alpha * o1->R[2]; + p[1] = o1->pos[1] + alpha * o1->R[6]; + p[2] = o1->pos[2] + alpha * o1->R[10]; + return dCollideSpheres (p,ccyl->radius,o2->pos,sphere->radius,contact); +} + + +int dCollideCCylinderBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dBoxClass); + dxCCylinder *cyl = (dxCCylinder*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // get p1,p2 = cylinder axis endpoints, get radius + dVector3 p1,p2; + dReal clen = cyl->lz * REAL(0.5); + p1[0] = o1->pos[0] + clen * o1->R[2]; + p1[1] = o1->pos[1] + clen * o1->R[6]; + p1[2] = o1->pos[2] + clen * o1->R[10]; + p2[0] = o1->pos[0] - clen * o1->R[2]; + p2[1] = o1->pos[1] - clen * o1->R[6]; + p2[2] = o1->pos[2] - clen * o1->R[10]; + dReal radius = cyl->radius; + + // copy out box center, rotation matrix, and side array + dReal *c = o2->pos; + dReal *R = o2->R; + const dReal *side = box->side; + + // get the closest point between the cylinder axis and the box + dVector3 pl,pb; + dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb); + + // generate contact point + return dCollideSpheres (pl,radius,pb,0,contact); +} + + +int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + int i; + const dReal tolerance = REAL(1e-5); + + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dCCylinderClass); + dxCCylinder *cyl1 = (dxCCylinder*) o1; + dxCCylinder *cyl2 = (dxCCylinder*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + // copy out some variables, for convenience + dReal lz1 = cyl1->lz * REAL(0.5); + dReal lz2 = cyl2->lz * REAL(0.5); + dReal *pos1 = o1->pos; + dReal *pos2 = o2->pos; + dReal axis1[3],axis2[3]; + axis1[0] = o1->R[2]; + axis1[1] = o1->R[6]; + axis1[2] = o1->R[10]; + axis2[0] = o2->R[2]; + axis2[1] = o2->R[6]; + axis2[2] = o2->R[10]; + + // if the cylinder axes are close to parallel, we'll try to detect up to + // two contact points along the body of the cylinder. if we can't find any + // points then we'll fall back to the closest-points algorithm. note that + // we are not treating this special case for reasons of degeneracy, but + // because we want two contact points in some situations. the closet-points + // algorithm is robust in all casts, but it can return only one contact. + + dVector3 sphere1,sphere2; + dReal a1a2 = dDOT (axis1,axis2); + dReal det = REAL(1.0)-a1a2*a1a2; + if (det < tolerance) { + // the cylinder axes (almost) parallel, so we will generate up to two + // contacts. alpha1 and alpha2 (line position parameters) are related by: + // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) + // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) + // first compute where the two cylinders overlap in alpha1 space: + if (a1a2 < 0) { + axis2[0] = -axis2[0]; + axis2[1] = -axis2[1]; + axis2[2] = -axis2[2]; + } + dReal q[3]; + for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; + dReal k = dDOT (axis1,q); + dReal a1lo = -lz1; + dReal a1hi = lz1; + dReal a2lo = -lz2 - k; + dReal a2hi = lz2 - k; + dReal lo = (a1lo > a2lo) ? a1lo : a2lo; + dReal hi = (a1hi < a2hi) ? a1hi : a2hi; + if (lo <= hi) { + int num_contacts = flags & NUMC_MASK; + if (num_contacts >= 2 && lo < hi) { + // generate up to two contacts. if one of those contacts is + // not made, fall back on the one-contact strategy. + for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; + int n1 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + if (n1) { + for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; + dContactGeom *c2 = CONTACT(contact,skip); + int n2 = dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius, c2); + if (n2) { + c2->g1 = o1; + c2->g2 = o2; + return 2; + } + } + } + + // just one contact to generate, so put it in the middle of + // the range + dReal alpha1 = (lo + hi) * REAL(0.5); + dReal alpha2 = alpha1 + k; + for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; + for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; + return dCollideSpheres (sphere1,cyl1->radius, + sphere2,cyl2->radius,contact); + } + } + + // use the closest point algorithm + dVector3 a1,a2,b1,b2; + a1[0] = o1->pos[0] + axis1[0]*lz1; + a1[1] = o1->pos[1] + axis1[1]*lz1; + a1[2] = o1->pos[2] + axis1[2]*lz1; + a2[0] = o1->pos[0] - axis1[0]*lz1; + a2[1] = o1->pos[1] - axis1[1]*lz1; + a2[2] = o1->pos[2] - axis1[2]*lz1; + b1[0] = o2->pos[0] + axis2[0]*lz2; + b1[1] = o2->pos[1] + axis2[1]*lz2; + b1[2] = o2->pos[2] + axis2[2]*lz2; + b2[0] = o2->pos[0] - axis2[0]*lz2; + b2[1] = o2->pos[1] - axis2[1]*lz2; + b2[2] = o2->pos[2] - axis2[2]*lz2; + + dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2); + return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); +} + + +int dCollideCCylinderPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dPlaneClass); + dxCCylinder *ccyl = (dxCCylinder*) o1; + dxPlane *plane = (dxPlane*) o2; + + // collide the deepest capping sphere with the plane + dReal sign = (dDOT14 (plane->p,o1->R+2) > 0) ? REAL(-1.0) : REAL(1.0); + dVector3 p; + p[0] = o1->pos[0] + o1->R[2] * ccyl->lz * REAL(0.5) * sign; + p[1] = o1->pos[1] + o1->R[6] * ccyl->lz * REAL(0.5) * sign; + p[2] = o1->pos[2] + o1->R[10] * ccyl->lz * REAL(0.5) * sign; + + dReal k = dDOT (p,plane->p); + dReal depth = plane->p[3] - k + ccyl->radius; + if (depth < 0) return 0; + contact->normal[0] = plane->p[0]; + contact->normal[1] = plane->p[1]; + contact->normal[2] = plane->p[2]; + contact->pos[0] = p[0] - plane->p[0] * ccyl->radius; + contact->pos[1] = p[1] - plane->p[1] * ccyl->radius; + contact->pos[2] = p[2] - plane->p[2] * ccyl->radius; + contact->depth = depth; + + int ncontacts = 1; + if ((flags & NUMC_MASK) >= 2) { + // collide the other capping sphere with the plane + p[0] = o1->pos[0] - o1->R[2] * ccyl->lz * REAL(0.5) * sign; + p[1] = o1->pos[1] - o1->R[6] * ccyl->lz * REAL(0.5) * sign; + p[2] = o1->pos[2] - o1->R[10] * ccyl->lz * REAL(0.5) * sign; + + k = dDOT (p,plane->p); + depth = plane->p[3] - k + ccyl->radius; + if (depth >= 0) { + dContactGeom *c2 = CONTACT(contact,skip); + c2->normal[0] = plane->p[0]; + c2->normal[1] = plane->p[1]; + c2->normal[2] = plane->p[2]; + c2->pos[0] = p[0] - plane->p[0] * ccyl->radius; + c2->pos[1] = p[1] - plane->p[1] * ccyl->radius; + c2->pos[2] = p[2] - plane->p[2] * ccyl->radius; + c2->depth = depth; + ncontacts = 2; + } + } + + for (int i=0; i < ncontacts; i++) { + CONTACT(contact,i*skip)->g1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + return ncontacts; +} + + +// if mode==1 then use the sphere exit contact, not the entry contact + +static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, + dContactGeom *contact, int mode) +{ + dVector3 q; + q[0] = ray->pos[0] - sphere_pos[0]; + q[1] = ray->pos[1] - sphere_pos[1]; + q[2] = ray->pos[2] - sphere_pos[2]; + dReal B = dDOT14(q,ray->R+2); + dReal C = dDOT(q,q) - radius*radius; + // note: if C <= 0 then the start of the ray is inside the sphere + dReal k = B*B - C; + if (k < 0) return 0; + k = dSqrt(k); + dReal alpha; + if (mode && C >= 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + else { + alpha = -B - k; + if (alpha < 0) { + alpha = -B + k; + if (alpha < 0) return 0; + } + } + if (alpha > ray->length) return 0; + contact->pos[0] = ray->pos[0] + alpha*ray->R[0*4+2]; + contact->pos[1] = ray->pos[1] + alpha*ray->R[1*4+2]; + contact->pos[2] = ray->pos[2] + alpha*ray->R[2*4+2]; + dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0); + contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]); + contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]); + contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; +} + + +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dSphereClass); + dxRay *ray = (dxRay*) o1; + dxSphere *sphere = (dxSphere*) o2; + contact->g1 = ray; + contact->g2 = sphere; + return ray_sphere_helper (ray,sphere->pos,sphere->radius,contact,0); +} + + +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dBoxClass); + dxRay *ray = (dxRay*) o1; + dxBox *box = (dxBox*) o2; + + contact->g1 = ray; + contact->g2 = box; + + int i; + + // compute the start and delta of the ray relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = ray->pos[0] - box->pos[0]; + tmp[1] = ray->pos[1] - box->pos[1]; + tmp[2] = ray->pos[2] - box->pos[2]; + dMULTIPLY1_331 (s,box->R,tmp); + tmp[0] = ray->R[0*4+2]; + tmp[1] = ray->R[1*4+2]; + tmp[2] = ray->R[2*4+2]; + dMULTIPLY1_331 (v,box->R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = 1; + } + else sign[i] = -1; + } + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * box->side[0]; + h[1] = REAL(0.5) * box->side[1]; + h[2] = REAL(0.5) * box->side[2]; + + // do a few early exit tests + if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || + (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || + (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || + (v[0] == 0 && v[1] == 0 && v[2] == 0)) { + return 0; + } + + // compute the t=[lo..hi] range for where s+v*t intersects the box + dReal lo = -dInfinity; + dReal hi = dInfinity; + int nlo = 0, nhi = 0; + for (i=0; i<3; i++) { + if (v[i] != 0) { + dReal k = (-h[i] - s[i])/v[i]; + if (k > lo) { + lo = k; + nlo = i; + } + k = (h[i] - s[i])/v[i]; + if (k < hi) { + hi = k; + nhi = i; + } + } + } + + // check if the ray intersects + if (lo > hi) return 0; + dReal alpha; + int n; + if (lo >= 0) { + alpha = lo; + n = nlo; + } + else { + alpha = hi; + n = nhi; + } + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->pos[0] + alpha*ray->R[0*4+2]; + contact->pos[1] = ray->pos[1] + alpha*ray->R[1*4+2]; + contact->pos[2] = ray->pos[2] + alpha*ray->R[2*4+2]; + contact->normal[0] = box->R[0*4+n] * sign[n]; + contact->normal[1] = box->R[1*4+n] * sign[n]; + contact->normal[2] = box->R[2*4+n] * sign[n]; + contact->depth = alpha; + return 1; +} + + +int dCollideRayCCylinder (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dCCylinderClass); + dxRay *ray = (dxRay*) o1; + dxCCylinder *ccyl = (dxCCylinder*) o2; + + contact->g1 = ray; + contact->g2 = ccyl; + dReal lz2 = ccyl->lz * REAL(0.5); + + // compute some useful info + dVector3 cs,q,r; + dReal C,k; + cs[0] = ray->pos[0] - ccyl->pos[0]; + cs[1] = ray->pos[1] - ccyl->pos[1]; + cs[2] = ray->pos[2] - ccyl->pos[2]; + k = dDOT41(ccyl->R+2,cs); // position of ray start along ccyl axis + q[0] = k*ccyl->R[0*4+2] - cs[0]; + q[1] = k*ccyl->R[1*4+2] - cs[1]; + q[2] = k*ccyl->R[2*4+2] - cs[2]; + C = dDOT(q,q) - ccyl->radius*ccyl->radius; + // if C < 0 then ray start position within infinite extension of cylinder + + // see if ray start position is inside the capped cylinder + int inside_ccyl = 0; + if (C < 0) { + if (k < -lz2) k = -lz2; + else if (k > lz2) k = lz2; + r[0] = ccyl->pos[0] + k*ccyl->R[0*4+2]; + r[1] = ccyl->pos[1] + k*ccyl->R[1*4+2]; + r[2] = ccyl->pos[2] + k*ccyl->R[2*4+2]; + if ((ray->pos[0]-r[0])*(ray->pos[0]-r[0]) + + (ray->pos[1]-r[1])*(ray->pos[1]-r[1]) + + (ray->pos[2]-r[2])*(ray->pos[2]-r[2]) < ccyl->radius*ccyl->radius) { + inside_ccyl = 1; + } + } + + // compute ray collision with infinite cylinder, except for the case where + // the ray is outside the capped cylinder but within the infinite cylinder + // (it that case the ray can only hit endcaps) + if (!inside_ccyl && C < 0) { + // set k to cap position to check + if (k < 0) k = -lz2; else k = lz2; + } + else { + dReal uv = dDOT44(ccyl->R+2,ray->R+2); + r[0] = uv*ccyl->R[0*4+2] - ray->R[0*4+2]; + r[1] = uv*ccyl->R[1*4+2] - ray->R[1*4+2]; + r[2] = uv*ccyl->R[2*4+2] - ray->R[2*4+2]; + dReal A = dDOT(r,r); + dReal B = 2*dDOT(q,r); + k = B*B-4*A*C; + if (k < 0) { + // the ray does not intersect the infinite cylinder, but if the ray is + // inside and parallel to the cylinder axis it may intersect the end + // caps. set k to cap position to check. + if (!inside_ccyl) return 0; + if (uv < 0) k = -lz2; else k = lz2; + } + else { + k = dSqrt(k); + A = dRecip (2*A); + dReal alpha = (-B-k)*A; + if (alpha < 0) { + alpha = (-B+k)*A; + if (alpha < 0) return 0; + } + if (alpha > ray->length) return 0; + + // the ray intersects the infinite cylinder. check to see if the + // intersection point is between the caps + contact->pos[0] = ray->pos[0] + alpha*ray->R[0*4+2]; + contact->pos[1] = ray->pos[1] + alpha*ray->R[1*4+2]; + contact->pos[2] = ray->pos[2] + alpha*ray->R[2*4+2]; + q[0] = contact->pos[0] - ccyl->pos[0]; + q[1] = contact->pos[1] - ccyl->pos[1]; + q[2] = contact->pos[2] - ccyl->pos[2]; + k = dDOT14(q,ccyl->R+2); + dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0); + if (k >= -lz2 && k <= lz2) { + contact->normal[0] = nsign * (contact->pos[0] - + (ccyl->pos[0] + k*ccyl->R[0*4+2])); + contact->normal[1] = nsign * (contact->pos[1] - + (ccyl->pos[1] + k*ccyl->R[1*4+2])); + contact->normal[2] = nsign * (contact->pos[2] - + (ccyl->pos[2] + k*ccyl->R[2*4+2])); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; + } + + // the infinite cylinder intersection point is not between the caps. + // set k to cap position to check. + if (k < 0) k = -lz2; else k = lz2; + } + } + + // check for ray intersection with the caps. k must indicate the cap + // position to check + q[0] = ccyl->pos[0] + k*ccyl->R[0*4+2]; + q[1] = ccyl->pos[1] + k*ccyl->R[1*4+2]; + q[2] = ccyl->pos[2] + k*ccyl->R[2*4+2]; + return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl); +} + + +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dPlaneClass); + dxRay *ray = (dxRay*) o1; + dxPlane *plane = (dxPlane*) o2; + + dReal alpha = plane->p[3] - dDOT (plane->p,ray->pos); + // note: if alpha > 0 the starting point is below the plane + dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0); + dReal k = dDOT14(plane->p,ray->R+2); + if (k==0) return 0; // ray parallel to plane + alpha /= k; + if (alpha < 0 || alpha > ray->length) return 0; + contact->pos[0] = ray->pos[0] + alpha*ray->R[0*4+2]; + contact->pos[1] = ray->pos[1] + alpha*ray->R[1*4+2]; + contact->pos[2] = ray->pos[2] + alpha*ray->R[2*4+2]; + contact->normal[0] = nsign*plane->p[0]; + contact->normal[1] = nsign*plane->p[1]; + contact->normal[2] = nsign*plane->p[2]; + contact->depth = alpha; + contact->g1 = ray; + contact->g2 = plane; + return 1; +} + +//// CYLINDER CODE ADDED +//**************************************************************************** +// flat cylinder public API + +dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) : +dxGeom (space,1) +{ + dAASSERT (_radius > 0 && _length > 0); + type = dCylinderClass; + radius = _radius; + lz = _length; +} + + +void dxCylinder::computeAABB() +{ + dReal xrange = dFabs (R[0] * radius) + dFabs (R[1] * radius) + REAL(0.5)* dFabs (R[2] * + lz); + dReal yrange = dFabs (R[4] * radius) + dFabs (R[5] * radius) + REAL(0.5)* dFabs (R[6] * + lz); + dReal zrange = dFabs (R[8] * radius) + dFabs (R[9] * radius) + REAL(0.5)* dFabs (R[10] * + lz); + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + + +dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length) +{ + return new dxCylinder (space,radius,length); +} + +void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length) +{ + dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); + dAASSERT (radius > 0 && length > 0); + dxCylinder *c = (dxCylinder*) cylinder; + c->radius = radius; + c->lz = length; + dGeomMoved (cylinder); +} + +void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length) +{ + dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder"); + dxCylinder *c = (dxCylinder*) cylinder; + *radius = c->radius; + *length = c->lz; +} + +// int dCollideCylinderPlane(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip) +// { +// dIASSERT (skip >= (int)sizeof(dContactGeom)); +// dIASSERT (dGeomGetClass(o1) == dCylinderClass); +// dIASSERT (dGeomGetClass(o2) == dPlaneClass); +// contact->g1 = const_cast (o1); +// contact->g2 = const_cast (o2); + +// int ret = 0; + +// dReal radius; +// dReal hlz; +// dGeomCylinderGetParams(o1,&radius,&hlz); +// hlz /= 2; + +// const dReal *R2 = dGeomGetRotation(o1);// rotation of cylinder +// dMatrix3 R; +// for (int j = 0; j < 12;j++) +// R[j] = R2[j]; + +// //rotate R about its own axis +// //dMatrix3 R3; +// //dRFromAxisAndAngle (R3,R2[0],R2[1],R2[2],180); +// //dRFromAxisAndAngle (R3,0,0,1,0); +// //dMultiply0 (R,R2,R3,3,3,3); + + +// //to make this cylinder code work here we just rotate it 90 degrees +// dReal tmp[3]; +// tmp[0] = R[4]; +// tmp[1] = R[5]; +// tmp[2] = R[6]; +// R[4] = R[8]; +// R[5] = R[9]; +// R[6] = R[10]; +// R[8] = tmp[0]; +// R[9] = tmp[1]; +// R[10] = tmp[2]; +// // R[0] *= -1; +// // R[1] *= -1; +// // R[2] *= -1; + +// const dReal* p = dGeomGetPosition(o1); +// dVector4 n; // normal vector +// dReal pp; +// dGeomPlaneGetParams (o2, n); +// pp=n[3]; +// dReal cos1,sin1; +// cos1=dFabs(dDOT14(n,R+1)); + +// cos1=cos10 ? hlz*R[1]:-hlz*R[1]; +// pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5]; +// pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9]; + +// contact->pos[0] = pos[0]; +// contact->pos[1] = pos[1]; +// contact->pos[2] = pos[2]; +// contact->depth = outDepth; +// ret=1; + +// if(dFabs(Q2)>M_SQRT1_2){ + +// if (ret <= (flags & NUMC_MASK)){ + +// CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0]; +// CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4]; +// CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8]; +// CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1); + +// if(CONTACT(contact,ret*skip)->depth>0.f) +// ret++; +// } + +// if (ret <= (flags & NUMC_MASK)){ + +// CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2]; +// CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6]; +// CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10]; +// CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3); + +// if(CONTACT(contact,ret*skip)->depth>0.f) ret++; +// } + +// } else { + +// if (ret <= (flags & NUMC_MASK)){ + +// CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]); +// CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]); +// CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]); +// CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2); + +// if(CONTACT(contact,ret*skip)->depth>0.f) ret++; +// } + +// } +// for (int i=0; ig1 = const_cast (o1); +// CONTACT(contact,i*skip)->g2 = const_cast (o2); +// CONTACT(contact,i*skip)->normal[0] =n[0]; +// CONTACT(contact,i*skip)->normal[1] =n[1]; +// CONTACT(contact,i*skip)->normal[2] =n[2]; +// } +// return ret; +// } diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_std.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_std.h new file mode 100644 index 00000000..fef64e76 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_std.h @@ -0,0 +1,79 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +the standard ODE geometry primitives. + +*/ + +#ifndef _ODE_COLLISION_STD_H_ +#define _ODE_COLLISION_STD_H_ + +#include "ode/ode_common.h" +#include "ode/ode_collision_kernel.h" + + +// primitive collision functions - these have the dColliderFn interface, i.e. +// the same interface as dCollide(). the first and second geom arguments must +// have the specified types. + +int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCCylinderSphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideCCylinderBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideCCylinderPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); +int dCollideRayCCylinder (dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip); +int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); + +// Cylinder - Box by (C) CroTeam +// Ported by Nguyen Binh +int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +// Cylinder - Sphere (C) by CroTeam +// Ported by Nguyen Binh +int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, int flags, dContactGeom + *contact, int skip); + +int dCollideCylinderPlane(dxGeom *gCylinder, dxGeom *gSphere, int flags, dContactGeom + *contact, int skip); + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_transform.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_transform.cpp new file mode 100644 index 00000000..2853e3db --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_transform.cpp @@ -0,0 +1,235 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +geom transform + +*/ + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_transform.h" +#include "ode/ode_collision_util.h" + +// 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 + +//**************************************************************************** +// dxGeomTransform class + +struct dxGeomTransform : public dxGeom { + dxGeom *obj; // object that is being transformed + int cleanup; // 1 to destroy obj when destroyed + int infomode; // 1 to put Tx geom in dContactGeom g1 + + // cached final object transform (body tx + relative tx). this is set by + // computeAABB(), and it is valid while the AABB is valid. + dVector3 final_pos; + dMatrix3 final_R; + + dxGeomTransform (dSpaceID space); + ~dxGeomTransform(); + void computeAABB(); + void computeFinalTx(); +}; + + +dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1) +{ + type = dGeomTransformClass; + obj = 0; + cleanup = 0; + infomode = 0; + dSetZero (final_pos,4); + dRSetIdentity (final_R); +} + + +dxGeomTransform::~dxGeomTransform() +{ + if (obj && cleanup) delete obj; +} + + +void dxGeomTransform::computeAABB() +{ + if (!obj) { + dSetZero (aabb,6); + return; + } + + // backup the relative pos and R pointers of the encapsulated geom object + dReal *posbak = obj->pos; + dReal *Rbak = obj->R; + + // compute temporary pos and R for the encapsulated geom object + computeFinalTx(); + obj->pos = final_pos; + obj->R = final_R; + + // compute the AABB + obj->computeAABB(); + memcpy (aabb,obj->aabb,6*sizeof(dReal)); + + // restore the pos and R + obj->pos = posbak; + obj->R = Rbak; +} + + +// utility function for dCollideTransform() : compute final pos and R +// for the encapsulated geom object + +void dxGeomTransform::computeFinalTx() +{ + dMULTIPLY0_331 (final_pos,R,obj->pos); + final_pos[0] += pos[0]; + final_pos[1] += pos[1]; + final_pos[2] += pos[2]; + dMULTIPLY0_333 (final_R,R,obj->R); +} + +//**************************************************************************** +// collider function: +// this collides a transformed geom with another geom. the other geom can +// also be a transformed geom, but this case is not handled specially. + +int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dGeomTransformClass); + + dxGeomTransform *tr = (dxGeomTransform*) o1; + if (!tr->obj) return 0; + dUASSERT (tr->obj->parent_space==0, + "GeomTransform encapsulated object must not be in a space"); + dUASSERT (tr->obj->body==0, + "GeomTransform encapsulated object must not be attached " + "to a body"); + + // backup the relative pos and R pointers of the encapsulated geom object, + // and the body pointer + dReal *posbak = tr->obj->pos; + dReal *Rbak = tr->obj->R; + dxBody *bodybak = tr->obj->body; + + // compute temporary pos and R for the encapsulated geom object. + // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag, + // because computeFinalTx() will have already been called in + // dxGeomTransform::computeAABB() + + if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx(); + tr->obj->pos = tr->final_pos; + tr->obj->R = tr->final_R; + tr->obj->body = o1->body; + + // do the collision + int n = dCollide (tr->obj,o2,flags,contact,skip); + + // if required, adjust the 'g1' values in the generated contacts so that + // thay indicated the GeomTransform object instead of the encapsulated + // object. + if (tr->infomode) { + for (int i=0; ig1 = o1; + } + } + + // restore the pos, R and body + tr->obj->pos = posbak; + tr->obj->R = Rbak; + tr->obj->body = bodybak; + return n; +} + +//**************************************************************************** +// public API + +dGeomID dCreateGeomTransform (dSpaceID space) +{ + return new dxGeomTransform (space); +} + + +void dGeomTransformSetGeom (dGeomID g, dGeomID obj) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + if (tr->obj && tr->cleanup) delete tr->obj; + tr->obj = obj; +} + + +dGeomID dGeomTransformGetGeom (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->obj; +} + + +void dGeomTransformSetCleanup (dGeomID g, int mode) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + tr->cleanup = mode; +} + + +int dGeomTransformGetCleanup (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->cleanup; +} + + +void dGeomTransformSetInfo (dGeomID g, int mode) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + tr->infomode = mode; +} + + +int dGeomTransformGetInfo (dGeomID g) +{ + dUASSERT (g && g->type == dGeomTransformClass, + "argument not a geom transform"); + dxGeomTransform *tr = (dxGeomTransform*) g; + return tr->infomode; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_transform.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_transform.h new file mode 100644 index 00000000..0c65b0f7 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_transform.h @@ -0,0 +1,40 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +geom transform + +*/ + +#ifndef _ODE_COLLISION_TRANSFORM_H_ +#define _ODE_COLLISION_TRANSFORM_H_ + +#include "ode/ode_common.h" +#include "ode/ode_collision_kernel.h" + + +int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip); + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh.cpp new file mode 100644 index 00000000..161f3dac --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh.cpp @@ -0,0 +1,547 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + + // 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 + +// TriMesh code by Erwin de Vries. + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +// Trimesh data +dxTriMeshData::dxTriMeshData(){ +#ifndef dTRIMESH_ENABLED + dUASSERT(g, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work"); +#endif +} + +dxTriMeshData::~dxTriMeshData(){ + // +} + +void +dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* in_Normals, + bool Single){ + Mesh.SetNbTriangles(IndexCount / 3); + Mesh.SetNbVertices(VertexCount); + Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices); + Mesh.SetStrides(TriStride, VertexStide); + Mesh.Single = Single; + + // Build tree + BuildSettings Settings; + // recommended in Opcode User Manual + //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER; + // used in ODE, why? + //Settings.mRules = SPLIT_BEST_AXIS; + + // best compromise? + Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER; + + + OPCODECREATE TreeBuilder; + TreeBuilder.mIMesh = &Mesh; + + TreeBuilder.mSettings = Settings; + TreeBuilder.mNoLeaf = true; + TreeBuilder.mQuantized = false; + + TreeBuilder.mKeepOriginal = false; + TreeBuilder.mCanRemap = false; + + + + BVTree.Build(TreeBuilder); + + // compute model space AABB + dVector3 AABBMax, AABBMin; + AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity; + AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity; + if( Single ) { + const char* verts = (const char*)Vertices; + for( int i = 0; i < VertexCount; ++i ) { + const float* v = (const float*)verts; + if( v[0] > AABBMax[0] ) AABBMax[0] = v[0]; + if( v[1] > AABBMax[1] ) AABBMax[1] = v[1]; + if( v[2] > AABBMax[2] ) AABBMax[2] = v[2]; + if( v[0] < AABBMin[0] ) AABBMin[0] = v[0]; + if( v[1] < AABBMin[1] ) AABBMin[1] = v[1]; + if( v[2] < AABBMin[2] ) AABBMin[2] = v[2]; + verts += VertexStide; + } + } else { + const char* verts = (const char*)Vertices; + for( int i = 0; i < VertexCount; ++i ) { + const double* v = (const double*)verts; + if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0]; + if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1]; + if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2]; + if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0]; + if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1]; + if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2]; + verts += VertexStide; + } + } + AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5); + AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5); + AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5); + AABBExtents[0] = AABBMax[0] - AABBCenter[0]; + AABBExtents[1] = AABBMax[1] - AABBCenter[1]; + AABBExtents[2] = AABBMax[2] - AABBCenter[2]; + + // user data (not used by OPCODE) + for (int i=0; i<16; i++) + last_trans[i] = 0.0; + + Normals = (dReal *) in_Normals; +} + +dTriMeshDataID dGeomTriMeshDataCreate(){ + return new dxTriMeshData(); +} + +void dGeomTriMeshDataDestroy(dTriMeshDataID g){ + delete g; +} + +void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) +{ + dUASSERT(g, "argument not trimesh data"); + + double *elem; + + switch (data_id) { + case TRIMESH_FACE_NORMALS: + g->Normals = (dReal *) in_data; + break; + + case TRIMESH_LAST_TRANSFORMATION: + elem = (double *) in_data; + for (int i=0; i<16; i++) + g->last_trans[i] = (dReal) elem[i]; + + break; + default: + dUASSERT(data_id, "invalid data type"); + break; + } + + return; +} + + + +void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) +{ + dUASSERT(g, "argument not trimesh data"); + + switch (data_id) { + case TRIMESH_FACE_NORMALS: + return (void *) g->Normals; + break; + + case TRIMESH_LAST_TRANSFORMATION: + return (void *) g->last_trans; + break; + default: + dUASSERT(data_id, "invalid data type"); + break; +} + + return NULL; +} + + +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + dUASSERT(g, "argument not trimesh data"); + + g->Build(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + true); +} + + +void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, (void*)NULL); +} + + +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals){ + dUASSERT(g, "argument not trimesh data"); + + g->Build(Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, + Normals, + false); +} + + +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) { + dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount, + Indices, IndexCount, TriStride, NULL); +} + + +void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount, + const int* Normals){ +#ifdef dSINGLE + dGeomTriMeshDataBuildSingle1(g, + Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(unsigned int), + Normals); +#else + dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, + Indices, IndexCount, 3 * sizeof(unsigned int), + Normals); +#endif +} + + +void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount) { + dGeomTriMeshDataBuildSimple1(g, + Vertices, VertexCount, Indices, IndexCount, + (const int*)NULL); +} + + +// Trimesh +PlanesCollider dxTriMesh::_PlanesCollider; +SphereCollider dxTriMesh::_SphereCollider; +//OBBCollider dxTriMesh::_OBBCollider; +RayCollider dxTriMesh::_RayCollider; +AABBTreeCollider dxTriMesh::_AABBTreeCollider; +LSSCollider dxTriMesh::_LSSCollider; + +SphereCache dxTriMesh::defaultSphereCache; +//OBBCache dxTriMesh::defaultBoxCache; +LSSCache dxTriMesh::defaultCCylinderCache; + +CollisionFaces dxTriMesh::Faces; + +dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1){ + type = dTriMeshClass; + + this->Data = Data; + + _RayCollider.SetDestination(&Faces); + + _PlanesCollider.SetTemporalCoherence(true); + + _SphereCollider.SetTemporalCoherence(true); + _SphereCollider.SetPrimitiveTests(false); + + + _OBBCollider.SetTemporalCoherence(true); + + // no first-contact test (i.e. return full contact info) + _AABBTreeCollider.SetFirstContact( false ); + // temporal coherence only works with "first conact" tests + _AABBTreeCollider.SetTemporalCoherence(false); + // Perform full BV-BV tests (true) or SAT-lite tests (false) + _AABBTreeCollider.SetFullBoxBoxTest( true ); + // Perform full Primitive-BV tests (true) or SAT-lite tests (false) + _AABBTreeCollider.SetFullPrimBoxTest( true ); + _LSSCollider.SetTemporalCoherence(false); + + /* TC has speed/space 'issues' that don't make it a clear + win by default on spheres/boxes. */ + this->doSphereTC = false; + this->doBoxTC = false; + this->doCCylinderTC = false; + + + this->forceNormalMode = false; + + const char* msg; + if ((msg =_AABBTreeCollider.ValidateSettings())) + dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__); + _LSSCollider.SetPrimitiveTests(false); + _LSSCollider.SetFirstContact(false); +} + +dxTriMesh::~dxTriMesh(){ + // +} + + +void dxTriMesh::ClearTCCache(){ + /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches - + but the destructor isn't called when doing this, so we would leak. + So, call the previous caches' containers' destructors by hand first. */ + int i, n; + n = SphereTCCache.size(); + for( i = 0; i < n; ++i ) { + SphereTCCache[i].~SphereTC(); + } + SphereTCCache.setSize(0); + n = BoxTCCache.size(); + for( i = 0; i < n; ++i ) { + BoxTCCache[i].~BoxTC(); + } + BoxTCCache.setSize(0); + n = CCylinderTCCache.size(); + for( i = 0; i < n; ++i ) { + CCylinderTCCache[i].~CCylinderTC(); + } + CCylinderTCCache.setSize(0); +} + + +int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){ + return 1; +} + + +void dxTriMesh::computeAABB() { + const dxTriMeshData* d = Data; + dVector3 c; + + dMULTIPLY0_331( c, R, d->AABBCenter ); + + dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) + + dFabs(R[1] * Data->AABBExtents[1]) + + dFabs(R[2] * Data->AABBExtents[2]); + dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) + + dFabs(R[5] * Data->AABBExtents[1]) + + dFabs(R[6] * Data->AABBExtents[2]); + dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) + + dFabs(R[9] * Data->AABBExtents[1]) + + dFabs(R[10] * Data->AABBExtents[2]); + + aabb[0] = c[0] + pos[0] - xrange; + aabb[1] = c[0] + pos[0] + xrange; + aabb[2] = c[1] + pos[1] - yrange; + aabb[3] = c[1] + pos[1] + yrange; + aabb[4] = c[2] + pos[2] - zrange; + aabb[5] = c[2] + pos[2] + zrange; +} + +dGeomID dCreateTriMesh(dSpaceID space, + dTriMeshDataID Data, + dTriCallback* Callback, + dTriArrayCallback* ArrayCallback, + dTriRayCallback* RayCallback) +{ + dxTriMesh* Geom = new dxTriMesh(space, Data); + Geom->Callback = Callback; + Geom->ArrayCallback = ArrayCallback; + Geom->RayCallback = RayCallback; + + return Geom; +} + +void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->Callback = Callback; +} + +dTriCallback* dGeomTriMeshGetCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->Callback; +} + +void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->ArrayCallback = ArrayCallback; +} + +dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->ArrayCallback; +} + +void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->RayCallback = Callback; +} + +dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->RayCallback; +} + +void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + ((dxTriMesh*)g)->Data = Data; +} + +dTriMeshDataID dGeomTriMeshGetData(dGeomID g) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + return ((dxTriMesh*)g)->Data; +} + + + +void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + switch (geomClass) + { + case dSphereClass: + ((dxTriMesh*)g)->doSphereTC = (1 == enable); + break; + case dBoxClass: + ((dxTriMesh*)g)->doBoxTC = (1 == enable); + break; + case dCCylinderClass: + ((dxTriMesh*)g)->doCCylinderTC = (1 == enable); + break; + } +} + +int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass) +{ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + switch (geomClass) + { + case dSphereClass: + if (((dxTriMesh*)g)->doSphereTC) + return 1; + break; + case dBoxClass: + if (((dxTriMesh*)g)->doBoxTC) + return 1; + break; + case dCCylinderClass: + if (((dxTriMesh*)g)->doCCylinderTC) + return 1; + break; + } + return 0; +} + +void dGeomTriMeshClearTCCache(dGeomID g){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + Geom->ClearTCCache(); +} + +void dGeomTriMeshSetForceNormalMode(dGeomID g, int enable){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + Geom->setForceNormalMode(enable); +} + +/* + * returns the TriMeshDataID + */ +dTriMeshDataID +dGeomTriMeshGetTriMeshDataID(dGeomID g) +{ + dxTriMesh* Geom = (dxTriMesh*) g; + return Geom->Data; +} + +// Getting data +void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + + dVector3 v[3]; + FetchTriangle(Geom, Index, Position, Rotation, v); + + if (v0){ + (*v0)[0] = v[0][0]; + (*v0)[1] = v[0][1]; + (*v0)[2] = v[0][2]; + (*v0)[3] = v[0][3]; + } + if (v1){ + (*v1)[0] = v[1][0]; + (*v1)[1] = v[1][1]; + (*v1)[2] = v[1][2]; + (*v1)[3] = v[1][3]; + } + if (v2){ + (*v2)[0] = v[2][0]; + (*v2)[1] = v[2][1]; + (*v2)[2] = v[2][2]; + (*v2)[3] = v[2][3]; + } +} + +void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){ + dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); + + dxTriMesh* Geom = (dxTriMesh*)g; + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + + dVector3 dv[3]; + FetchTriangle(Geom, Index, Position, Rotation, dv); + + GetPointFromBarycentric(dv, u, v, Out); +} + +int dGeomTriMeshGetTriangleCount (dGeomID g) +{ + dxTriMesh* Geom = (dxTriMesh*)g; + return Geom->Data->Mesh.GetNbTriangles(); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh.h new file mode 100644 index 00000000..cf61aa55 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh.h @@ -0,0 +1,191 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + * TriMesh code by Erwin de Vries. + * + * Trimesh data. + * This is where the actual vertexdata (pointers), and BV tree is stored. + * Vertices should be single precision! + * This should be more sophisticated, so that the user can easyly implement + * another collision library, but this is a lot of work, and also costs some + * performance because some data has to be copied. + */ + +#ifndef _ODE_COLLISION_TRIMESH_H_ +#define _ODE_COLLISION_TRIMESH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Data storage for triangle meshes. + */ +struct dxTriMeshData; +typedef struct dxTriMeshData* dTriMeshDataID; + +/* + * These dont make much sense now, but they will later when we add more + * features. + */ +dTriMeshDataID dGeomTriMeshDataCreate(void); +void dGeomTriMeshDataDestroy(dTriMeshDataID g); + +enum { TRIMESH_FACE_NORMALS, TRIMESH_LAST_TRANSFORMATION }; +void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data); +void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id); + +/* + * Build TriMesh data with single pricision used in vertex data . + */ +void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride); +/* same again with a normals array (used as trimesh-trimesh optimization) */ +void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals); +/* +* Build TriMesh data with double pricision used in vertex data . +*/ +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride); +/* same again with a normals array (used as trimesh-trimesh optimization) */ +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals); + +/* + * Simple build. Single/double precision based on dSINGLE/dDOUBLE! + */ +void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount); +/* same again with a normals array (used as trimesh-trimesh optimization) */ +void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, + const dReal* Vertices, int VertexCount, + const int* Indices, int IndexCount, + const int* Normals); +/* + * Per triangle callback. Allows the user to say if he wants a collision with + * a particular triangle. + */ +typedef int dTriCallback(dGeomID TriMesh, dGeomID RefObject, int TriangleIndex); +void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback); +dTriCallback* dGeomTriMeshGetCallback(dGeomID g); + +/* + * Per object callback. Allows the user to get the list of triangles in 1 + * shot. Maybe we should remove this one. + */ +typedef void dTriArrayCallback(dGeomID TriMesh, dGeomID RefObject, const int* TriIndices, int TriCount); +void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback); +dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g); + +/* + * Ray callback. + * Allows the user to say if a ray collides with a triangle on barycentric + * coords. The user can for example sample a texture with alpha transparency + * to determine if a collision should occur. + */ +typedef int dTriRayCallback(dGeomID TriMesh, dGeomID Ray, int TriangleIndex, dReal u, dReal v); +void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback); +dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g); + +/* + * Trimesh class + * Construction. Callbacks are optional. + */ +dGeomID dCreateTriMesh(dSpaceID space, dTriMeshDataID Data, dTriCallback* Callback, dTriArrayCallback* ArrayCallback, dTriRayCallback* RayCallback); + +void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data); +dTriMeshDataID dGeomTriMeshGetData(dGeomID g); + + +// enable/disable/check temporal coherence +void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable); +int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass); + +/* + * Clears the internal temporal coherence caches. When a geom has its + * collision checked with a trimesh once, data is stored inside the trimesh. + * With large worlds with lots of seperate objects this list could get huge. + * We should be able to do this automagically. + */ +void dGeomTriMeshClearTCCache(dGeomID g); + + +/* + * sets the trimesh to "force-normal" mode in which its normals are always used + * for tri-on-tri collisions with a non-force-normal geom + */ + void dGeomTriMeshSetForceNormalMode(dGeomID g,int enable); + + +/* + * returns the TriMeshDataID + */ +dTriMeshDataID dGeomTriMeshGetTriMeshDataID(dGeomID g); + +/* + * Gets a triangle. + */ +void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2); + +/* + * Gets the point on the requested triangle and the given barycentric + * coordinates. + */ +void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out); + +/* + +This is how the strided data works: + +struct StridedVertex{ + dVector3 Vertex; + // Userdata +}; +int VertexStride = sizeof(StridedVertex); + +struct StridedTri{ + int Indices[3]; + // Userdata +}; +int TriStride = sizeof(StridedTri); + +*/ + + +int dGeomTriMeshGetTriangleCount (dGeomID g); + + +#ifdef __cplusplus +} +#endif + +#endif /* _ODE_COLLISION_TRIMESH_H_ */ + diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_box.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_box.cpp new file mode 100644 index 00000000..bad0cd62 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_box.cpp @@ -0,0 +1,1349 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + + +/************************************************************************* + * * + * Triangle-box collider by Alen Ladavac and Vedran Klanac. * + * Ported to ODE by Oskari Nyman. * + * * + *************************************************************************/ + // 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 + + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxGeom* in_g1, dxGeom* in_g2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount); + + +// largest number, double or float +#if defined(dSINGLE) + #define MAXVALUE FLT_MAX +#else + #define MAXVALUE DBL_MAX +#endif + + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) do{ \ + (r)[0]=(a)[0] - (b)[0]; \ + (r)[1]=(a)[1] - (b)[1]; \ + (r)[2]=(a)[2] - (b)[2]; }while(0) + + +// dVector3 +// a=b +#define SET(a,b) do{ \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; }while(0) + + +// dMatrix3 +// a=b +#define SETM(a,b) do{ \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; \ + (a)[3]=(b)[3]; \ + (a)[4]=(b)[4]; \ + (a)[5]=(b)[5]; \ + (a)[6]=(b)[6]; \ + (a)[7]=(b)[7]; \ + (a)[8]=(b)[8]; \ + (a)[9]=(b)[9]; \ + (a)[10]=(b)[10]; \ + (a)[11]=(b)[11]; }while(0) + + +// dVector3 +// r=a+b +#define ADD(a,b,r) do{ \ + (r)[0]=(a)[0] + (b)[0]; \ + (r)[1]=(a)[1] + (b)[1]; \ + (r)[2]=(a)[2] + (b)[2]; }while(0) + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) do{ \ + (v)[0]=(m)[(a)+0]; \ + (v)[1]=(m)[(a)+4]; \ + (v)[2]=(m)[(a)+8]; }while(0) + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) \ + ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ) + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) do{ \ + plane[0]=normal[0];\ + plane[1]=normal[1];\ + plane[2]=normal[2];\ + plane[3]=d; }while(0) + + +// dVector3 +// length of vector a +#define LENGTHOF(a) \ + dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]) + + +struct _Context{ + + dMatrix3 mHullBoxRot; + dVector3 vHullBoxPos; + dVector3 vBoxHalfSize; + + // mesh data + dVector3 vHullDstPos; + + // global collider data + dVector3 vBestNormal; + dReal fBestDepth; + int iBestAxis; + int iExitAxis; + dVector3 vE0, vE1, vE2, vN; + + // global info for contact creation + int iFlags; + dContactGeom *ContactGeoms; + int iStride; + dxGeom *Geom1; + dxGeom *Geom2; + int ctContacts; + + _Context():ctContacts(0), + iBestAxis(0), + iExitAxis(0){} +}; + +// box data +//static dMatrix3 mHullBoxRot; +//static dVector3 vHullBoxPos; +//static dVector3 vBoxHalfSize; + +// mesh data +//static dVector3 vHullDstPos; + +// global collider data +//static dVector3 vBestNormal; +//static dReal fBestDepth; +//static int iBestAxis = 0; +//static int iExitAxis = 0; +//static dVector3 vE0; +//static dVector3 vE1; +//static dVector3 vE2; +//static dVector3 vN; + +// global info for contact creation +//static int iFlags; +//static dContactGeom *ContactGeoms; +//static int iStride; +//static dxGeom *Geom1; +//static dxGeom *Geom2; +//static int ctContacts = 0; + + + +// Test normal of mesh face as separating axis for intersection +static BOOL _cldTestNormal(_Context &ctx, dReal fp0, dReal fR, dVector3 vNormal, int iAxis ) +{ + // calculate overlapping interval of box and triangle + dReal fDepth = fR+fp0; + + // if we do not overlap + if ( fDepth<0 ) { + // do nothing + return FALSE; + } + + // calculate normal's length + dReal fLength = LENGTHOF(vNormal); + // if long enough + if ( fLength > 0.0f ) { + + dReal fOneOverLength = 1.0f/fLength; + // normalize depth + fDepth = fDepth*fOneOverLength; + + // get minimum depth + if (fDepth=0); + ctx.fBestDepth = fDepth; + } + + } + + return TRUE; +} + + + + +// Test box axis as separating axis +static BOOL _cldTestFace(_Context &ctx, dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD, + dVector3 vNormal, int iAxis ) +{ + dReal fMin, fMax; + + // find min of triangle interval + if ( fp0 < fp1 ) { + if ( fp0 < fp2 ) { + fMin = fp0; + } else { + fMin = fp2; + } + } else { + if( fp1 < fp2 ) { + fMin = fp1; + } else { + fMin = fp2; + } + } + + // find max of triangle interval + if ( fp0 > fp1 ) { + if ( fp0 > fp2 ) { + fMax = fp0; + } else { + fMax = fp2; + } + } else { + if( fp1 > fp2 ) { + fMax = fp1; + } else { + fMax = fp2; + } + } + + // calculate minimum and maximum depth + dReal fDepthMin = fR - fMin; + dReal fDepthMax = fMax + fR; + + // if we dont't have overlapping interval + if ( fDepthMin < 0 || fDepthMax < 0 ) { + // do nothing + return FALSE; + } + + dReal fDepth = 0; + + // if greater depth is on negative side + if ( fDepthMin > fDepthMax ) { + // use smaller depth (one from positive side) + fDepth = fDepthMax; + // flip normal direction + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + fD = -fD; + // if greater depth is on positive side + } else { + // use smaller depth (one from negative side) + fDepth = fDepthMin; + } + + + // if lower depth than best found so far + if (fDepth=0); + ctx.fBestDepth = fDepth; + } + + return TRUE; +} + + + + + +// Test cross products of box axis and triangle edges as separating axis +static BOOL _cldTestEdge(_Context &ctx, dReal fp0, dReal fp1, dReal fR, dReal fD, + dVector3 vNormal, int iAxis ) +{ + dReal fMin, fMax; + + // calculate min and max interval values + if ( fp0 < fp1 ) { + fMin = fp0; + fMax = fp1; + } else { + fMin = fp1; + fMax = fp0; + } + + // check if we overlapp + dReal fDepthMin = fR - fMin; + dReal fDepthMax = fMax + fR; + + // if we don't overlapp + if ( fDepthMin < 0 || fDepthMax < 0 ) { + // do nothing + return FALSE; + } + + dReal fDepth; + + + // if greater depth is on negative side + if ( fDepthMin > fDepthMax ) { + // use smaller depth (one from positive side) + fDepth = fDepthMax; + // flip normal direction + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + fD = -fD; + // if greater depth is on positive side + } else { + // use smaller depth (one from negative side) + fDepth = fDepthMin; + } + + // calculate normal's length + dReal fLength = LENGTHOF(vNormal); + + // if long enough + if ( fLength > 0.0f ) { + + // normalize depth + dReal fOneOverLength = 1.0f/fLength; + fDepth = fDepth*fOneOverLength; + fD*=fOneOverLength; + + + // if lower depth than best found so far (favor face over edges) + if (fDepth*1.5f=0); + ctx.fBestDepth = fDepth; + } + } + + return TRUE; +} + + + + + +// clip polygon with plane and generate new polygon points +static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1= 0 ) { + // emit point + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + +} + + + + +static BOOL _cldTestSeparatingAxes(_Context &ctx, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { + // reset best axis + ctx.iBestAxis = 0; + ctx.iExitAxis = -1; + ctx.fBestDepth = MAXVALUE; + + // calculate edges + SUBTRACT(v1,v0,ctx.vE0); + SUBTRACT(v2,v0,ctx.vE1); + SUBTRACT(ctx.vE1,ctx.vE0,ctx.vE2); + + // calculate poly normal + dCROSS(ctx.vN,=,ctx.vE0,ctx.vE1); + + // extract box axes as vectors + dVector3 vA0,vA1,vA2; + GETCOL(ctx.mHullBoxRot,0,vA0); + GETCOL(ctx.mHullBoxRot,1,vA1); + GETCOL(ctx.mHullBoxRot,2,vA2); + + // box halfsizes + dReal fa0 = ctx.vBoxHalfSize[0]; + dReal fa1 = ctx.vBoxHalfSize[1]; + dReal fa2 = ctx.vBoxHalfSize[2]; + + // calculate relative position between box and triangle + dVector3 vD; + SUBTRACT(v0,ctx.vHullBoxPos,vD); + + // calculate length of face normal + dReal fNLen = LENGTHOF( ctx.vN ); + + dVector3 vL; + dReal fp0, fp1, fp2, fR, fD; + + // Test separating axes for intersection + // ************************************************ + // Axis 1 - Triangle Normal + SET(vL,ctx.vN); + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0; + fR=fa0*dFabs( dDOT(ctx.vN,vA0) ) + fa1 * dFabs( dDOT(ctx.vN,vA1) ) + fa2 * dFabs( dDOT(ctx.vN,vA2) ); + + + if( !_cldTestNormal(ctx, fp0, fR, vL, 1) ) { + ctx.iExitAxis=1; + return FALSE; + } + + // ************************************************ + + // Test Faces + // ************************************************ + // Axis 2 - Box X-Axis + SET(vL,vA0); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 + dDOT(vA0,ctx.vE0); + fp2 = fp0 + dDOT(vA0,ctx.vE1); + fR = fa0; + + + if( !_cldTestFace(ctx, fp0, fp1, fp2, fR, fD, vL, 2) ) { + ctx.iExitAxis=2; + return FALSE; + } + // ************************************************ + + // ************************************************ + // Axis 3 - Box Y-Axis + SET(vL,vA1); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 + dDOT(vA1,ctx.vE0); + fp2 = fp0 + dDOT(vA1,ctx.vE1); + fR = fa1; + + + if( !_cldTestFace(ctx, fp0, fp1, fp2, fR, fD, vL, 3) ) { + ctx.iExitAxis=3; + return FALSE; + } + + // ************************************************ + + // ************************************************ + // Axis 4 - Box Z-Axis + SET(vL,vA2); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 + dDOT(vA2,ctx.vE0); + fp2 = fp0 + dDOT(vA2,ctx.vE1); + fR = fa2; + + + if( !_cldTestFace(ctx, fp0, fp1, fp2, fR, fD, vL, 4) ) { + ctx.iExitAxis=4; + return FALSE; + } + + // ************************************************ + + // Test Edges + // ************************************************ + // Axis 5 - Box X-Axis cross Edge0 + dCROSS(vL,=,vA0,ctx.vE0); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0 + dDOT(vA0,ctx.vN); + fR = fa1 * dFabs(dDOT(vA2,ctx.vE0)) + fa2 * dFabs(dDOT(vA1,ctx.vE0)); + + + if( !_cldTestEdge(ctx, fp1, fp2, fR, fD, vL, 5) ) { + ctx.iExitAxis=5; + return FALSE; + } + // ************************************************ + + // ************************************************ + // Axis 6 - Box X-Axis cross Edge1 + dCROSS(vL,=,vA0,ctx.vE1); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA0,ctx.vN); + fp2 = fp0; + fR = fa1 * dFabs(dDOT(vA2,ctx.vE1)) + fa2 * dFabs(dDOT(vA1,ctx.vE1)); + + + if( !_cldTestEdge(ctx, fp0, fp1, fR, fD, vL, 6) ) { + ctx.iExitAxis=6; + return FALSE; + } + // ************************************************ + + // ************************************************ + // Axis 7 - Box X-Axis cross Edge2 + dCROSS(vL,=,vA0,ctx.vE2); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA0,ctx.vN); + fp2 = fp0 - dDOT(vA0,ctx.vN); + fR = fa1 * dFabs(dDOT(vA2,ctx.vE2)) + fa2 * dFabs(dDOT(vA1,ctx.vE2)); + + + if( !_cldTestEdge(ctx, fp0, fp1, fR, fD, vL, 7) ) { + ctx.iExitAxis=7; + return FALSE; + } + + // ************************************************ + + // ************************************************ + // Axis 8 - Box Y-Axis cross Edge0 + dCROSS(vL,=,vA1,ctx.vE0); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0 + dDOT(vA1,ctx.vN); + fR = fa0 * dFabs(dDOT(vA2,ctx.vE0)) + fa2 * dFabs(dDOT(vA0,ctx.vE0)); + + + if( !_cldTestEdge(ctx, fp0, fp2, fR, fD, vL, 8) ) { + ctx.iExitAxis=8; + return FALSE; + } + + // ************************************************ + + // ************************************************ + // Axis 9 - Box Y-Axis cross Edge1 + dCROSS(vL,=,vA1,ctx.vE1); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA1,ctx.vN); + fp2 = fp0; + fR = fa0 * dFabs(dDOT(vA2,ctx.vE1)) + fa2 * dFabs(dDOT(vA0,ctx.vE1)); + + + if( !_cldTestEdge(ctx, fp0, fp1, fR, fD, vL, 9) ) { + ctx.iExitAxis=9; + return FALSE; + } + + // ************************************************ + + // ************************************************ + // Axis 10 - Box Y-Axis cross Edge2 + dCROSS(vL,=,vA1,ctx.vE2); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA1,ctx.vN); + fp2 = fp0 - dDOT(vA1,ctx.vN); + fR = fa0 * dFabs(dDOT(vA2,ctx.vE2)) + fa2 * dFabs(dDOT(vA0,ctx.vE2)); + + + if( !_cldTestEdge(ctx, fp0, fp1, fR, fD, vL, 10) ) { + ctx.iExitAxis=10; + return FALSE; + } + + // ************************************************ + + // ************************************************ + // Axis 11 - Box Z-Axis cross Edge0 + dCROSS(vL,=,vA2,ctx.vE0); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0; + fp2 = fp0 + dDOT(vA2,ctx.vN); + fR = fa0 * dFabs(dDOT(vA1,ctx.vE0)) + fa1 * dFabs(dDOT(vA0,ctx.vE0)); + + + if( !_cldTestEdge(ctx, fp0, fp2, fR, fD, vL, 11) ) { + ctx.iExitAxis=11; + return FALSE; + } + // ************************************************ + + // ************************************************ + // Axis 12 - Box Z-Axis cross Edge1 + dCROSS(vL,=,vA2,ctx.vE1); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA2,ctx.vN); + fp2 = fp0; + fR = fa0 * dFabs(dDOT(vA1,ctx.vE1)) + fa1 * dFabs(dDOT(vA0,ctx.vE1)); + + + if( !_cldTestEdge(ctx, fp0, fp1, fR, fD, vL, 12) ) { + ctx.iExitAxis=12; + return FALSE; + } + // ************************************************ + + // ************************************************ + // Axis 13 - Box Z-Axis cross Edge2 + dCROSS(vL,=,vA2,ctx.vE2); + fD = dDOT(vL,ctx.vN)/fNLen; + fp0 = dDOT(vL,vD); + fp1 = fp0 - dDOT(vA2,ctx.vN); + fp2 = fp0 - dDOT(vA2,ctx.vN); + fR = fa0 * dFabs(dDOT(vA1,ctx.vE2)) + fa1 * dFabs(dDOT(vA0,ctx.vE2)); + + + if( !_cldTestEdge(ctx, fp0, fp1, fR, fD, vL, 13) ) { + ctx.iExitAxis=13; + return FALSE; + } + + // ************************************************ + return TRUE; +} + + + + + +// find two closest points on two lines +static BOOL _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1, + dVector3 vPoint2, dVector3 vLenVec2, + dReal &fvalue1, dReal &fvalue2) +{ + // calulate denominator + dVector3 vp; + SUBTRACT(vPoint2,vPoint1,vp); + dReal fuaub = dDOT(vLenVec1,vLenVec2); + dReal fq1 = dDOT(vLenVec1,vp); + dReal fq2 = -dDOT(vLenVec2,vp); + dReal fd = 1.0f - fuaub * fuaub; + + // if denominator is positive + if (fd > 0.0f) { + // calculate points of closest approach + fd = 1.0f/fd; + fvalue1 = (fq1 + fuaub*fq2)*fd; + fvalue2 = (fuaub*fq1 + fq2)*fd; + return TRUE; + // otherwise + } else { + // lines are parallel + fvalue1 = 0.0f; + fvalue2 = 0.0f; + return FALSE; + } + +} + + + + + +// clip and generate contacts +static void _cldClipping(_Context &ctx, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { + + // if we have edge/edge intersection + if ( ctx.iBestAxis > 4 ) { + + dVector3 vub,vPb,vPa; + + SET(vPa,ctx.vHullBoxPos); + + // calculate point on box edge + for( int i=0; i<3; i++) { + dVector3 vRotCol; + GETCOL(ctx.mHullBoxRot,i,vRotCol); + dReal fSign = dDOT(ctx.vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f; + + vPa[0] += fSign * ctx.vBoxHalfSize[i] * vRotCol[0]; + vPa[1] += fSign * ctx.vBoxHalfSize[i] * vRotCol[1]; + vPa[2] += fSign * ctx.vBoxHalfSize[i] * vRotCol[2]; + } + + int iEdge = (ctx.iBestAxis-5)%3; + + // decide which edge is on triangle + if ( iEdge == 0 ) { + SET(vPb,v0); + SET(vub,ctx.vE0); + } else if ( iEdge == 1) { + SET(vPb,v2); + SET(vub,ctx.vE1); + } else { + SET(vPb,v1); + SET(vub,ctx.vE2); + } + + + // setup direction parameter for face edge + dNormalize3(vub); + + dReal fParam1, fParam2; + + // setup direction parameter for box edge + dVector3 vua; + int col=(ctx.iBestAxis-5)/3; + GETCOL(ctx.mHullBoxRot,col,vua); + + // find two closest points on both edges + _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 ); + vPa[0] += vua[0]*fParam1; + vPa[1] += vua[1]*fParam1; + vPa[2] += vua[2]*fParam1; + + vPb[0] += vub[0]*fParam2; + vPb[1] += vub[1]*fParam2; + vPb[2] += vub[2]*fParam2; + + // calculate collision point + dVector3 vPntTmp; + ADD(vPa,vPb,vPntTmp); + + vPntTmp[0]*=0.5f; + vPntTmp[1]*=0.5f; + vPntTmp[2]*=0.5f; + + // generate contact point between two closest points +#ifdef ORIG + if (ctx.ctContacts < (ctx.iFlags & 0x0ffff)) { + dContactGeom* Contact = SAFECONTACT(ctx.iFlags, ctx.ContactGeoms, ctx.ctContacts, ctx.iStride); + Contact->depth = ctx.fBestDepth; + SET(Contact->normal,ctx.vBestNormal); + SET(Contact->pos,vPntTmp); + Contact->g1 = ctx.Geom1; + Contact->g2 = ctx.Geom2; + ctx.ctContacts++; + } +#endif + GenerateContact(ctx.iFlags, ctx.ContactGeoms, ctx.iStride, ctx.Geom1, ctx.Geom2, + vPntTmp, ctx.vBestNormal, ctx.fBestDepth, ctx.ctContacts); + + + + // if triangle is the referent face then clip box to triangle face + } else if ( ctx.iBestAxis == 1 ) { + + + dVector3 vNormal2; + vNormal2[0]=-ctx.vBestNormal[0]; + vNormal2[1]=-ctx.vBestNormal[1]; + vNormal2[2]=-ctx.vBestNormal[2]; + + + // vNr is normal in box frame, pointing from triangle to box + dMatrix3 mTransposed; + mTransposed[0*4+0]=ctx.mHullBoxRot[0*4+0]; + mTransposed[0*4+1]=ctx.mHullBoxRot[1*4+0]; + mTransposed[0*4+2]=ctx.mHullBoxRot[2*4+0]; + + mTransposed[1*4+0]=ctx.mHullBoxRot[0*4+1]; + mTransposed[1*4+1]=ctx.mHullBoxRot[1*4+1]; + mTransposed[1*4+2]=ctx.mHullBoxRot[2*4+1]; + + mTransposed[2*4+0]=ctx.mHullBoxRot[0*4+2]; + mTransposed[2*4+1]=ctx.mHullBoxRot[1*4+2]; + mTransposed[2*4+2]=ctx.mHullBoxRot[2*4+2]; + + dVector3 vNr; + vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2]; + vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2]; + vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2]; + + + dVector3 vAbsNormal; + vAbsNormal[0] = dFabs( vNr[0] ); + vAbsNormal[1] = dFabs( vNr[1] ); + vAbsNormal[2] = dFabs( vNr[2] ); + + // get closest face from box + int iB0, iB1, iB2; + if (vAbsNormal[1] > vAbsNormal[0]) { + if (vAbsNormal[1] > vAbsNormal[2]) { + iB1 = 0; iB0 = 1; iB2 = 2; + } else { + iB1 = 0; iB2 = 1; iB0 = 2; + } + } else { + + if (vAbsNormal[0] > vAbsNormal[2]) { + iB0 = 0; iB1 = 1; iB2 = 2; + } else { + iB1 = 0; iB2 = 1; iB0 = 2; + } + } + + // Here find center of box face we are going to project + dVector3 vCenter; + dVector3 vRotCol; + GETCOL(ctx.mHullBoxRot,iB0,vRotCol); + + if (vNr[iB0] > 0) { + vCenter[0] = ctx.vHullBoxPos[0] - v0[0] - ctx.vBoxHalfSize[iB0] * vRotCol[0]; + vCenter[1] = ctx.vHullBoxPos[1] - v0[1] - ctx.vBoxHalfSize[iB0] * vRotCol[1]; + vCenter[2] = ctx.vHullBoxPos[2] - v0[2] - ctx.vBoxHalfSize[iB0] * vRotCol[2]; + } else { + vCenter[0] = ctx.vHullBoxPos[0] - v0[0] + ctx.vBoxHalfSize[iB0] * vRotCol[0]; + vCenter[1] = ctx.vHullBoxPos[1] - v0[1] + ctx.vBoxHalfSize[iB0] * vRotCol[1]; + vCenter[2] = ctx.vHullBoxPos[2] - v0[2] + ctx.vBoxHalfSize[iB0] * vRotCol[2]; + } + + // Here find 4 corner points of box + dVector3 avPoints[4]; + + dVector3 vRotCol2; + GETCOL(ctx.mHullBoxRot,iB1,vRotCol); + GETCOL(ctx.mHullBoxRot,iB2,vRotCol2); + + for(int x=0;x<3;x++) { + avPoints[0][x] = vCenter[x] + (ctx.vBoxHalfSize[iB1] * vRotCol[x]) - (ctx.vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[1][x] = vCenter[x] - (ctx.vBoxHalfSize[iB1] * vRotCol[x]) - (ctx.vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[2][x] = vCenter[x] - (ctx.vBoxHalfSize[iB1] * vRotCol[x]) + (ctx.vBoxHalfSize[iB2] * vRotCol2[x]); + avPoints[3][x] = vCenter[x] + (ctx.vBoxHalfSize[iB1] * vRotCol[x]) + (ctx.vBoxHalfSize[iB2] * vRotCol2[x]); + } + + + // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes) + dVector3 avTempArray1[9]; + dVector3 avTempArray2[9]; + dVector4 plPlane; + + int iTempCnt1=0; + int iTempCnt2=0; + + // zeroify vectors - necessary? + for(int i=0; i<9; i++) { + avTempArray1[i][0]=0; + avTempArray1[i][1]=0; + avTempArray1[i][2]=0; + + avTempArray2[i][0]=0; + avTempArray2[i][1]=0; + avTempArray2[i][2]=0; + } + + + // Normal plane + dVector3 vTemp; + vTemp[0]=-ctx.vN[0]; + vTemp[1]=-ctx.vN[1]; + vTemp[2]=-ctx.vN[2]; + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p0 + dVector3 vTemp2; + SUBTRACT(v1,v0,vTemp2); + dCROSS(vTemp,=,ctx.vN,vTemp2); + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p1 + SUBTRACT(v2,v1,vTemp2); + dCROSS(vTemp,=,ctx.vN,vTemp2); + dNormalize3(vTemp); + SUBTRACT(v0,v2,vTemp2); + CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp)); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p2 + SUBTRACT(v0,v2,vTemp2); + dCROSS(vTemp,=,ctx.vN,vTemp2); + dNormalize3(vTemp); + CONSTRUCTPLANE(plPlane,vTemp,0); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // END of clipping polygons + + + + // for each generated contact point + for ( int i=0; i 0) { + fTempDepth = 0; + } + + dVector3 vPntTmp; + ADD(avTempArray2[i],v0,vPntTmp); + +#ifdef ORIG + if (ctx.ctContacts < (ctx.iFlags & 0x0ffff)) { + dContactGeom* Contact = SAFECONTACT(ctx.iFlags, ctx.ContactGeoms, ctx.ctContacts, ctx.iStride); + + Contact->depth = -fTempDepth; + SET(Contact->normal,ctx.vBestNormal); + SET(Contact->pos,vPntTmp); + Contact->g1 = ctx.Geom1; + Contact->g2 = ctx.Geom2; + ctx.ctContacts++; + } +#endif + GenerateContact(ctx.iFlags, ctx.ContactGeoms, ctx.iStride, ctx.Geom1, ctx.Geom2, + vPntTmp, ctx.vBestNormal, -fTempDepth, ctx.ctContacts); + } + + //dAASSERT(ctx.ctContacts>0); + + // if box face is the referent face, then clip triangle on box face + } else { // 2 <= if ctx.iBestAxis <= 4 + + // get normal of box face + dVector3 vNormal2; + SET(vNormal2,ctx.vBestNormal); + + // get indices of box axes in correct order + int iA0,iA1,iA2; + iA0 = ctx.iBestAxis-2; + if ( iA0 == 0 ) { + iA1 = 1; iA2 = 2; + } else if ( iA0 == 1 ) { + iA1 = 0; iA2 = 2; + } else { + iA1 = 0; iA2 = 1; + } + + dVector3 avPoints[3]; + // calculate triangle vertices in box frame + SUBTRACT(v0,ctx.vHullBoxPos,avPoints[0]); + SUBTRACT(v1,ctx.vHullBoxPos,avPoints[1]); + SUBTRACT(v2,ctx.vHullBoxPos,avPoints[2]); + + // CLIP Polygons + // define temp data for clipping + dVector3 avTempArray1[9]; + dVector3 avTempArray2[9]; + + int iTempCnt1, iTempCnt2; + + // zeroify vectors - necessary? + for(int i=0; i<9; i++) { + avTempArray1[i][0]=0; + avTempArray1[i][1]=0; + avTempArray1[i][2]=0; + + avTempArray2[i][0]=0; + avTempArray2[i][1]=0; + avTempArray2[i][2]=0; + } + + // clip triangle with 5 box planes (1 face plane, 4 edge planes) + + dVector4 plPlane; + + // Normal plane + dVector3 vTemp; + vTemp[0]=-vNormal2[0]; + vTemp[1]=-vNormal2[1]; + vTemp[2]=-vNormal2[2]; + CONSTRUCTPLANE(plPlane,vTemp,ctx.vBoxHalfSize[iA0]); + + _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p0 + GETCOL(ctx.mHullBoxRot,iA1,vTemp); + CONSTRUCTPLANE(plPlane,vTemp,ctx.vBoxHalfSize[iA1]); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p1 + GETCOL(ctx.mHullBoxRot,iA1,vTemp); + vTemp[0]=-vTemp[0]; + vTemp[1]=-vTemp[1]; + vTemp[2]=-vTemp[2]; + CONSTRUCTPLANE(plPlane,vTemp,ctx.vBoxHalfSize[iA1]); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // Plane p2 + GETCOL(ctx.mHullBoxRot,iA2,vTemp); + CONSTRUCTPLANE(plPlane,vTemp,ctx.vBoxHalfSize[iA2]); + + _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); + + + // Plane p3 + GETCOL(ctx.mHullBoxRot,iA2,vTemp); + vTemp[0]=-vTemp[0]; + vTemp[1]=-vTemp[1]; + vTemp[2]=-vTemp[2]; + CONSTRUCTPLANE(plPlane,vTemp,ctx.vBoxHalfSize[iA2]); + + _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); + + + // for each generated contact point + for ( int i=0; i 0) { + fTempDepth = 0; + } + + // generate contact data + dVector3 vPntTmp; + ADD(avTempArray1[i],ctx.vHullBoxPos,vPntTmp); + +#ifdef ORIG + if (ctx.ctContacts < (ctx.iFlags & 0x0ffff)) { + dContactGeom* Contact = SAFECONTACT(ctx.iFlags, ctx.ContactGeoms, ctx.ctContacts, ctx.iStride); + + Contact->depth = -fTempDepth; + SET(Contact->normal,ctx.vBestNormal); + SET(Contact->pos,vPntTmp); + Contact->g1 = ctx.Geom1; + Contact->g2 = ctx.Geom2; + ctx.ctContacts++; + } +#endif + GenerateContact(ctx.iFlags, ctx.ContactGeoms, ctx.iStride, ctx.Geom1, ctx.Geom2, + vPntTmp, ctx.vBestNormal, -fTempDepth, ctx.ctContacts); + } + + //dAASSERT(ctx.ctContacts>0); + } + +} + + + + + +// test one mesh triangle on intersection with given box +static void _cldTestOneTriangle(_Context &ctx, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser) +{ + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(ctx, v0, v1, v2) ) { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( ctx.iBestAxis == 0 ) { + // this should not happen (we should already exit in that case) + //dMessage (0, "best separation axis not found"); + // do nothing + return; + } + + _cldClipping(ctx, v0, v1, v2); +} + + + + + +// box to mesh collider +int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){ + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + + // get source hull position, orientation and half size + const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom); + const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom); + + // ericf fix.. slot this in in place of ugly global vars so we dont die multithreaded + _Context ctx; + + // to global + //SETM(mHullBoxRot,mRotBox); + SETM(ctx.mHullBoxRot,mRotBox); + SET(ctx.vHullBoxPos,vPosBox); + + dGeomBoxGetLengths(BoxGeom, ctx.vBoxHalfSize); + ctx.vBoxHalfSize[0] *= 0.5f; + ctx.vBoxHalfSize[1] *= 0.5f; + ctx.vBoxHalfSize[2] *= 0.5f; + + + + // get destination hull position and orientation + const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh); + const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh); + + // to global + SET(ctx.vHullDstPos,vPosMesh); + + + + // global info for contact creation + ctx.ctContacts = 0; + ctx.iStride=Stride; + ctx.iFlags=Flags; + ctx.ContactGeoms=Contacts; + ctx.Geom1=TriMesh; + ctx.Geom2=BoxGeom; + + + + // reset stuff + ctx.fBestDepth = MAXVALUE; + ctx.vBestNormal[0]=0; + ctx.vBestNormal[1]=0; + ctx.vBestNormal[2]=0; + + OBBCollider& Collider = TriMesh->_OBBCollider; + + + + + // Make OBB + OBB Box; + Box.mCenter.x = vPosBox[0]; + Box.mCenter.y = vPosBox[1]; + Box.mCenter.z = vPosBox[2]; + + + Box.mExtents.x = ctx.vBoxHalfSize[0]; + Box.mExtents.y = ctx.vBoxHalfSize[1]; + Box.mExtents.z = ctx.vBoxHalfSize[2]; + + Box.mRot.m[0][0] = mRotBox[0]; + Box.mRot.m[1][0] = mRotBox[1]; + Box.mRot.m[2][0] = mRotBox[2]; + + Box.mRot.m[0][1] = mRotBox[4]; + Box.mRot.m[1][1] = mRotBox[5]; + Box.mRot.m[2][1] = mRotBox[6]; + + Box.mRot.m[0][2] = mRotBox[8]; + Box.mRot.m[1][2] = mRotBox[9]; + Box.mRot.m[2][2] = mRotBox[10]; + + Matrix4x4 amatrix; + Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix); + + Matrix4x4 InvBoxMatrix; + InvertPRMatrix(InvBoxMatrix, BoxMatrix); + + // TC results + if (TriMesh->doBoxTC) { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){ + if (TriMesh->BoxTCCache[i].Geom == BoxGeom){ + BoxTC = &TriMesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1]; + BoxTC->Geom = BoxGeom; + BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0 + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix)); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(TriMesh->boxCache, Box, TriMesh->Data->BVTree, null, + &MakeMatrix(vPosMesh, mRotMesh, amatrix)); + // Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null, + // &MakeMatrix(vPosMesh, mRotMesh, amatrix)); + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0){ + if (TriMesh->ArrayCallback != null){ + TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount); + } + + //int OutTriCount = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++){ + + + const int& Triint = Triangles[i]; + if (!Callback(TriMesh, BoxGeom, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv); + + + // test this triangle + _cldTestOneTriangle(ctx,dv[0],dv[1],dv[2]); + } + } + + + return ctx.ctContacts; +} + + + + +// GenerateContact - Written by Jeff Smith (jeff@burri.to) +// Generate a "unique" contact. A unique contact has a unique +// position or normal. If the potential contact has the same +// position and normal as an existing contact, but a larger +// penetration depth, this new depth is used instead +// +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxGeom* in_g1, dxGeom* in_g2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount) +{ + //if (in_Depth < 0.0) + //return; + + if (OutTriCount == (in_Flags & 0x0ffff)) + return; // contacts are full! + + dContactGeom* Contact; + dVector3 diff; + bool duplicate = false; + for (int i=0; ipos[j]; + if (dDOT(diff, diff) < ODE_EPSILON) + { + // same normal? + if (fabs(dDOT(in_Normal, Contact->normal)) > ((dReal)1)-ODE_EPSILON) + { + if (in_Depth > Contact->depth) + Contact->depth = in_Depth; + duplicate = true; + } + } + } + + if (!duplicate) + { + // Add a new contact + Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); + + Contact->pos[0] = in_ContactPos[0]; + Contact->pos[1] = in_ContactPos[1]; + Contact->pos[2] = in_ContactPos[2]; + Contact->pos[3] = 0.0; + + Contact->normal[0] = in_Normal[0]; + Contact->normal[1] = in_Normal[1]; + Contact->normal[2] = in_Normal[2]; + Contact->normal[3] = 0.0; + + Contact->depth = in_Depth; + + Contact->g1 = in_g1; + Contact->g2 = in_g2; + + OutTriCount++; + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_ccylinder.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_ccylinder.cpp new file mode 100644 index 00000000..8d99bff9 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_ccylinder.cpp @@ -0,0 +1,1005 @@ +/************************************************************************* +* * +* Open Dynamics Engine, Copyright (C) 2001-2003 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. * +* * +*************************************************************************/ + +/* + * Triangle-CCylinder(Capsule) collider by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +// NOTES from Nguyen Binh +// 14 Apr : Seem to be robust +// There is a problem when you use original Step and set contact friction +// surface.mu = dInfinity; +// More description : +// When I dropped CCylinder over the bunny ears, it seems to stuck +// there for a while. I think the cause is when you set surface.mu = dInfinity; +// the friction force is too high so it just hang the capsule there. +// So the good cure for this is to set mu = around 1.5 (in my case) +// For StepFast1, this become as solid as rock : StepFast1 just approximate +// friction force. + +// NOTES from Croteam's Alen +//As a side note... there are some extra contacts that can be generated +//on the edge between two triangles, and if the capsule penetrates deeply into +//the triangle (usually happens with large mass or low FPS), some such +//contacts can in some cases push the capsule away from the edge instead of +//away from the two triangles. This shows up as capsule slowing down a bit +//when hitting an edge while sliding along a flat tesselated grid of +//triangles. This is only if capsule is standing upwards. + +//Same thing can appear whenever a smooth object (e.g sphere) hits such an +//edge, and it needs to be solved as a special case probably. This is a +//problem we are looking forward to address soon. + +// 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 + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +// largest number, double or float +#if defined(dSINGLE) +#define MAX_REAL FLT_MAX +#define MIN_REAL (-FLT_MAX) +#else +#define MAX_REAL DBL_MAX +#define MIN_REAL (-DBL_MAX) +#endif + +// To optimize before send contacts to dynamic part +#define OPTIMIZE_CONTACTS + +// dVector3 +// r=a-b +#define SUBTRACT(a,b,r) \ + (r)[0]=(a)[0] - (b)[0]; \ + (r)[1]=(a)[1] - (b)[1]; \ + (r)[2]=(a)[2] - (b)[2]; + + +// dVector3 +// a=b +#define SET(a,b) \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; + + +// dMatrix3 +// a=b +#define SETM(a,b) \ + (a)[0]=(b)[0]; \ + (a)[1]=(b)[1]; \ + (a)[2]=(b)[2]; \ + (a)[3]=(b)[3]; \ + (a)[4]=(b)[4]; \ + (a)[5]=(b)[5]; \ + (a)[6]=(b)[6]; \ + (a)[7]=(b)[7]; \ + (a)[8]=(b)[8]; \ + (a)[9]=(b)[9]; \ + (a)[10]=(b)[10]; \ + (a)[11]=(b)[11]; + + +// dVector3 +// r=a+b +#define ADD(a,b,r) \ + (r)[0]=(a)[0] + (b)[0]; \ + (r)[1]=(a)[1] + (b)[1]; \ + (r)[2]=(a)[2] + (b)[2]; + + +// dMatrix3, int, dVector3 +// v=column a from m +#define GETCOL(m,a,v) \ + (v)[0]=(m)[(a)+0]; \ + (v)[1]=(m)[(a)+4]; \ + (v)[2]=(m)[(a)+8]; + + +// dVector4, dVector3 +// distance between plane p and point v +#define POINTDISTANCE(p,v) \ + ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ); \ + + +// dVector4, dVector3, dReal +// construct plane from normal and d +#define CONSTRUCTPLANE(plane,normal,d) \ + plane[0]=normal[0];\ + plane[1]=normal[1];\ + plane[2]=normal[2];\ + plane[3]=d; + + +// dVector3 +// length of vector a +#define LENGTHOF(a) \ + dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);\ + +inline dReal _length2OfVector3(dVector3 v) +{ + return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] ); +} + + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +static sLocalContactData *gLocalContacts; +static unsigned int ctContacts = 0; + +// capsule data +// real time data +static dMatrix3 mCapsuleRotation; +static dVector3 vCapsulePosition; +static dVector3 vCapsuleAxis; +// static data +static dReal vCapsuleRadius; +static dReal fCapsuleSize; + +// mesh data +//static dMatrix4 mHullDstPl; +static dMatrix3 mTriMeshRot; +static dVector3 mTriMeshPos; +static dVector3 vE0, vE1, vE2; + +// Two geom +dxGeom* gCylinder; +dxGeom* gTriMesh; + +// global collider data +static dVector3 vNormal; +static dReal fBestDepth; +static dReal fBestCenter; +static dReal fBestrt; +static int iBestAxis; +static dVector3 vN = {0,0,0,0}; + +static dVector3 vV0; +static dVector3 vV1; +static dVector3 vV2; + +// ODE contact's specific +static int iFlags; +static dContactGeom *ContactGeoms; +static int iStride; + +// Capsule lie on axis number 3 = (Z axis) +static const int nCAPSULE_AXIS = 2; + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + SUBTRACT(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + SUBTRACT(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +inline void _OptimizeLocalContacts() +{ + int nContacts = ctContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j])) + { + // If they are seem to be the samed then filtered + // out the least penetrate one + if (_IsBetter(gLocalContacts[j],gLocalContacts[i])) + { + gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} + +inline int _ProcessLocalContacts() +{ + if (ctContacts == 0) + { + delete[] gLocalContacts; + return 0; + } + +#ifdef OPTIMIZE_CONTACTS + if (ctContacts > 1) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + unsigned int iContact = 0; + dContactGeom* Contact = 0; + + int nFinalContact = 0; + + for (iContact = 0; iContact < ctContacts; iContact ++) + { + // Ensure that we haven't created too many contacts + if( nFinalContact >= (iFlags & NUMC_MASK)) + { + break; + } + + if (1 == gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride); + Contact->depth = gLocalContacts[iContact].fDepth; + SET(Contact->normal,gLocalContacts[iContact].vNormal); + SET(Contact->pos,gLocalContacts[iContact].vPos); + Contact->g1 = gCylinder; + Contact->g2 = gTriMesh; + + nFinalContact++; + } + } + // debug + //if (nFinalContact != ctContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",ctContacts,ctContacts-nFinalContact); + //} + + delete[] gLocalContacts; + return nFinalContact; +} + +BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 ); + dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return FALSE; + // if both points in front of the plane + } else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return TRUE; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + SET(vEpnt0,vIntersectionPoint); + } else + { + SET(vEpnt1,vIntersectionPoint); + } + return TRUE; + } + return TRUE; +} + +static BOOL _cldTestAxis(const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3 vAxis, + int iAxis, + BOOL bNoFlip = FALSE) +{ + + // calculate length of separating axis vector + dReal fL = LENGTHOF(vAxis); + // if not long enough + // TODO : dReal epsilon please + if ( fL < 1e-5f ) + { + // do nothing + //iLastOutAxis = 0; + return TRUE; + } + + // otherwise normalize it + dNormalize3(vAxis); + + // project capsule on vAxis + dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))*(fCapsuleSize*REAL(0.5)-vCapsuleRadius) + vCapsuleRadius; + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dDOT( vV0 , vAxis ); + afv[1] = dDOT( vV1 , vAxis ); + afv[2] = dDOT( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = MIN_REAL; + + // for each vertex + for(int i=0; i<3; i++) + { + // find minimum + if (afv[i]fMax) + { + fMax = afv[i]; + } + } + + // find triangle's center of interval on axis + dReal fCenter = (fMin+fMax)*REAL(0.5); + // calculate triangles half interval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > ( frc + fTriangleRadius ) ) + { + // exit, we have no intersection + return FALSE; + } + + // calculate depth + dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius); + + // if greater then best found so far + if ( fDepth > fBestDepth ) + { + // remember depth + fBestDepth = fDepth; + fBestCenter = fCenter; + fBestrt = fTriangleRadius; + + vNormal[0] = vAxis[0]; + vNormal[1] = vAxis[1]; + vNormal[2] = vAxis[2]; + + iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if (fCenter<0 && !bNoFlip) + { + vNormal[0] = -vNormal[0]; + vNormal[1] = -vNormal[1]; + vNormal[2] = -vNormal[2]; + + fBestCenter = -fCenter; + } + } + + return TRUE; +} + +// helper for less key strokes +inline void _CalculateAxis(const dVector3& v1, + const dVector3& v2, + const dVector3& v3, + const dVector3& v4, + dVector3& r) +{ + dVector3 t1; + dVector3 t2; + + SUBTRACT(v1,v2,t1); + dCROSS(t2,=,t1,v3); + dCROSS(r,=,t2,v4); +} + +static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2) +{ + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + dVector3 vCp1; + vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + // reset best axis + iBestAxis = 0; + // reset best depth + fBestDepth = -MAX_REAL; + // reset separating axis vector + dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; + + // Epsilon value for checking axis vector length + const dReal fEpsilon = 1e-6f; + + // Translate triangle to Cc cord. + SUBTRACT(v0 , vCapsulePosition, vV0); + SUBTRACT(v1 , vCapsulePosition, vV1); + SUBTRACT(v2 , vCapsulePosition, vV2); + + // We begin to test for 19 separating axis now + // I wonder does it help if we employ the method like ISA-GJK??? + // Or at least we should do experiment and find what axis will + // be most likely to be separating axis to check it first. + + // Original + // axis vN + //vAxis = -vN; + vAxis[0] = - vN[0]; + vAxis[1] = - vN[1]; + vAxis[2] = - vN[2]; + if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE)) + { + return FALSE; + } + + // axis CxE0 - Edge 0 + dCROSS(vAxis,=,vCapsuleAxis,vE0); + //vAxis = dCROSS( vCapsuleAxis cross vE0 ); + if( _length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) { + return FALSE; + } + } + + // axis CxE1 - Edge 1 + dCROSS(vAxis,=,vCapsuleAxis,vE1); + //vAxis = ( vCapsuleAxis cross vE1 ); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) { + return FALSE; + } + } + + // axis CxE2 - Edge 2 + //vAxis = ( vCapsuleAxis cross vE2 ); + dCROSS(vAxis,=,vCapsuleAxis,vE2); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) { + return FALSE; + } + } + + // first capsule point + // axis ((Cp0-V0) x E0) x E0 + _CalculateAxis(vCp0,v0,vE0,vE0,vAxis); +// vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) { + return FALSE; + } + } + + // axis ((Cp0-V1) x E1) x E1 + _CalculateAxis(vCp0,v1,vE1,vE1,vAxis); + //vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) { + return FALSE; + } + } + + // axis ((Cp0-V2) x E2) x E2 + _CalculateAxis(vCp0,v2,vE2,vE2,vAxis); + //vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) { + return FALSE; + } + } + + // second capsule point + // axis ((Cp1-V0) x E0) x E0 + _CalculateAxis(vCp1,v0,vE0,vE0,vAxis); + //vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) { + return FALSE; + } + } + + // axis ((Cp1-V1) x E1) x E1 + _CalculateAxis(vCp1,v1,vE1,vE1,vAxis); + //vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) { + return FALSE; + } + } + + // axis ((Cp1-V2) x E2) x E2 + _CalculateAxis(vCp1,v2,vE2,vE2,vAxis); + //vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) { + return FALSE; + } + } + + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + _CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) { + return FALSE; + } + } + + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + _CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) { + return FALSE; + } + } + + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + _CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis); + //vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis; + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) { + return FALSE; + } + } + + // Test as separating axes direction vectors between each triangle + // edge and each capsule's cap center + + // first triangle vertex and first capsule point + //vAxis = v0 - vCp0; + SUBTRACT(v0,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) { + return FALSE; + } + } + + // second triangle vertex and first capsule point + //vAxis = v1 - vCp0; + SUBTRACT(v1,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) { + return FALSE; + } + } + + // third triangle vertex and first capsule point + //vAxis = v2 - vCp0; + SUBTRACT(v2,vCp0,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) { + return FALSE; + } + } + + // first triangle vertex and second capsule point + //vAxis = v0 - vCp1; + SUBTRACT(v0,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) { + return FALSE; + } + } + + // second triangle vertex and second capsule point + //vAxis = v1 - vCp1; + SUBTRACT(v1,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) { + return FALSE; + } + } + + // third triangle vertex and second capsule point + //vAxis = v2 - vCp1; + SUBTRACT(v2,vCp1,vAxis); + if(_length2OfVector3( vAxis ) > fEpsilon ) { + if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) { + return FALSE; + } + } + + return TRUE; +} + +// test one mesh triangle on intersection with capsule +static void _cldTestOneTriangleVSCCylinder( const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2 ) +{ + + // calculate edges + SUBTRACT(v1,v0,vE0); + SUBTRACT(v2,v1,vE1); + SUBTRACT(v0,v2,vE2); + + dVector3 _minus_vE0; + SUBTRACT(v0,v1,_minus_vE0); + + // calculate poly normal + dCROSS(vN,=,vE1,_minus_vE0); + dNormalize3(vN); + + // create plane from triangle + dReal plDistance = -dDOT(v0,vN); + dVector4 plTrianglePlane; + CONSTRUCTPLANE(plTrianglePlane,vN,plDistance); + + // calculate capsule distance to plane + dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition); + + // Capsule must be over positive side of triangle + if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + SET (vPnt0,v0); + dVector3 vPnt1; + SET (vPnt1,v1); + dVector3 vPnt2; + SET (vPnt2,v2); + + if (fDistanceCapsuleCenterToPlane < 0 ) + { + SET (vPnt0,v0); + SET (vPnt1,v2); + SET (vPnt2,v1); + } + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( iBestAxis == 0 ) + { + // this should not happen (we should already exit in that case) + ASSERT(FALSE); + // do nothing + return; + } + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius; + vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius; + vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint0[1] = vCposTrans[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint0[2] = vCposTrans[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius); + + // transform capsule edge points into triangle space + vCEdgePoint0[0] -= vPnt0[0]; + vCEdgePoint0[1] -= vPnt0[1]; + vCEdgePoint0[2] -= vPnt0[2]; + + vCEdgePoint1[0] -= vPnt0[0]; + vCEdgePoint1[1] -= vPnt0[1]; + vCEdgePoint1[2] -= vPnt0[2]; + + dVector4 plPlane; + dVector3 _minus_vN; + _minus_vN[0] = -vN[0]; + _minus_vN[1] = -vN[1]; + _minus_vN[2] = -vN[2]; + // triangle plane + CONSTRUCTPLANE(plPlane,_minus_vN,0); + //plPlane = Plane4f( -vN, 0); + + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + // plane with edge 0 + dVector3 vTemp; + dCROSS(vTemp,=,vN,vE0); + CONSTRUCTPLANE(plPlane, vTemp, 1e-5f); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCROSS(vTemp,=,vN,vE1); + CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-1e-5f)); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return; + } + + dCROSS(vTemp,=,vN,vE2); + CONSTRUCTPLANE(plPlane, vTemp, 1e-5f); + if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { + return; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += vPnt0[0]; + vCEdgePoint0[1] += vPnt0[1]; + vCEdgePoint0[2] += vPnt0[2]; + + vCEdgePoint1[0] += vPnt0[0]; + vCEdgePoint1[1] += vPnt0[1]; + vCEdgePoint1[2] += vPnt0[2]; + + // calculate depths for both contact points + SUBTRACT(vCEdgePoint0,vCapsulePosition,vTemp); + dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt); + SUBTRACT(vCEdgePoint1,vCapsulePosition,vTemp); + dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt); + + // clamp depths to zero + if(fDepth0 < 0) + { + fDepth0 = 0.0f; + } + + if(fDepth1 < 0 ) + { + fDepth1 = 0.0f; + } + + // Cached contacts's data + // contact 0 + if ((int)ctContacts < (iFlags & NUMC_MASK)) { + gLocalContacts[ctContacts].fDepth = fDepth0; + SET(gLocalContacts[ctContacts].vNormal,vNormal); + SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0); + gLocalContacts[ctContacts].nFlags = 1; + ctContacts++; + + if ((int)ctContacts < (iFlags & NUMC_MASK)) { + // contact 1 + gLocalContacts[ctContacts].fDepth = fDepth1; + SET(gLocalContacts[ctContacts].vNormal,vNormal); + SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1); + gLocalContacts[ctContacts].nFlags = 1; + ctContacts++; + } + } + +} + +// capsule - trimesh by CroTeam +// Ported by Nguyem Binh +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dxTriMesh* TriMesh = (dxTriMesh*)o1; + gCylinder = o2; + gTriMesh = o1; + + const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder); + memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3)); + + const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder); + memcpy(vCapsulePosition,pDst,sizeof(dVector3)); + + vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; + vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS]; + vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS]; + + // Get size of CCylinder + dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize); + fCapsuleSize += 2*vCapsuleRadius; + + const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh); + memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3)); + + const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh); + memcpy(mTriMeshPos,pTriPos,sizeof(dVector3)); + + // global info for contact creation + iStride =skip; + iFlags =flags; + ContactGeoms =contact; + + // reset contact counter + ctContacts = 0; + // allocat local contact workspace + gLocalContacts = new sLocalContactData[(iFlags & NUMC_MASK)]; + + // reset best depth + fBestDepth = - MAX_REAL; + fBestCenter = 0; + fBestrt = 0; + + // reset collision normal + vNormal[0] = REAL(0.0); + vNormal[1] = REAL(0.0); + vNormal[2] = REAL(0.0); + + // Will it better to use LSS here? -> confirm Pierre. + OBBCollider& Collider = TriMesh->_OBBCollider; + + Point cCenter((float) vCapsulePosition[0],(float) vCapsulePosition[1],(float) vCapsulePosition[2]); + Point cExtents((float) vCapsuleRadius,(float) vCapsuleRadius,(float) fCapsuleSize/2); + + Matrix3x3 obbRot; + + obbRot[0][0] = (float) mCapsuleRotation[0]; + obbRot[1][0] = (float) mCapsuleRotation[1]; + obbRot[2][0] = (float) mCapsuleRotation[2]; + + obbRot[0][1] = (float) mCapsuleRotation[4]; + obbRot[1][1] = (float) mCapsuleRotation[5]; + obbRot[2][1] = (float) mCapsuleRotation[6]; + + obbRot[0][2] = (float) mCapsuleRotation[8]; + obbRot[1][2] = (float) mCapsuleRotation[9]; + obbRot[2][2] = (float) mCapsuleRotation[10]; + + OBB obbCCylinder(cCenter,cExtents,obbRot); + + Matrix4x4 CCylinderMatrix; + MakeMatrix(vCapsulePosition, mCapsuleRotation, CCylinderMatrix); + + Matrix4x4 MeshMatrix; + MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix); + + // TC results + if (TriMesh->doBoxTC) { + dxTriMesh::BoxTC* BoxTC = 0; + for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){ + if (TriMesh->BoxTCCache[i].Geom == gCylinder){ + BoxTC = &TriMesh->BoxTCCache[i]; + break; + } + } + if (!BoxTC){ + TriMesh->BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1]; + BoxTC->Geom = gCylinder; + BoxTC->FatCoeff = 1.0f; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCCylinder, TriMesh->Data->BVTree, null, &MeshMatrix); + } + else { + Collider.SetTemporalCoherence(false); + //Collider.Collide(dxTriMesh::defaultBoxCache, obbCCylinder, TriMesh->Data->BVTree, null,&MeshMatrix); + Collider.Collide(TriMesh->boxCache, obbCCylinder, TriMesh->Data->BVTree, null,&MeshMatrix); + } + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0) + { + if (TriMesh->ArrayCallback != null) + { + TriMesh->ArrayCallback(TriMesh, gCylinder, Triangles, TriCount); + } + + //int OutTriCount = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + if((int)ctContacts>=(iFlags & NUMC_MASK)) + { + break; + } + + const int& Triint = Triangles[i]; + if (!Callback(TriMesh, gCylinder, Triint)) continue; + + + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv); + + // test this triangle + _cldTestOneTriangleVSCCylinder(dv[0],dv[1],dv[2]); + + } + } + + return _ProcessLocalContacts(); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_distance.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_distance.cpp new file mode 100644 index 00000000..8246775c --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_distance.cpp @@ -0,0 +1,1264 @@ +// This file contains some code based on the code from Magic Software. +// That code is available under a Free Source License Agreement +// that can be found at http://www.magic-software.com/License/free.pdf + +// 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 + +#include "ode/ode_common.h" +#include "ode/ode_math.h" +#include "ode/ode_collision.h" +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between a point and a triangle. + + @param pfSParam Barycentric coordinate of triangle at point closest to p (u) + @param pfTParam Barycentric coordinate of triangle at point closest to p (v) + @return Shortest distance squared. + + The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* pfSParam, dReal* pfTParam ) +{ + dVector3 kDiff; + Vector3Subtract( triOrigin, p, kDiff ); + dReal fA00 = dDOT( triEdge0, triEdge0 ); + dReal fA01 = dDOT( triEdge0, triEdge1 ); + dReal fA11 = dDOT( triEdge1, triEdge1 ); + dReal fB0 = dDOT( kDiff, triEdge0 ); + dReal fB1 = dDOT( kDiff, triEdge1 ); + dReal fC = dDOT( kDiff, kDiff ); + dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); + dReal fS = fA01*fB1-fA11*fB0; + dReal fT = fA01*fB0-fA00*fB1; + dReal fSqrDist; + + if ( fS + fT <= fDet ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4 + { + if ( fB0 < REAL(0.0) ) + { + fT = REAL(0.0); + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else // region 3 + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else if ( fT < REAL(0.0) ) // region 5 + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else // region 0 + { + // minimum at interior point + if ( fDet == REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(0.0); + fSqrDist = dInfinity; + } + else + { + float fInvDet = REAL(1.0)/fDet; + fS *= fInvDet; + fT *= fInvDet; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + } + else + { + float fTmp0, fTmp1, fNumer, fDenom; + + if ( fS < REAL(0.0) ) // region 2 + { + fTmp0 = fA01 + fB0; + fTmp1 = fA11 + fB1; + if ( fTmp1 > fTmp0 ) + { + fNumer = fTmp1 - fTmp0; + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = fNumer/fDenom; + fT = REAL(1.0) - fS; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + else + { + fS = REAL(0.0); + if ( fTmp1 <= REAL(0.0) ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + else if ( fT < REAL(0.0) ) // region 6 + { + fTmp0 = fA01 + fB1; + fTmp1 = fA00 + fB0; + if ( fTmp1 > fTmp0 ) + { + fNumer = fTmp1 - fTmp0; + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fT = REAL(1.0); + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = fNumer/fDenom; + fS = REAL(1.0) - fT; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + else + { + fT = REAL(0.0); + if ( fTmp1 <= REAL(0.0) ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + } + else // region 1 + { + fNumer = fA11 + fB1 - fA01 - fB0; + if ( fNumer <= REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fDenom = fA00-REAL(2.0)*fA01+fA11; + if ( fNumer >= fDenom ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = fNumer/fDenom; + fT = REAL(1.0) - fS; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + } + } + } + + if ( pfSParam ) + *pfSParam = (float)fS; + + if ( pfTParam ) + *pfTParam = (float)fT; + + return dReal(fabs(fSqrDist)); +} + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between two line segments. + @param pfSegP0 t value for seg1 where the shortest distance between + the segments exists. + param pfSegP0 t value for seg2 where the shortest distance between + the segments exists. + @return Shortest distance squared. + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, + const dVector3 seg2Origin, const dVector3 seg2Direction, + dReal* pfSegP0, dReal* pfSegP1 ) +{ + const dReal gs_fTolerance = 1e-05f; + dVector3 kDiff, kNegDiff, seg1NegDirection; + Vector3Subtract( seg1Origin, seg2Origin, kDiff ); + Vector3Negate( kDiff, kNegDiff ); + dReal fA00 = dDOT( seg1Direction, seg1Direction ); + Vector3Negate( seg1Direction, seg1NegDirection ); + dReal fA01 = dDOT( seg1NegDirection, seg2Direction ); + dReal fA11 = dDOT( seg2Direction, seg2Direction ); + dReal fB0 = dDOT( kDiff, seg1Direction ); + dReal fC = dDOT( kDiff, kDiff ); + dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); + dReal fB1, fS, fT, fSqrDist, fTmp; + + if ( fDet >= gs_fTolerance ) + { + // line segments are not parallel + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = fA01*fB1-fA11*fB0; + fT = fA01*fB0-fA00*fB1; + + if ( fS >= REAL(0.0) ) + { + if ( fS <= fDet ) + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 0 (interior) + { + // minimum at two interior points of 3D lines + dReal fInvDet = REAL(1.0)/fDet; + fS *= fInvDet; + fT *= fInvDet; + fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + + fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; + } + else // region 3 (side) + { + fT = REAL(1.0); + fTmp = fA01+fB0; + if ( fTmp >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else if ( -fTmp >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + } + else // region 7 (side) + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + } + else + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 1 (side) + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + else // region 2 (corner) + { + fTmp = fA01+fB0; + if ( -fTmp <= fA00 ) + { + fT = REAL(1.0); + if ( fTmp >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + else + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + } + } + else // region 8 (corner) + { + if ( -fB0 < fA00 ) + { + fT = REAL(0.0); + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fSqrDist = fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(1.0); + fTmp = fA01+fB1; + if ( fTmp >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( -fTmp >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); + } + else + { + fT = -fTmp/fA11; + fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; + } + } + } + } + } + else + { + if ( fT >= REAL(0.0) ) + { + if ( fT <= fDet ) // region 5 (side) + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + else // region 4 (corner) + { + fTmp = fA01+fB0; + if ( fTmp < REAL(0.0) ) + { + fT = REAL(1.0); + if ( -fTmp >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); + } + else + { + fS = -fTmp/fA00; + fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + } + else // region 6 (corner) + { + if ( fB0 < REAL(0.0) ) + { + fT = REAL(0.0); + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else + { + fS = -fB0/fA00; + fSqrDist = fB0*fS+fC; + } + } + else + { + fS = REAL(0.0); + if ( fB1 >= REAL(0.0) ) + { + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB1 >= fA11 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB1/fA11; + fSqrDist = fB1*fT+fC; + } + } + } + } + } + else + { + // line segments are parallel + if ( fA01 > REAL(0.0) ) + { + // direction vectors form an obtuse angle + if ( fB0 >= REAL(0.0) ) + { + fS = REAL(0.0); + fT = REAL(0.0); + fSqrDist = fC; + } + else if ( -fB0 <= fA00 ) + { + fS = -fB0/fA00; + fT = REAL(0.0); + fSqrDist = fB0*fS+fC; + } + else + { + //fB1 = -kDiff % seg2.m; + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = REAL(1.0); + fTmp = fA00+fB0; + if ( -fTmp >= fA01 ) + { + fT = REAL(1.0); + fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1); + } + else + { + fT = -fTmp/fA01; + fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1)); + } + } + } + else + { + // direction vectors form an acute angle + if ( -fB0 >= fA00 ) + { + fS = REAL(1.0); + fT = REAL(0.0); + fSqrDist = fA00+REAL(2.0)*fB0+fC; + } + else if ( fB0 <= REAL(0.0) ) + { + fS = -fB0/fA00; + fT = REAL(0.0); + fSqrDist = fB0*fS+fC; + } + else + { + fB1 = dDOT( kNegDiff, seg2Direction ); + fS = REAL(0.0); + if ( fB0 >= -fA01 ) + { + fT = REAL(1.0); + fSqrDist = fA11+REAL(2.0)*fB1+fC; + } + else + { + fT = -fB0/fA01; + fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT); + } + } + } + } + + if ( pfSegP0 ) + *pfSegP0 = fS; + + if ( pfSegP1 ) + *pfSegP1 = fT; + + return dReal(fabs(fSqrDist)); +} + +//------------------------------------------------------------------------------ +/** + @brief Finds the shortest distance squared between a line segment and + a triangle. + + @param pfSegP t value for the line segment where the shortest distance between + the segment and the triangle occurs. + So the point along the segment that is the shortest distance + away from the triangle can be obtained by (seg.end - seg.start) * t. + @param pfTriP0 Barycentric coordinate of triangle at point closest to seg (u) + @param pfTriP1 Barycentric coordinate of triangle at point closest to seg (v) + @return Shortest distance squared. + + The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v + + Taken from: + Magic Software, Inc. + http://www.magic-software.com +*/ +dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, + const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 ) +{ + const dReal gs_fTolerance = 1e-06f; + dVector3 segDirection, segNegDirection, kDiff, kNegDiff; + Vector3Subtract( segEnd, segOrigin, segDirection ); + Vector3Negate( segDirection, segNegDirection ); + Vector3Subtract( triOrigin, segOrigin, kDiff ); + Vector3Negate( kDiff, kNegDiff ); + dReal fA00 = dDOT( segDirection, segDirection ); + dReal fA01 = dDOT( segNegDirection, triEdge0 ); + dReal fA02 = dDOT( segNegDirection, triEdge1 ); + dReal fA11 = dDOT( triEdge0, triEdge0 ); + dReal fA12 = dDOT( triEdge0, triEdge1 ); + dReal fA22 = dDOT( triEdge1, triEdge1 ); + dReal fB0 = dDOT( kNegDiff, segDirection ); + dReal fB1 = dDOT( kDiff, triEdge0 ); + dReal fB2 = dDOT( kDiff, triEdge1 ); + + dVector3 kTriSegOrigin, kTriSegDirection, kPt; + dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0; + + // Set up for a relative error test on the angle between ray direction + // and triangle normal to determine parallel/nonparallel status. + dVector3 kN; + dCROSS( kN, =, triEdge0, triEdge1 ); + dReal fNSqrLen = dDOT( kN, kN ); + dReal fDot = dDOT( segDirection, kN ); + bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen); + + if ( bNotParallel ) + { + dReal fCof00 = fA11*fA22-fA12*fA12; + dReal fCof01 = fA02*fA12-fA01*fA22; + dReal fCof02 = fA01*fA12-fA02*fA11; + dReal fCof11 = fA00*fA22-fA02*fA02; + dReal fCof12 = fA02*fA01-fA00*fA12; + dReal fCof22 = fA00*fA11-fA01*fA01; + dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02); + dReal fRhs0 = -fB0*fInvDet; + dReal fRhs1 = -fB1*fInvDet; + dReal fRhs2 = -fB2*fInvDet; + + fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2; + fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2; + fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2; + + if ( fR < REAL(0.0) ) + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4m + { + // min on face s=0 or t=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3m + { + // min on face s=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR,&fT ); + fS = REAL(0.0); + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + else if ( fT < REAL(0.0) ) // region 5m + { + // min on face t=0 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 0m + { + // min on face r=0 + fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS, &fT ); + fR = REAL(0.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2m + { + // min on face s=0 or s+t=1 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6m + { + // min on face t=0 or s+t=1 or r=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1m + { + // min on face s+t=1 or r=0 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + } + else if ( fR <= REAL(1.0) ) + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4 + { + // min on face s=0 or t=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3 + { + // min on face s=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + } + } + else if ( fT < REAL(0.0) ) // region 5 + { + // min on face t=0 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + } + else // region 0 + { + // global minimum is interior, done + fSqrDist = REAL(0.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2 + { + // min on face s=0 or s+t=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6 + { + // min on face t=0 or s+t=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1 + { + // min on face s+t=1 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + } + } + } + else // fR > 1 + { + if ( fS+fT <= REAL(1.0) ) + { + if ( fS < REAL(0.0) ) + { + if ( fT < REAL(0.0) ) // region 4p + { + // min on face s=0 or t=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fS0 ); + fT0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 3p + { + // min on face s=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + else if ( fT < REAL(0.0) ) // region 5p + { + // min on face t=0 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 0p + { + // min face on r=1 + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS, &fT ); + fR = REAL(1.0); + } + } + else + { + if ( fS < REAL(0.0) ) // region 2p + { + // min on face s=0 or s+t=1 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else if ( fT < REAL(0.0) ) // region 6p + { + // min on face t=0 or s+t=1 or r=1 + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fS ); + fT = REAL(0.0); + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + else // region 1p + { + // min on face s+t=1 or r=1 + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR, &fT ); + fS = REAL(1.0) - fT; + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + } + } + } + else + { + // segment and triangle are parallel + Vector3Copy( triOrigin, kTriSegOrigin ); + Vector3Copy( triEdge0, kTriSegDirection ); + fSqrDist = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, &fR, &fS ); + fT = REAL(0.0); + + Vector3Copy( triEdge1, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, + &fR0, &fT0 ); + fS0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + Vector3Add( triOrigin, triEdge0, kTriSegOrigin ); + Vector3Subtract( triEdge1, triEdge0, kTriSegDirection ); + fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, + kTriSegOrigin, kTriSegDirection, &fR0, &fT0 ); + fS0 = REAL(1.0) - fT0; + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(0.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + + Vector3Add( segOrigin, segDirection, kPt ); + fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, + &fS0, &fT0 ); + fR0 = REAL(1.0); + if ( fSqrDist0 < fSqrDist ) + { + fSqrDist = fSqrDist0; + fR = fR0; + fS = fS0; + fT = fT0; + } + } + + if ( pfSegP ) + *pfSegP = fR; + + if ( pfTriP0 ) + *pfTriP0 = fS; + + if ( pfTriP1 ) + *pfTriP1 = fT; + + return fSqrDist; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_internal.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_internal.h new file mode 100644 index 00000000..ba0ee380 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_internal.h @@ -0,0 +1,364 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. + +#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_ +#define _ODE_COLLISION_TRIMESH_INTERNAL_H_ + +// cylinder - trimesh +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTPL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +//**************************************************************************** +// dxTriMesh class + +#ifdef TRIMESH_INTERNAL + +#include "ode/ode_collision_kernel.h" +#include "ode/ode_collision_trimesh.h" + +#define BAN_OPCODE_AUTOLINK +#include "ode/ode_Opcode.h" +using namespace Opcode; + +struct dxTriMeshData : public dBase { + Model BVTree; + MeshInterface Mesh; + + dxTriMeshData(); + ~dxTriMeshData(); + + void Build(const void* Vertices, int VertexStide, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals, + bool Single); + + /* aabb in model space */ + dVector3 AABBCenter; + dVector3 AABBExtents; + + /* data for use in collison resolution */ + const void* Normals; + //Matrix4x4 last_trans; + dMatrix4 last_trans; +}; + + +struct dxTriMesh : public dxGeom{ + // Callbacks + dTriCallback* Callback; + dTriArrayCallback* ArrayCallback; + dTriRayCallback* RayCallback; + + // Data types + dxTriMeshData* Data; + + + // Colliders + static PlanesCollider _PlanesCollider; + static SphereCollider _SphereCollider; + + + // ericf change.. keep one of these per mesh now for multithreading + + //static OBBCollider _OBBCollider; + OBBCollider _OBBCollider; + + + static RayCollider _RayCollider; + static AABBTreeCollider _AABBTreeCollider; + static LSSCollider _LSSCollider; + + // Some constants + static CollisionFaces Faces; + + // Temporal coherence + struct SphereTC : public SphereCache{ + dxGeom* Geom; + }; + dArray SphereTCCache; + static SphereCache defaultSphereCache; + + struct BoxTC : public OBBCache{ + dxGeom* Geom; + }; + dArray BoxTCCache; + + // ericf change - we keep one of these per trimesh + // so we can multithread.. + //static OBBCache defaultBoxCache; + OBBCache boxCache; + + struct CCylinderTC : public LSSCache{ + dxGeom* Geom; + }; + dArray CCylinderTCCache; + static LSSCache defaultCCylinderCache; + + bool doSphereTC; + bool doBoxTC; + bool doCCylinderTC; + + bool forceNormalMode; + // Functions + dxTriMesh(dSpaceID Space, dTriMeshDataID Data); + ~dxTriMesh(); + + void ClearTCCache(); + + void setForceNormalMode(int f) {forceNormalMode=f;} + + int AABBTest(dxGeom* g, dReal aabb[6]); + void computeAABB(); +}; + +/* // Fetches a contact */ +/* inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ */ +/* dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff)); */ +/* return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); */ +/* } */ + +// Fetches a triangle +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){ + VertexPointers VP; + TriMesh->Data->Mesh.GetTriangle(VP, Index); + for (int i = 0; i < 3; i++){ + Out[i][0] = VP.Vertex[i]->x; + Out[i][1] = VP.Vertex[i]->y; + Out[i][2] = VP.Vertex[i]->z; + Out[i][3] = 0; + } +} + +// Fetches a triangle +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){ + VertexPointers VP; + TriMesh->Data->Mesh.GetTriangle(VP, Index); + for (int i = 0; i < 3; i++){ + dVector3 v; + v[0] = VP.Vertex[i]->x; + v[1] = VP.Vertex[i]->y; + v[2] = VP.Vertex[i]->z; + v[3] = 0; + + dMULTIPLY0_331(Out[i], Rotation, v); + Out[i][0] += Position[0]; + Out[i][1] += Position[1]; + Out[i][2] += Position[2]; + Out[i][3] = 0; + } +} + +// Creates an OPCODE matrix from an ODE matrix +inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){ + Out.m[0][0] = (float) Rotation[0]; + Out.m[1][0] = (float) Rotation[1]; + Out.m[2][0] = (float) Rotation[2]; + + Out.m[0][1] = (float) Rotation[4]; + Out.m[1][1] = (float) Rotation[5]; + Out.m[2][1] = (float) Rotation[6]; + + Out.m[0][2] = (float) Rotation[8]; + Out.m[1][2] = (float) Rotation[9]; + Out.m[2][2] = (float) Rotation[10]; + + Out.m[3][0] = (float) Position[0]; + Out.m[3][1] = (float) Position[1]; + Out.m[3][2] = (float) Position[2]; + + Out.m[0][3] = 0.0f; + Out.m[1][3] = 0.0f; + Out.m[2][3] = 0.0f; + Out.m[3][3] = 1.0f; + + return Out; +} + +// Outputs a matrix to 3 vectors +inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){ + Right[0] = Matrix[0 * 4 + 0]; + Right[1] = Matrix[1 * 4 + 0]; + Right[2] = Matrix[2 * 4 + 0]; + Right[3] = REAL(0.0); + Up[0] = Matrix[0 * 4 + 1]; + Up[1] = Matrix[1 * 4 + 1]; + Up[2] = Matrix[2 * 4 + 1]; + Up[3] = REAL(0.0); + Direction[0] = Matrix[0 * 4 + 2]; + Direction[1] = Matrix[1 * 4 + 2]; + Direction[2] = Matrix[2 * 4 + 2]; + Direction[3] = REAL(0.0); +} + +// Outputs a matrix to 3 vectors +inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){ + Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]); +} + +// Creates an OPCODE matrix from an ODE matrix +inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + return MakeMatrix(Position, Rotation, Out); +} + +// Finds barycentric +inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){ + dReal w = REAL(1.0) - u - v; + + Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v); + Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v); + Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v); + Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v); +} + +// Performs a callback +inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){ + if (TriMesh->Callback != null){ + return TriMesh->Callback(TriMesh, Object, TriIndex); + } + else return true; +} + +// Some utilities +template const T& dcMAX(const T& x, const T& y){ + return x > y ? x : y; +} + +template const T& dcMIN(const T& x, const T& y){ + return x < y ? x : y; +} + +dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, + const dVector3 triEdge1, const dVector3 triEdge2, + dReal* pfSParam = 0, dReal* pfTParam = 0 ); + +dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, + const dVector3 seg2Origin, const dVector3 seg2Direction, + dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 ); + +dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, + const dVector3 triOrigin, + const dVector3 triEdge1, const dVector3 triEdge2, + dReal* t = 0, dReal* u = 0, dReal* v = 0 ); + +inline +void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result ) +{ + result[0] = left[0] - right[0]; + result[1] = left[1] - right[1]; + result[2] = left[2] - right[2]; + result[3] = REAL(0.0); +} + +inline +void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result ) +{ + result[0] = left[0] + right[0]; + result[1] = left[1] + right[1]; + result[2] = left[2] + right[2]; + result[3] = REAL(0.0); +} + +inline +void Vector3Negate( const dVector3 in, dVector3 out ) +{ + out[0] = -in[0]; + out[1] = -in[1]; + out[2] = -in[2]; + out[3] = REAL(0.0); +} + +inline +void Vector3Copy( const dVector3 in, dVector3 out ) +{ + out[0] = in[0]; + out[1] = in[1]; + out[2] = in[2]; + out[3] = REAL(0.0); +} + +inline +void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out ) +{ + out[0] = in[0] * scalar; + out[1] = in[1] * scalar; + out[2] = in[2] * scalar; + out[3] = REAL(0.0); +} + +inline +void TransformVector3( const dVector3 in, + const dMatrix3 orientation, const dVector3 position, + dVector3 out ) +{ + dMULTIPLY0_331( out, orientation, in ); + out[0] += position[0]; + out[1] += position[1]; + out[2] += position[2]; +} + +//------------------------------------------------------------------------------ +/** + @brief Check for intersection between triangle and capsule. + + @param dist [out] Shortest distance squared between the triangle and + the capsule segment (central axis). + @param t [out] t value of point on segment that's the shortest distance + away from the triangle, the coordinates of this point + can be found by (cap.seg.end - cap.seg.start) * t, + or cap.seg.ipol(t). + @param u [out] Barycentric coord on triangle. + @param v [out] Barycentric coord on triangle. + @return True if intersection exists. + + The third Barycentric coord is implicit, ie. w = 1.0 - u - v + The Barycentric coords give the location of the point on the triangle + closest to the capsule (where the distance between the two shapes + is the shortest). +*/ +inline +bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, + const dReal radius, const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* dist, dReal* t, dReal* u, dReal* v ) +{ + dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, + t, u, v ); + + if ( dist ) + *dist = sqrDist; + + return ( sqrDist <= (radius * radius) ); +} + +#endif //TRIMESH_INTERNAL + +#endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_ diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_plane.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_plane.cpp new file mode 100644 index 00000000..5ff9a9d4 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_plane.cpp @@ -0,0 +1,129 @@ +// TriMesh vs Plane. +// +// CURRENT STATE: +// - Meshes collide with planes, but require a large number of contacts. +// - Have simple contact reduction (basically takes the contacts with the greatest depth). +// TODO LIST: +// - Reduce the number of contacts better. +// +//-James Dolan. + +// 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 + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +#define REDUCE_CONTACTS 1 + +int dCollideTPL(dxGeom* gmesh, dxGeom* gplane, int Flags, dContactGeom* Contacts, int Stride) +{ + //printf("eep\n"); + int ret = 0; + dxTriMesh *pTriMesh = (dxTriMesh *) gmesh; + dReal planeEq[4]; + Matrix4x4 WMat; + const dVector3 &TLPosition = *(const dVector3*)dGeomGetPosition(pTriMesh); + const dMatrix3 &TLRotation = *(const dMatrix3*)dGeomGetRotation(pTriMesh); + PlanesCache planeCache; + dxBody *pTriMeshBody = pTriMesh->body; + + if(!pTriMeshBody) + return ret; + + PlanesCollider &planeCollider = pTriMesh->_PlanesCollider; + + dGeomPlaneGetParams(gplane, planeEq); + + planeCollider.Collide(planeCache, (Plane *)planeEq, 1, pTriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, WMat)); + + int iTriCount = planeCollider.GetNbTouchedPrimitives(); + + if(iTriCount > 0) + { + const int *pIndices = (const int*)planeCollider.GetTouchedPrimitives(); + if(pTriMesh->ArrayCallback) + { + pTriMesh->ArrayCallback(pTriMesh, gplane, pIndices, iTriCount); + } + int iOutContactCount = 0; + int iMaxContactCount = (Flags & 0xffff); + for(int i=0; i 0) + { + if(iOutContactCount < iMaxContactCount) + { + // Just add the contact. + dContactGeom *pContact = SAFECONTACT(Flags, Contacts, iOutContactCount, Stride); + *pContact = tNewContact; + iOutContactCount++; + } + else + { + #if REDUCE_CONTACTS + // Replace the contact with the shortest depth + // assuming our depth is greater. + dContactGeom *pContact = SAFECONTACT(Flags, Contacts, 0, Stride); + for(int j=1; jdepth < pContact->depth) + { + pContact = pTemp; + } + } + if(pContact->depth < tNewContact.depth) + { + *pContact = tNewContact; + } + #else + break; + #endif + } + } + } + ret = iOutContactCount; + } + + return ret; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_ray.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_ray.cpp new file mode 100644 index 00000000..d539c944 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_ray.cpp @@ -0,0 +1,134 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + + // 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 + +// TriMesh code by Erwin de Vries. + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){ + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + RayCollider& Collider = TriMesh->_RayCollider; + + dReal Length = dGeomRayGetLength(RayGeom); + + int FirstContact, BackfaceCull; + dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + + Collider.SetFirstContact(FirstContact != 0); + Collider.SetClosestHit(ClosestHit != 0); + Collider.SetCulling(BackfaceCull != 0); + Collider.SetMaxDist(Length); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + /* Make Ray */ + Ray WorldRay; + WorldRay.mOrig.x = Origin[0]; + WorldRay.mOrig.y = Origin[1]; + WorldRay.mOrig.z = Origin[2]; + WorldRay.mDir.x = Direction[0]; + WorldRay.mDir.y = Direction[1]; + WorldRay.mDir.z = Direction[2]; + + /* Intersect */ + Matrix4x4 amatrix; + int TriCount = 0; + if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) { + TriCount = TriMesh->Faces.GetNbFaces(); + } + + if (TriCount == 0) { + return 0; + } + + const CollisionFace* Faces = TriMesh->Faces.GetFaces(); + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++) { + if (OutTriCount == (Flags & 0xffff)) { + break; + } + if (TriMesh->RayCallback == null || + TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, + Faces[i].mU, Faces[i].mV)) { + const int& TriIndex = Faces[i].mFaceID; + if (!Callback(TriMesh, RayGeom, TriIndex)) { + continue; + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + dVector3 dv[3]; + FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); + + float T = Faces[i].mDistance; + Contact->pos[0] = Origin[0] + (Direction[0] * T); + Contact->pos[1] = Origin[1] + (Direction[1] * T); + Contact->pos[2] = Origin[2] + (Direction[2] * T); + Contact->pos[3] = REAL(0.0); + + dVector3 vu; + vu[0] = dv[1][0] - dv[0][0]; + vu[1] = dv[1][1] - dv[0][1]; + vu[2] = dv[1][2] - dv[0][2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = dv[2][0] - dv[0][0]; + vv[1] = dv[2][1] - dv[0][1]; + vv[2] = dv[2][2] - dv[0][2]; + vv[3] = REAL(0.0); + + dCROSS(Contact->normal, =, vv, vu); // Reversed + + dNormalize3(Contact->normal); + + Contact->depth = T; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + + OutTriCount++; + } + } + return OutTriCount; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_sphere.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_sphere.cpp new file mode 100644 index 00000000..ed2bb560 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_sphere.cpp @@ -0,0 +1,502 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + + // 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 + +// TriMesh code by Erwin de Vries. + +#include + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +#define MERGECONTACTS + +// Ripped from Opcode 1.1. +static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, float& u, float& v){ + //calculate plane of triangle + dVector4 Plane; + dCROSS(Plane, =, Edge0, Edge1); + Plane[3] = dDOT(Plane, Origin); + + double min = 0.00000001; + + if ((std::abs(Plane[0]) <= min) && (std::abs(Plane[1]) <= min) + && (std::abs(Plane[2]) <= min) && (std::abs(Plane[3]) <= min)){ + printf("ode_collision_trimesh_sphere.cpp: Plane vector has zero length..\n"); + printf("vals: %f %f %f %f\n", std::abs(Plane[0]), std::abs(Plane[1]), std::abs(Plane[2]), std::abs(Plane[3])); + return 0; + } + //normalize + dNormalize4(Plane); + + /* If the center of the sphere is within the positive halfspace of the + * triangle's plane, allow a contact to be generated. + * If the center of the sphere made it into the positive halfspace of a + * back-facing triangle, then the physics update and/or velocity needs + * to be adjusted (penetration has occured anyway). + */ + + float side = dDOT(Plane,Center) - Plane[3]; + + if(side < 0.0f) { + return false; + } + + // now onto the bulk of the collision... + + dVector3 Diff; + Diff[0] = Origin[0] - Center[0]; + Diff[1] = Origin[1] - Center[1]; + Diff[2] = Origin[2] - Center[2]; + Diff[3] = Origin[3] - Center[3]; + + float A00 = dDOT(Edge0, Edge0); + float A01 = dDOT(Edge0, Edge1); + float A11 = dDOT(Edge1, Edge1); + + float B0 = dDOT(Diff, Edge0); + float B1 = dDOT(Diff, Edge1); + + float C = dDOT(Diff, Diff); + + float Det = dFabs(A00 * A11 - A01 * A01); + u = A01 * B1 - A11 * B0; + v = A01 * B0 - A00 * B1; + + float DistSq; + + if (u + v <= Det){ + if(u < REAL(0.0)){ + if(v < REAL(0.0)){ // region 4 + if(B0 < REAL(0.0)){ + v = REAL(0.0); + if (-B0 >= A00){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + else{ + u = REAL(0.0); + if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else if(-B1 >= A11){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else{ // region 3 + u = REAL(0.0); + if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else if(-B1 >= A11){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else if(v < REAL(0.0)){ // region 5 + v = REAL(0.0); + if (B0 >= REAL(0.0)){ + u = REAL(0.0); + DistSq = C; + } + else if (-B0 >= A00){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + else{ // region 0 + // minimum at interior point + if (Det == REAL(0.0)){ + u = REAL(0.0); + v = REAL(0.0); + DistSq = FLT_MAX; + } + else{ + float InvDet = REAL(1.0) / Det; + u *= InvDet; + v *= InvDet; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + } + else{ + float Tmp0, Tmp1, Numer, Denom; + + if(u < REAL(0.0)){ // region 2 + Tmp0 = A01 + B0; + Tmp1 = A11 + B1; + if (Tmp1 > Tmp0){ + Numer = Tmp1 - Tmp0; + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + u = REAL(1.0); + v = REAL(0.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = Numer / Denom; + v = REAL(1.0) - u; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + else{ + u = REAL(0.0); + if(Tmp1 <= REAL(0.0)){ + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else if(B1 >= REAL(0.0)){ + v = REAL(0.0); + DistSq = C; + } + else{ + v = -B1 / A11; + DistSq = B1 * v + C; + } + } + } + else if(v < REAL(0.0)){ // region 6 + Tmp0 = A01 + B1; + Tmp1 = A00 + B0; + if (Tmp1 > Tmp0){ + Numer = Tmp1 - Tmp0; + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + v = REAL(1.0); + u = REAL(0.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + v = Numer / Denom; + u = REAL(1.0) - v; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + else{ + v = REAL(0.0); + if (Tmp1 <= REAL(0.0)){ + u = REAL(1.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else if(B0 >= REAL(0.0)){ + u = REAL(0.0); + DistSq = C; + } + else{ + u = -B0 / A00; + DistSq = B0 * u + C; + } + } + } + else{ // region 1 + Numer = A11 + B1 - A01 - B0; + if (Numer <= REAL(0.0)){ + u = REAL(0.0); + v = REAL(1.0); + DistSq = A11 + REAL(2.0) * B1 + C; + } + else{ + Denom = A00 - REAL(2.0) * A01 + A11; + if (Numer >= Denom){ + u = REAL(1.0); + v = REAL(0.0); + DistSq = A00 + REAL(2.0) * B0 + C; + } + else{ + u = Numer / Denom; + v = REAL(1.0) - u; + DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; + } + } + } + } + + Dist = dSqrt(dFabs(DistSq)); + + if (Dist <= Radius){ + Dist = Radius - Dist; + return true; + } + else return false; +} + +int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + // Init + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + SphereCollider& Collider = TriMesh->_SphereCollider; + + const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); + dReal Radius = dGeomSphereGetRadius(SphereGeom); + + // Sphere + Sphere Sphere; + Sphere.mCenter.x = Position[0]; + Sphere.mCenter.y = Position[1]; + Sphere.mCenter.z = Position[2]; + Sphere.mRadius = Radius; + + Matrix4x4 amatrix; + + // TC results + if (TriMesh->doSphereTC) { + dxTriMesh::SphereTC* sphereTC = 0; + for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){ + if (TriMesh->SphereTCCache[i].Geom == SphereGeom){ + sphereTC = &TriMesh->SphereTCCache[i]; + break; + } + } + + if (!sphereTC){ + TriMesh->SphereTCCache.push(dxTriMesh::SphereTC()); + + sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1]; + sphereTC->Geom = SphereGeom; + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, + &MakeMatrix(TLPosition, TLRotation, amatrix)); + } + else { + Collider.SetTemporalCoherence(false); + Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, + &MakeMatrix(TLPosition, TLRotation, amatrix)); + } + + // get results + int TriCount = Collider.GetNbTouchedPrimitives(); + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (TriCount != 0){ + if (TriMesh->ArrayCallback != null){ + TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount); + } + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++){ + if (OutTriCount == (Flags & 0xffff)){ + break; + } + + const int& TriIndex = Triangles[i]; + + dVector3 dv[3]; + FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); + + dVector3& v0 = dv[0]; + dVector3& v1 = dv[1]; + dVector3& v2 = dv[2]; + + dVector3 vu; + vu[0] = v1[0] - v0[0]; + vu[1] = v1[1] - v0[1]; + vu[2] = v1[2] - v0[2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = v2[0] - v0[0]; + vv[1] = v2[1] - v0[1]; + vv[2] = v2[2] - v0[2]; + vv[3] = REAL(0.0); + + dReal Depth; + float u, v; + if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){ + continue; // Sphere doesnt hit triangle + } + dReal w = REAL(1.0) - u - v; + + if (Depth < REAL(0.0)){ + Depth = REAL(0.0); + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v); + Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v); + Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v); + Contact->pos[3] = REAL(0.0); + + dVector4 Plane; + dCROSS(Plane, =, vv, vu); // Reversed + Plane[3] = dDOT(Plane, v0); // Using normal as plane. + + dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later + Plane[0] /= Area; + Plane[1] /= Area; + Plane[2] /= Area; + Plane[3] /= Area; + + Contact->normal[0] = Plane[0]; + Contact->normal[1] = Plane[1]; + Contact->normal[2] = Plane[2]; + Contact->normal[3] = REAL(0.0); + + Contact->depth = Depth; + + //Contact->g1 = TriMesh; + //Contact->g2 = SphereGeom; + + OutTriCount++; + } + +#ifdef MERGECONTACTS // Merge all contacts into 1 + if (OutTriCount != 0){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride); + + if (OutTriCount != 1){ + Contact->normal[0] *= Contact->depth; + Contact->normal[1] *= Contact->depth; + Contact->normal[2] *= Contact->depth; + Contact->normal[3] *= Contact->depth; + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride); + + Contact->pos[0] += TempContact->pos[0]; + Contact->pos[1] += TempContact->pos[1]; + Contact->pos[2] += TempContact->pos[2]; + Contact->pos[3] += TempContact->pos[3]; + + Contact->normal[0] += TempContact->normal[0] * TempContact->depth; + Contact->normal[1] += TempContact->normal[1] * TempContact->depth; + Contact->normal[2] += TempContact->normal[2] * TempContact->depth; + Contact->normal[3] += TempContact->normal[3] * TempContact->depth; + } + + Contact->pos[0] /= OutTriCount; + Contact->pos[1] /= OutTriCount; + Contact->pos[2] /= OutTriCount; + Contact->pos[3] /= OutTriCount; + + // Remember to divide in square space. + Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount); + + dNormalize3(Contact->normal); + } + + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + + return 1; + } + else return 0; +#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts + if (OutTriCount != 0){ + if (OutTriCount != 1){ + dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal; + Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth; + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + Normal[0] += Contact->normal[0] * Contact->depth; + Normal[1] += Contact->normal[1] * Contact->depth; + Normal[2] += Contact->normal[2] * Contact->depth; + Normal[3] += Contact->normal[3] * Contact->depth; + } + dNormalize3(Normal); + + for (int i = 1; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + Contact->normal[0] = Normal[0]; + Contact->normal[1] = Normal[1]; + Contact->normal[2] = Normal[2]; + Contact->normal[3] = Normal[3]; + + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + } + } + else{ + SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh; + SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom; + } + + return OutTriCount; + } + else return 0; +#else //MERGECONTACTNORMALS // Just gather penetration depths and return + for (int i = 0; i < OutTriCount; i++){ + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); + + //Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal)); + + /*Contact->normal[0] /= Contact->depth; + Contact->normal[1] /= Contact->depth; + Contact->normal[2] /= Contact->depth; + Contact->normal[3] /= Contact->depth;*/ + + Contact->g1 = TriMesh; + Contact->g2 = SphereGeom; + } + + return OutTriCount; +#endif // MERGECONTACTS + } + else return 0; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_trimesh.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_trimesh.cpp new file mode 100644 index 00000000..581baea2 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_trimesh_trimesh.cpp @@ -0,0 +1,2162 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + + // 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 + +// TriMesh/TriMesh collision code by Jeff Smith (c) 2004 +// + +#ifdef _MSC_VER +#include +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +//static inline double round(double x){return x >= 0.0 ? floor(x + 0.5) : ceil(x - 0.5);} +#endif + +#include "ode/ode_collision.h" +#include "ode/ode_matrix.h" +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" +#include + + +#define TRIMESH_INTERNAL +#include "ode/ode_collision_trimesh_internal.h" + +#define SMALL_ELT 2.5e-4 +#define EXPANDED_ELT_THRESH 1.0e-3 +#define DISTANCE_EPSILON 1.0e-8 +#define VELOCITY_EPSILON 1.0e-5 +#define TINY_PENETRATION 5.0e-6 + +// new method for tri on tri intersection by Eric Froemling +// it is pretty much a stripped out version of the original +// which no longer uses body velocity information (we want resting contacts to somewhat work) +// it basically assigns the contact normal to be one of the two face normals in a random-ish fashion +// and the penetration depth is the co-normal distance between any two vertices A and B, +// i.e. d = DOT(n, (A-B)) - this can erroneously return huge penetration depths +// so we rely somewhat on contact filtering, penetration depth limiting, etc outside of the ode lib + +// another improvement over the original is that if fewer contacts are requested than we find, +// the contacts we return are sampled evenly across the list of contacts - in the old code +// just the first n contacts would be returned, which would usually be clustered to one side of a +// collision area thus creating incorrect collisionss + +#define ERICF_METHOD 1 + +struct LineContactSet +{ + dVector3 Points[8]; + int Count; +}; + + +//static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); +static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*, + const dVector3, const dVector3, dReal, int&); +static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, + dReal isectpt1[3],dReal isectpt2[3]); +inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); +static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); +//static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); +#ifndef ERICF_METHOD +static bool FindTriSolidIntrsection(const dVector3 Tri[3], + const dVector4 Planes[6], int numSides, + LineContactSet& ClippedPolygon ); +static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, + dVector3 in_n, dVector3* in_col_v, dReal &out_depth); +static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); +static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point); +static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v); +#endif + + + + +/* some math macros */ +#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \ + dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \ + dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; } + +#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) + +#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; } + +#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; } + +#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; } + +#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; } + +#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; } + +#define COMBO(combo,p,t,q) { combo[0]=p[0]+t*q[0]; combo[1]=p[1]+t*q[1]; combo[2]=p[2]+t*q[2]; } + +#define LENGTH(x) ((dReal) dSqrt(dDOT(x, x))) + +#define DEPTH(d, p, q, n) d = (p[0] - q[0])*n[0] + (p[1] - q[1])*n[1] + (p[2] - q[2])*n[2]; + +inline const dReal dMin(const dReal x, const dReal y) +{ + return x < y ? x : y; +} + + +inline void +SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, + dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, + dVector3 n, dVector3 n1, dVector3 n2) +{ + if (pen_v == v1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } +} + + + + +int +dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + dxTriMesh* TriMesh1 = (dxTriMesh*) g1; + dxTriMesh* TriMesh2 = (dxTriMesh*) g2; + + dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals; + dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals; + + const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); + // TLRotation1 = column-major order + const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); + + const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); + // TLRotation2 = column-major order + const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); + + AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider; + + static BVTCache ColCache; + ColCache.Model0 = &TriMesh1->Data->BVTree; + ColCache.Model1 = &TriMesh2->Data->BVTree; + + // Collision query + Matrix4x4 amatrix, bmatrix; + BOOL IsOk = Collider.Collide(ColCache, + &MakeMatrix(TLPosition1, TLRotation1, amatrix), + &MakeMatrix(TLPosition2, TLRotation2, bmatrix) ); + + + // Make "double" versions of these matrices, if appropriate + dMatrix4 A, B; + dMakeMatrix4(TLPosition1, TLRotation1, A); + dMakeMatrix4(TLPosition2, TLRotation2, B); + + + if (IsOk) { + // Get collision status => if true, objects overlap + if ( Collider.GetContactStatus() ) { + // Number of colliding pairs and list of pairs + int TriCount = Collider.GetNbPairs(); + int requestedCount = Flags & 0xffff; + const Pair* CollidingPairs = Collider.GetPairs(); + +#if ERICF_METHOD + if (TriCount > 0){ + int OutTriCount = 0; + int id1, id2; + dVector3 v1[3], v2[3], CoplanarPt; + dVector3 e1, e2, n1, n2, n; + dReal depth; + + // only do these expensive inversions once + dMatrix4 InvMatrix1, InvMatrix2; + dInvertMatrix4(A, InvMatrix1); + dInvertMatrix4(B, InvMatrix2); + + bool forceNormal1 = false; + bool forceNormal2 = false; + if (TriMesh1->forceNormalMode) + forceNormal1 = true; + if (TriMesh2->forceNormalMode) + forceNormal2 = true; + if (forceNormal1 && forceNormal2){ + forceNormal1 = false; + forceNormal2 = false; + } + + //if they want less than we have, we skip around so as not to return only one side, etc + float triInterval = (float)TriCount/requestedCount; + if (triInterval < 1) triInterval = 1; + //cout << "they want " << requestedCount << " we have " << TriCount << " using interval " << triInterval << endl; + int i; + int iteration = 0; + for (float iFloat = 0; iFloat < TriCount; iFloat += triInterval){ + iteration++; + i = (int)round(iFloat); + if (i >= TriCount) + break; + if (OutTriCount < requestedCount) { + + int IsCoplanar = 0; + dReal IsectPt1[3], IsectPt2[3]; + + id1 = CollidingPairs[i].id0; + id2 = CollidingPairs[i].id1; + + // grab the colliding triangles + FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1); + FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2); + // Since we'll be doing matrix transfomrations, we need to + // make sure that all vertices have four elements + for (int j=0; j<3; j++) { + v1[j][3] = 1.0; + v2[j][3] = 1.0; + } + + if (TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2],&IsCoplanar, + IsectPt1, IsectPt2) ){ + + //cout << "got contact " << i << endl; + + // Compute the normals of the colliding faces + if (TriNormals1 == NULL) { + SUB( e1, v1[1], v1[0] ); + SUB( e2, v1[2], v1[0] ); + CROSS( n1, e1, e2 ); + dNormalize3(n1); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e1[0] = TriNormals1[id1*3]; + e1[1] = TriNormals1[id1*3 + 1]; + e1[2] = TriNormals1[id1*3 + 2]; + e1[3] = 0.0; + + //dMultiply1(n1, TLRotation1, e1, 3, 3, 1); + dMultiply0(n1, TLRotation1, e1, 3, 3, 1); + n1[3] = 1.0; + } + + if (TriNormals2 == NULL) { + SUB( e1, v2[1], v2[0] ); + SUB( e2, v2[2], v2[0] ); + CROSS( n2, e1, e2); + dNormalize3(n2); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e2[0] = TriNormals2[id2*3]; + e2[1] = TriNormals2[id2*3 + 1]; + e2[2] = TriNormals2[id2*3 + 2]; + e2[3] = 0.0; + + //dMultiply1(n2, TLRotation2, e2, 3, 3, 1); + dMultiply0(n2, TLRotation2, e2, 3, 3, 1); + n2[3] = 1.0; + } + + if (IsCoplanar) { + // We can reach this case if the faces are coplanar, OR + // if they don't actually intersect. (OPCODE can make + // mistakes) + if (fabs(dDOT(n1, n2)) > 0.999) { + // If the faces are coplanar, we declare that the point of + // contact is at the average location of the vertices of + // both faces + dVector3 ContactPt; + for (int j=0; j<3; j++) { + ContactPt[j] = 0.0; + for (int k=0; k<3; k++) + ContactPt[j] += v1[k][j] + v2[k][j]; + ContactPt[j] /= 6.0; + } + ContactPt[3] = 1.0; + + // and the contact normal is the normal of face 2 + // (could be face 1, because they are the same) + SET(n, n2); + + // and the penetration depth is the co-normal + // distance between any two vertices A and B, + // i.e. d = DOT(n, (A-B)) + DEPTH(depth, v1[1], v2[1], n); + if (depth < 0) + depth *= -1.0; + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, n, depth, OutTriCount); + } + } + else{ + for (int j=0; j<3; j++) + CoplanarPt[j] = (dReal) ( (IsectPt1[j] + IsectPt2[j]) / 2.0 ); + CoplanarPt[3] = 1.0; + + dVector3 ContactPt; + for (int j=0; j<3; j++) + ContactPt[j] = CoplanarPt[j]; + + if (((iteration%2==0) || forceNormal2) && (!forceNormal1)){ + //if (i%2==0){ + SET(n,n2); + for (int j = 0;j<3;j++) + n[j]*=-1; + } + else{ + SET(n, n1); + } + + depth = 0.01; + + // and the penetration depth is the co-normal + // distance between any two vertices A and B, + // i.e. d = DOT(n, (A-B)) + DEPTH(depth, v1[1], v2[1], n); + if (depth < 0) + depth *= -1.0; + + GenerateContact(Flags,Contacts,Stride,TriMesh1,TriMesh2, + ContactPt, n, depth, OutTriCount); + } + + } + } + else + break; + } + return OutTriCount; + } +#else + + if (TriCount > 0) { + // step through the pairs, adding contacts + int id1, id2; + int OutTriCount = 0; + dVector3 v1[3], v2[3], CoplanarPt; + dVector3 e1, e2, e3, n1, n2, n, ContactNormal; + dReal depth; + dVector3 orig_pos, old_pos1, old_pos2, elt1, elt2, elt_sum; + dVector3 elt_f1[3], elt_f2[3]; + dReal contact_elt_length = SMALL_ELT; + LineContactSet firstClippedTri, secondClippedTri; + dVector3 *firstClippedElt = NULL; + dVector3 *secondClippedElt = NULL; + + + // only do these expensive inversions once + dMatrix4 InvMatrix1, InvMatrix2; + dInvertMatrix4(A, InvMatrix1); + dInvertMatrix4(B, InvMatrix2); + + + for (int i = 0; i < TriCount; i++){ + if (OutTriCount < (Flags & 0xffff)) { + + id1 = CollidingPairs[i].id0; + id2 = CollidingPairs[i].id1; + + // grab the colliding triangles + FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1); + FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2); + // Since we'll be doing matrix transfomrations, we need to + // make sure that all vertices have four elements + for (int j=0; j<3; j++) { + v1[j][3] = 1.0; + v2[j][3] = 1.0; + } + + + int IsCoplanar = 0; + dReal IsectPt1[3], IsectPt2[3]; + + // Sometimes OPCODE makes mistakes, so we look at the return + // value for TriTriIntersectWithIsectLine. A retcode of "0" + // means no intersection took place + if ( TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], + &IsCoplanar, + IsectPt1, IsectPt2) ) { + + // Compute the normals of the colliding faces + // + if (TriNormals1 == NULL) { + SUB( e1, v1[1], v1[0] ); + SUB( e2, v1[2], v1[0] ); + CROSS( n1, e1, e2 ); + dNormalize3(n1); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e1[0] = TriNormals1[id1*3]; + e1[1] = TriNormals1[id1*3 + 1]; + e1[2] = TriNormals1[id1*3 + 2]; + e1[3] = 0.0; + + //dMultiply1(n1, TLRotation1, e1, 3, 3, 1); + dMultiply0(n1, TLRotation1, e1, 3, 3, 1); + n1[3] = 1.0; + } + + if (TriNormals2 == NULL) { + SUB( e1, v2[1], v2[0] ); + SUB( e2, v2[2], v2[0] ); + CROSS( n2, e1, e2); + dNormalize3(n2); + } + else { + // If we were passed normals, we need to adjust them to take into + // account the objects' current rotations + e2[0] = TriNormals2[id2*3]; + e2[1] = TriNormals2[id2*3 + 1]; + e2[2] = TriNormals2[id2*3 + 2]; + e2[3] = 0.0; + + //dMultiply1(n2, TLRotation2, e2, 3, 3, 1); + dMultiply0(n2, TLRotation2, e2, 3, 3, 1); + n2[3] = 1.0; + } + + + if (IsCoplanar) { + // We can reach this case if the faces are coplanar, OR + // if they don't actually intersect. (OPCODE can make + // mistakes) + if (fabs(dDOT(n1, n2)) > 0.999) { + // If the faces are coplanar, we declare that the point of + // contact is at the average location of the vertices of + // both faces + dVector3 ContactPt; + for (int j=0; j<3; j++) { + ContactPt[j] = 0.0; + for (int k=0; k<3; k++) + ContactPt[j] += v1[k][j] + v2[k][j]; + ContactPt[j] /= 6.0; + } + ContactPt[3] = 1.0; + + // and the contact normal is the normal of face 2 + // (could be face 1, because they are the same) + SET(n, n2); + + // and the penetration depth is the co-normal + // distance between any two vertices A and B, + // i.e. d = DOT(n, (A-B)) + DEPTH(depth, v1[1], v2[1], n); + if (depth < 0) + depth *= -1.0; + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, n, depth, OutTriCount); + } + } + else { + // Otherwise (in non-co-planar cases), we create a coplanar + // point -- the middle of the line of intersection -- that + // will be used for various computations down the road + for (int j=0; j<3; j++) + CoplanarPt[j] = (dReal) ( (IsectPt1[j] + IsectPt2[j]) / 2.0 ); + CoplanarPt[3] = 1.0; + + // Find the ELT of the coplanar point + // + dMultiply1(orig_pos, InvMatrix1, CoplanarPt, 4, 4, 1); + dMultiply1(old_pos1, TriMesh1->Data->last_trans, orig_pos, 4, 4, 1); + SUB(elt1, CoplanarPt, old_pos1); + + dMultiply1(orig_pos, InvMatrix2, CoplanarPt, 4, 4, 1); + dMultiply1(old_pos2, TriMesh2->Data->last_trans, orig_pos, 4, 4, 1); + SUB(elt2, CoplanarPt, old_pos2); + + SUB(elt_sum, elt1, elt2); // net motion of the coplanar point + + + // Calculate how much the vertices of each face moved in the + // direction of the opposite face's normal + // + dReal total_dp1, total_dp2; + total_dp1 = 0.0; + total_dp2 = 0.0; + + for (int ii=0; ii<3; ii++) { + // find the estimated linear translation (ELT) of the vertices + // on face 1, wrt to the center of face 2. + + // un-transform this vertex by the current transform + dMultiply1(orig_pos, InvMatrix1, v1[ii], 4, 4, 1 ); + + // re-transform this vertex by last_trans (to get its old + // position) + dMultiply1(old_pos1, TriMesh1->Data->last_trans, orig_pos, 4, 4, 1); + + // Then subtract this position from our current one to find + // the elapsed linear translation (ELT) + for (int k=0; k<3; k++) { + elt_f1[ii][k] = (v1[ii][k] - old_pos1[k]) - elt2[k]; + } + + // Take the dot product of the ELT for each vertex (wrt the + // center of face2) + total_dp1 += fabs( dDOT(elt_f1[ii], n2) ); + } + + for (int ii=0; ii<3; ii++) { + // find the estimated linear translation (ELT) of the vertices + // on face 2, wrt to the center of face 1. + dMultiply1(orig_pos, InvMatrix2, v2[ii], 4, 4, 1); + dMultiply1(old_pos2, TriMesh2->Data->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + elt_f2[ii][k] = (v2[ii][k] - old_pos2[k]) - elt1[k]; + } + + // Take the dot product of the ELT for each vertex (wrt the + // center of face2) and add them + total_dp2 += fabs( dDOT(elt_f2[ii], n1) ); + } + + + //////// + // Estimate the penetration depth. + // + dReal dp; + BOOL badPen = true; + dVector3 *pen_v; // the "penetrating vertices" + dVector3 *pen_elt; // the elt_f of the penetrating face + dVector3 *col_v; // the "collision vertices" (the penetrated face) + + + depth = 0.0; + if ((total_dp1 > DISTANCE_EPSILON) || (total_dp2 > DISTANCE_EPSILON)) { + //////// + // Find the collision normal, by finding the face + // that is pointed "most" in the direction of travel + // of the two triangles + // + if (total_dp2 > total_dp1) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } + } + else { + // the total_dp is very small, so let's fall back + // to a different test + if (LENGTH(elt2) > LENGTH(elt1)) { + pen_v = v2; + pen_elt = elt_f2; + col_v = v1; + SET(n, n1); + } + else { + pen_v = v1; + pen_elt = elt_f1; + col_v = v2; + SET(n, n2); + } + } + + + for (int j=0; j<3; j++) + if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, depth, OutTriCount); + badPen = false; + } + + + if (badPen) { + // try the other normal + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + for (int j=0; j<3; j++) + if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, depth, OutTriCount); + badPen = false; + } + } + + + + //////////////////////////////////////// + // + // If we haven't found a good penetration, then we're probably straddling + // the edge of one of the objects, or the penetraing face is big + // enough that all of its vertices are outside the bounds of the + // penetrated face. + // In these cases, we do a more expensive test. We clip the penetrating + // triangle with a solid defined by the penetrated triangle, and repeat + // the tests above on this new polygon + if (badPen) { + + // Switch pen_v and n back again + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + + // Find the three sides (no top or bottom) of the solid defined by + // the edges of the penetrated triangle. + + // The dVector4 "plane" structures contain the following information: + // [0]-[2]: The normal of the face, pointing INWARDS (i.e. + // the inverse normal + // [3]: The distance between the face and the center of the + // solid, along the normal + dVector4 SolidPlanes[3]; + dVector3 tmp1; + dVector3 sn; + + for (int j=0; j<3; j++) { + e1[j] = col_v[1][j] - col_v[0][j]; + e2[j] = col_v[0][j] - col_v[2][j]; + e3[j] = col_v[2][j] - col_v[1][j]; + } + + // side 1 + CROSS(sn, e1, n); + dNormalize3(sn); + SMULT( SolidPlanes[0], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]); + + + // side 2 + CROSS(sn, e2, n); + dNormalize3(sn); + SMULT( SolidPlanes[1], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[2]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]); + + + // side 3 + CROSS(sn, e3, n); + dNormalize3(sn); + SMULT( SolidPlanes[2], sn, -1.0 ); + + ADD(tmp1, col_v[2], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]); + + + FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri); + + firstClippedElt = new dVector3[firstClippedTri.Count]; + + for (int j=0; jData->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; + } + } + else { + dMultiply1(orig_pos, InvMatrix2, firstClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos2, TriMesh2->Data->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; + } + } + + contact_elt_length = fabs(dDOT(firstClippedElt[j], n)); + + if (dp >= 0.0) { + depth = dp; + if (depth == 0.0) + depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + firstClippedTri.Points[j], n, depth, OutTriCount); + badPen = false; + } + } + + } + } + + if (badPen) { + // Switch pen_v and n (again!) + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + + // Find the three sides (no top or bottom) of the solid created by + // the penetrated triangle. + // The dVector4 "plane" structures contain the following information: + // [0]-[2]: The normal of the face, pointing INWARDS (i.e. + // the inverse normal + // [3]: The distance between the face and the center of the + // solid, along the normal + dVector4 SolidPlanes[3]; + dVector3 tmp1; + + dVector3 sn; + for (int j=0; j<3; j++) { + e1[j] = col_v[1][j] - col_v[0][j]; + e2[j] = col_v[0][j] - col_v[2][j]; + e3[j] = col_v[2][j] - col_v[1][j]; + } + + // side 1 + CROSS(sn, e1, n); + dNormalize3(sn); + SMULT( SolidPlanes[0], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]); + + + // side 2 + CROSS(sn, e2, n); + dNormalize3(sn); + SMULT( SolidPlanes[1], sn, -1.0 ); + + ADD(tmp1, col_v[0], col_v[2]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]); + + + // side 3 + CROSS(sn, e3, n); + dNormalize3(sn); + SMULT( SolidPlanes[2], sn, -1.0 ); + + ADD(tmp1, col_v[2], col_v[1]); + SMULT(tmp1, tmp1, 0.5); // center of edge + // distance from center to edge along normal + SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]); + + FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri); + + secondClippedElt = new dVector3[secondClippedTri.Count]; + + for (int j=0; jData->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; + } + } + else { + dMultiply1(orig_pos, InvMatrix2, secondClippedTri.Points[j], 4, 4, 1); + dMultiply1(old_pos2, TriMesh2->Data->last_trans, orig_pos, 4, 4, 1); + for (int k=0; k<3; k++) { + secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; + } + } + + + contact_elt_length = fabs(dDOT(secondClippedElt[j],n)); + + if (dp >= 0.0) { + depth = dp; + if (depth == 0.0) + depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + secondClippedTri.Points[j], n, depth, OutTriCount); + badPen = false; + } + } + + + } + } + + + + ///////////////// + // All conventional tests have failed at this point, so now we deal with + // cases on a more "heuristic" basis + // + + if (badPen) { + // Switch pen_v and n (for the fourth time, so they're + // what my original guess said they were) + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + if (fabs(dDOT(n1, n2)) < 0.01) { + // If we reach this point, we have (close to) perpindicular + // faces, either resting on each other or sliding in a + // direction orthogonal to both surface normals. + if (LENGTH(elt_sum) < DISTANCE_EPSILON) { + depth = (dReal) fabs(dDOT(n, elt_sum)); + + if (depth > 1e-12) { + dNormalize3(n); + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + CoplanarPt, n, depth, OutTriCount); + badPen = false; + } + else { + // If the two faces are (nearly) perfectly at rest with + // respect to each other, then we ignore the contact, + // allowing the objects to slip a little in the hopes + // that next frame, they'll give us something to work + // with. + badPen = false; + } + } + else { + // The faces are perpindicular, but moving significantly + // This can be sliding, or an unusual edge-straddling + // penetration. + dVector3 cn; + + CROSS(cn, n1, n2); + dNormalize3(cn); + SET(n, cn); + + // The shallowest ineterpenetration of the faces + // is the depth + dVector3 ContactPt; + dVector3 dvTmp; + dReal rTmp; + depth = dInfinity; + for (int j=0; j<3; j++) { + for (int k=0; k<3; k++) { + SUB(dvTmp, col_v[k], pen_v[j]); + + rTmp = dDOT(dvTmp, n); + if ( fabs(rTmp) < fabs(depth) ) { + depth = rTmp; + SET( ContactPt, pen_v[j] ); + contact_elt_length = fabs(dDOT(pen_elt[j], n)); + } + } + } + if (depth < 0.0) { + SMULT(n, n, -1.0); + depth *= -1.0; + } + + if ((depth > 0.0) && (depth <= contact_elt_length)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, n, depth, OutTriCount); + badPen = false; + } + + } + } + } + + + if (badPen) { + // Use as the normal the direction of travel, rather than any particular + // face normal + // + dVector3 esn; + + if (pen_v == v1) { + SMULT(esn, elt_sum, -1.0); + } + else { + SET(esn, elt_sum); + } + dNormalize3(esn); + + + // The shallowest ineterpenetration of the faces + // is the depth + dVector3 ContactPt; + depth = dInfinity; + for (int j=0; j<3; j++) { + for (int k=0; k<3; k++) { + DEPTH(dp, col_v[k], pen_v[j], esn); + if ( (ExamineContactPoint(col_v, esn, pen_v[j])) && + ( fabs(dp) < fabs(depth)) ) { + depth = dp; + SET( ContactPt, pen_v[j] ); + contact_elt_length = fabs(dDOT(pen_elt[j], esn)); + } + } + } + + if ((depth > 0.0) && (depth <= contact_elt_length)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + ContactPt, esn, depth, OutTriCount); + badPen = false; + } + } + + + if (badPen) { + // If the direction of motion is perpindicular to both normals + if ( (fabs(dDOT(n1, elt_sum)) < 0.01) && (fabs(dDOT(n2, elt_sum)) < 0.01) ) { + dVector3 esn; + if (pen_v == v1) { + SMULT(esn, elt_sum, -1.0); + } + else { + SET(esn, elt_sum); + } + + dNormalize3(esn); + + + // Look at the clipped points again, checking them against this + // new normal + for (int j=0; j= 0.0) { + contact_elt_length = fabs(dDOT(firstClippedElt[j], esn)); + + depth = dp; + //if (depth == 0.0) + //depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + firstClippedTri.Points[j], esn, depth, OutTriCount); + badPen = false; + } + } + } + + if (badPen) { + // If this test failed, try it with the second set of clipped faces + for (int j=0; j= 0.0) { + contact_elt_length = fabs(dDOT(secondClippedElt[j], esn)); + + depth = dp; + //if (depth == 0.0) + //depth = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) + depth = contact_elt_length; + + if (depth <= contact_elt_length) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + secondClippedTri.Points[j], esn, depth, OutTriCount); + badPen = false; + } + } + } + } + } + } + + + + if (badPen) { + // if we have very little motion, we're dealing with resting contact + // and shouldn't reference the ELTs at all + // + if (LENGTH(elt_sum) < VELOCITY_EPSILON) { + + // instead of a "contact_elt_length" threshhold, we'll use an + // arbitrary, small one + for (int j=0; j<3; j++) { + DEPTH(dp, CoplanarPt, pen_v[j], n); + + if (dp == 0.0) + dp = TINY_PENETRATION; + + if ( (dp > 0.0) && (dp <= SMALL_ELT)) { + // Add a contact + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, (dReal) dp, OutTriCount); + badPen = false; + } + } + + + if (badPen) { + // try the other normal + SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); + + for (int j=0; j<3; j++) { + DEPTH(dp, CoplanarPt, pen_v[j], n); + + if (dp == 0.0) + dp = TINY_PENETRATION; + + if ( (dp > 0.0) && (dp <= SMALL_ELT)) { + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + pen_v[j], n, (dReal) dp, OutTriCount); + badPen = false; + } + } + } + + + + } + } + + if (badPen) { + // find the nearest existing contact, and replicate it's + // normal and depth + // + dContactGeom* Contact; + dVector3 pos_diff; + dReal min_dist, dist; + + min_dist = dInfinity; + depth = 0.0; + for (int j=0; jpos, CoplanarPt); + + dist = dDOT(pos_diff, pos_diff); + if (dist < min_dist) { + min_dist = dist; + depth = Contact->depth; + SMULT(ContactNormal, Contact->normal, -1.0); + } + } + + if (depth > 0.0) { + // Add a tiny contact at the coplanar point + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + CoplanarPt, ContactNormal, depth, OutTriCount); + badPen = false; + } + } + + + if (badPen) { + // Add a tiny contact at the coplanar point + if (-dDOT(elt_sum, n1) > -dDOT(elt_sum, n2)) { + SET(ContactNormal, n1); + } + else { + SET(ContactNormal, n2); + } + + GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, + CoplanarPt, ContactNormal, TINY_PENETRATION, OutTriCount); + badPen = false; + } + + + } // not coplanar (main loop) + } // TriTriIntersectWithIsectLine + + // Free memory + delete[] firstClippedElt; + firstClippedElt = NULL; + delete[] secondClippedElt; + secondClippedElt = NULL; + + } // if (OutTriCount < (Flags & 0xffff)) + } + // Return the number of contacts + return OutTriCount; + + } +#endif //eric method + + } + } + + + // There was some kind of failure during the Collide call or + // there are no faces overlapping + return 0; +} + + + +// static void +// GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) +// { +// dVector3 Out[3]; + +// FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); + +// for (int i = 0; i < 3; i++) +// triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); +// } + + +// +// +// +#define B11 B[0] +#define B12 B[1] +#define B13 B[2] +#define B14 B[3] +#define B21 B[4] +#define B22 B[5] +#define B23 B[6] +#define B24 B[7] +#define B31 B[8] +#define B32 B[9] +#define B33 B[10] +#define B34 B[11] +#define B41 B[12] +#define B42 B[13] +#define B43 B[14] +#define B44 B[15] + +#define Binv11 Binv[0] +#define Binv12 Binv[1] +#define Binv13 Binv[2] +#define Binv14 Binv[3] +#define Binv21 Binv[4] +#define Binv22 Binv[5] +#define Binv23 Binv[6] +#define Binv24 Binv[7] +#define Binv31 Binv[8] +#define Binv32 Binv[9] +#define Binv33 Binv[10] +#define Binv34 Binv[11] +#define Binv41 Binv[12] +#define Binv42 Binv[13] +#define Binv43 Binv[14] +#define Binv44 Binv[15] + +inline void +dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) +{ + B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; + B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; + B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; + + B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; +} + + +static void +dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) +{ + dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) + -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) + +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) + +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) + -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) + +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); + + dAASSERT (det != 0.0); + + det = 1.0 / det; + + Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); + Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); + Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); + Binv14 = 0.0f; + Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); + Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); + Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); + Binv24 = 0.0f; + Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); + Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); + Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); + Binv34 = 0.0f; + Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); + Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); + Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); + Binv44 = 1.0f; +} + + + +///////////////////////////////////////////////// +// +// Triangle/Triangle intersection utilities +// +// From the article "A Fast Triangle-Triangle Intersection Test", +// Journal of Graphics Tools, 2(2), 1997 +// +// Some of this functionality is duplicated in OPCODE (see +// OPC_TriTriOverlap.h) but we have replicated it here so we don't +// have to mess with the internals of OPCODE, as well as so we can +// further optimize some of the functions. +// +// This version computes the line of intersection as well (if they +// are not coplanar): +// int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], +// dReal U0[3],dReal U1[3],dReal U2[3], +// int *coplanar, +// dReal isectpt1[3],dReal isectpt2[3]); +// +// parameters: vertices of triangle 1: V0,V1,V2 +// vertices of triangle 2: U0,U1,U2 +// +// result : returns 1 if the triangles intersect, otherwise 0 +// "coplanar" returns whether the tris are coplanar +// isectpt1, isectpt2 are the endpoints of the line of +// intersection +// + + + +#define FABS(x) ((dReal)fabs(x)) /* implement as is fastest on your machine */ + +/* if USE_EPSILON_TEST is true then we do a check: + if |dv|b) \ + { \ + dReal c; \ + c=a; \ + a=b; \ + b=c; \ + } + +#define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \ + isect0=VV0+(VV1-VV0)*D0/(D0-D1); \ + isect1=VV0+(VV2-VV0)*D0/(D0-D2); + + +#define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \ + 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 */ \ + ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ + } \ + else if(D0D2>0.0f) \ + { \ + /* here we know that d0d1<=0.0 */ \ + ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \ + } \ + else if(D1!=0.0f) \ + { \ + ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ + } \ + else if(D2!=0.0f) \ + { \ + ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } + + + +/* this edge to edge test is 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 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \ + { \ + e=Ax*Cy-Ay*Cx; \ + if(f>0) \ + { \ + if(e>=0 && e<=f) return 1; \ + } \ + else \ + { \ + if(e<=0 && e>=f) return 1; \ + } \ + } + +#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \ +{ \ + dReal Ax,Ay,Bx,By,Cx,Cy,e,d,f; \ + Ax=V1[i0]-V0[i0]; \ + 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); \ +} + +#define POINT_IN_TRI(V0,U0,U1,U2) \ +{ \ + dReal a,b,c,d0,d1,d2; \ + /* is T1 completly inside T2? */ \ + /* check if V0 is inside tri(U0,U1,U2) */ \ + a=U1[i1]-U0[i1]; \ + b=-(U1[i0]-U0[i0]); \ + c=-a*U0[i0]-b*U0[i1]; \ + 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]; \ + 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]; \ + d2=a*V0[i0]+b*V0[i1]+c; \ + if(d0*d1>0.0) \ + { \ + if(d0*d2>0.0) return 1; \ + } \ +} + +int coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3]) +{ + dReal 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]= (dReal) fabs(N[0]); + A[1]= (dReal) fabs(N[1]); + A[2]= (dReal) fabs(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 0; +} + + + +#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 coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } \ +} + + + + +/* sort so that a<=b */ +#define SORT2(a,b,smallest) \ + if(a>b) \ + { \ + dReal c; \ + c=a; \ + a=b; \ + b=c; \ + smallest=1; \ + } \ + else smallest=0; + + +inline void isect2(dReal VTX0[3],dReal VTX1[3],dReal VTX2[3],dReal VV0,dReal VV1,dReal VV2, + dReal D0,dReal D1,dReal D2,dReal *isect0,dReal *isect1,dReal isectpoint0[3],dReal isectpoint1[3]) +{ + dReal tmp=D0/(D0-D1); + dReal diff[3]; + *isect0=VV0+(VV1-VV0)*tmp; + SUB(diff,VTX1,VTX0); + MULT(diff,diff,tmp); + ADD(isectpoint0,diff,VTX0); + tmp=D0/(D0-D2); + *isect1=VV0+(VV2-VV0)*tmp; + SUB(diff,VTX2,VTX0); + MULT(diff,diff,tmp); + ADD(isectpoint1,VTX0,diff); +} + + +#if 0 +#define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \ + tmp=D0/(D0-D1); \ + isect0=VV0+(VV1-VV0)*tmp; \ + SUB(diff,VTX1,VTX0); \ + MULT(diff,diff,tmp); \ + ADD(isectpoint0,diff,VTX0); \ + tmp=D0/(D0-D2); +/* isect1=VV0+(VV2-VV0)*tmp; \ */ +/* SUB(diff,VTX2,VTX0); \ */ +/* MULT(diff,diff,tmp); \ */ +/* ADD(isectpoint1,VTX0,diff); */ +#endif + +inline int compute_intervals_isectline(dReal VERT0[3],dReal VERT1[3],dReal VERT2[3], + dReal VV0,dReal VV1,dReal VV2,dReal D0,dReal D1,dReal D2, + dReal D0D1,dReal D0D2,dReal *isect0,dReal *isect1, + dReal isectpoint0[3],dReal isectpoint1[3]) +{ + 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 */ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D0D2>0.0f) + { + /* here we know that d0d1<=0.0 */ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D1*D2>0.0f || D0!=0.0f) + { + /* here we know that d0d1<=0.0 or that D0!=0.0 */ + isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D1!=0.0f) + { + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); + } + else if(D2!=0.0f) + { + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); + } + else + { + /* triangles are coplanar */ + return 1; + } + return 0; +} + +#define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \ + 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 */ \ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ + } +#if 0 + else if(D0D2>0.0f) \ + { \ + /* here we know that d0d1<=0.0 */ \ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D1*D2>0.0f || D0!=0.0f) \ + { \ + /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ + isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D1!=0.0f) \ + { \ + isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else if(D2!=0.0f) \ + { \ + isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ + } \ + else \ + { \ + /* triangles are coplanar */ \ + coplanar=1; \ + return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ + } +#endif + + +static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], + dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, + dReal isectpt1[3],dReal isectpt2[3]) +{ + dReal E1[3],E2[3]; + dReal N1[3],N2[3],d1,d2; + dReal du0,du1,du2,dv0,dv1,dv2; + dReal D[3]; + dReal isect1[2], isect2[2]; + dReal isectpointA1[3],isectpointA2[3]; + dReal isectpointB1[3],isectpointB2[3]; + dReal du0du1,du0du2,dv0dv1,dv0dv2; + short index; + dReal vp0,vp1,vp2; + dReal up0,up1,up2; + dReal b,c,max; + int smallest1,smallest2; + + /* compute plane equation of triangle(V0,V1,V2) */ + SUB(E1,V1,V0); + SUB(E2,V2,V0); + CROSS(N1,E1,E2); + d1=-DOT(N1,V0); + /* plane equation 1: N1.X+d1=0 */ + + /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/ + du0=DOT(N1,U0)+d1; + du1=DOT(N1,U1)+d1; + du2=DOT(N1,U2)+d1; + + /* coplanarity robustness check */ +#if USE_EPSILON_TEST==TRUE + if(fabs(du0)0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */ + return 0; /* no intersection occurs */ + + /* compute plane of triangle (U0,U1,U2) */ + SUB(E1,U1,U0); + SUB(E2,U2,U0); + CROSS(N2,E1,E2); + d2=-DOT(N2,U0); + /* plane equation 2: N2.X+d2=0 */ + + /* put V0,V1,V2 into plane equation 2 */ + dv0=DOT(N2,V0)+d2; + dv1=DOT(N2,V1)+d2; + dv2=DOT(N2,V2)+d2; + +#if USE_EPSILON_TEST==TRUE + if(fabs(dv0)0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */ + return 0; /* no intersection occurs */ + + /* compute direction of intersection line */ + CROSS(D,N1,N2); + + /* compute and index to the largest component of D */ + max= (dReal) fabs(D[0]); + index=0; + b= (dReal) fabs(D[1]); + c= (dReal) fabs(D[2]); + if(b>max) {max=b;index=1;} + if(c>max) {max=c;index=2;} + + /* this is the simplified projection onto L*/ + vp0=V0[index]; + vp1=V1[index]; + vp2=V2[index]; + + up0=U0[index]; + up1=U1[index]; + up2=U2[index]; + + /* compute interval for triangle 1 */ + *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2, + dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2); + if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); + + + /* compute interval for triangle 2 */ + compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2, + du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2); + + SORT2(isect1[0],isect1[1],smallest1); + SORT2(isect2[0],isect2[1],smallest2); + + if(isect1[1]isect1[1]) + { + if(smallest1==0) { SET(isectpt2,isectpointA2); } + else { SET(isectpt2,isectpointA1); } + } + else + { + if(smallest2==0) { SET(isectpt2,isectpointB2); } + else { SET(isectpt2,isectpointB1); } + } + } + return 1; +} + + + + + +// Find the intersectiojn point between a coplanar line segement, +// defined by X1 and X2, and a ray defined by X3 and direction N. +// +// This forumla for this calculation is: +// (c x b) . (a x b) +// Q = x1 + a ------------------- +// | a x b | ^2 +// +// where a = x2 - x1 +// b = x4 - x3 +// c = x3 - x1 +// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt +// and x4 is (CoplanarPt - n) +#if 0 +static int +IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, + dVector3 out_pt) +{ + dVector3 a, b, c, x4; + + ADD(x4, x3, n); // x4 = x3 + n + + SUB(a, x2, x1); // a = x2 - x1 + SUB(b, x4, x3); + SUB(c, x3, x1); + + dVector3 tmp1, tmp2; + CROSS(tmp1, c, b); + CROSS(tmp2, a, b); + + dReal num, denom; + num = dDOT(tmp1, tmp2); + denom = LENGTH( tmp2 ); + + dReal s; + s = num /(denom*denom); + + for (int i=0; i<3; i++) + out_pt[i] = x1[i] + a[i]*s; + + // Test if this intersection is "behind" x3, w.r.t. n + SUB(a, x3, out_pt); + if (dDOT(a, n) > 0.0) + return 0; + + // Test if this intersection point is outside the edge limits, + // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside + // else outside + SUB(a, out_pt, x1); + SUB(b, out_pt, x2); + if (dDOT(a,b) < 0.0) + return 1; + else + return 0; +} +#endif //0 + +// FindTriSolidIntersection - Clips the input trinagle TRI with the +// sides of a convex bounding solid, described by PLANES, returning +// the (convex) clipped polygon in CLIPPEDPOLYGON +// +#ifndef ERICF_METHOD +static bool +FindTriSolidIntrsection(const dVector3 Tri[3], + const dVector4 Planes[6], int numSides, + LineContactSet& ClippedPolygon ) +{ + // Set up the LineContactSet structure + for (int k=0; k<3; k++) { + SET(ClippedPolygon.Points[k], Tri[k]); + } + ClippedPolygon.Count = 3; + + // Clip wrt the sides + for ( int i = 0; i < numSides; i++ ) + ClipConvexPolygonAgainstPlane( Planes[i], Planes[i][3], ClippedPolygon ); + + return (ClippedPolygon.Count > 0); +} +#endif + + + +// ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by +// CONTACTS, with a plane (described by N and C). Note: the input +// vertices are assumed to be in counterclockwise order. +// +// This code is taken from The Nebula Device: +// http://nebuladevice.sourceforge.net/cgi-bin/twiki/view/Nebula/WebHome +// and is licensed under the following license: +// http://nebuladevice.sourceforge.net/doc/source/license.txt +// +#ifndef ERICF_METHOD +static void +ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, + LineContactSet& Contacts ) +{ + // test on which side of line are the vertices + int Positive = 0, Negative = 0, PIndex = -1; + int Quantity = Contacts.Count; + + dReal Test[8]; + for ( int i = 0; i < Contacts.Count; i++ ) { + // An epsilon is used here because it is possible for the dot product + // and C to be exactly equal to each other (in theory), but differ + // slightly because of floating point problems. Thus, add a little + // to the test number to push actually equal numbers over the edge + // towards the positive. This should probably be somehow a relative + // tolerance, and I don't think multiplying by the constant is the best + // way to do this. + Test[i] = dDOT(N, Contacts.Points[i]) - C + dFabs(C)*1e-08; + + if (Test[i] >= REAL(0.0)) { + Positive++; + if (PIndex < 0) { + PIndex = i; + } + } + else Negative++; + } + + if (Positive > 0) { + if (Negative > 0) { + // plane transversely intersects polygon + dVector3 CV[8]; + int CQuantity = 0, Cur, Prv; + dReal T; + + if (PIndex > 0) { + // first clip vertex on line + Cur = PIndex; + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // vertices on positive side of line + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + + // last clip vertex on line + if (Cur < Quantity) { + Prv = Cur - 1; + } + else { + Cur = 0; + Prv = Quantity - 1; + } + + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + } + else { + // iPIndex is 0 + // vertices on positive side of line + Cur = 0; + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + + // last clip vertex on line + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // skip vertices on negative side + while (Cur < Quantity && Test[Cur] < REAL(0.0)) { + Cur++; + } + + // first clip vertex on line + if (Cur < Quantity) { + Prv = Cur - 1; + T = Test[Cur] / (Test[Cur] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[Cur][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); + CV[CQuantity][1] = Contacts.Points[Cur][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); + CV[CQuantity][2] = Contacts.Points[Cur][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); + CV[CQuantity][3] = Contacts.Points[Cur][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); + CQuantity++; + + // vertices on positive side of line + while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { + CV[CQuantity][0] = Contacts.Points[Cur][0]; + CV[CQuantity][1] = Contacts.Points[Cur][1]; + CV[CQuantity][2] = Contacts.Points[Cur][2]; + CV[CQuantity][3] = Contacts.Points[Cur][3]; + CQuantity++; + Cur++; + } + } + else { + // iCur = 0 + Prv = Quantity - 1; + T = Test[0] / (Test[0] - Test[Prv]); + CV[CQuantity][0] = Contacts.Points[0][0] + + T * (Contacts.Points[Prv][0] - Contacts.Points[0][0]); + CV[CQuantity][1] = Contacts.Points[0][1] + + T * (Contacts.Points[Prv][1] - Contacts.Points[0][1]); + CV[CQuantity][2] = Contacts.Points[0][2] + + T * (Contacts.Points[Prv][2] - Contacts.Points[0][2]); + CV[CQuantity][3] = Contacts.Points[0][3] + + T * (Contacts.Points[Prv][3] - Contacts.Points[0][3]); + CQuantity++; + } + } + Quantity = CQuantity; + memcpy( Contacts.Points, CV, CQuantity * sizeof(dVector3) ); + } + // else polygon fully on positive side of plane, nothing to do + Contacts.Count = Quantity; + } + else { + Contacts.Count = 0; // This should not happen, but for safety + } + +} +#endif + + +// Determine if a potential collision point is +// +// + +#ifndef ERICF_METHOD +static int +ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point) +{ + // Cast a ray from in_point, along the collison normal. Does it intersect the + // collision face. + dReal t, u, v; + + if (!RayTriangleIntersect(in_point, in_n, v_col[0], v_col[1], v_col[2], + &t, &u, &v)) + return 0; + else + return 1; +} +#endif + + +// RayTriangleIntersect - If an intersection is found, t contains the +// distance along the ray (dir) and u/v contain u/v coordinates into +// the triangle. Returns 0 if no hit is found +// From "Real-Time Rendering," page 305 +// +#ifndef ERICF_METHOD +static int +RayTriangleIntersect(const dVector3 orig, const dVector3 dir, + const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, + dReal *t,dReal *u,dReal *v) + +{ + dReal edge1[3], edge2[3], tvec[3], pvec[3], qvec[3]; + dReal det,inv_det; + + // find vectors for two edges sharing vert0 + SUB(edge1, vert1, vert0); + SUB(edge2, vert2, vert0); + + // begin calculating determinant - also used to calculate U parameter + CROSS(pvec, dir, edge2); + + // if determinant is near zero, ray lies in plane of triangle + det = DOT(edge1, pvec); + + if ((det > -0.001) && (det < 0.001)) + return 0; + inv_det = 1.0 / det; + + // calculate distance from vert0 to ray origin + SUB(tvec, orig, vert0); + + // calculate U parameter and test bounds + *u = DOT(tvec, pvec) * inv_det; + if ((*u < 0.0) || (*u > 1.0)) + return 0; + + // prepare to test V parameter + CROSS(qvec, tvec, edge1); + + // calculate V parameter and test bounds + *v = DOT(dir, qvec) * inv_det; + if ((*v < 0.0) || ((*u + *v) > 1.0)) + return 0; + + // calculate t, ray intersects triangle + *t = DOT(edge2, qvec) * inv_det; + + return 1; +} + +static bool +SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, + dVector3 in_n, dVector3* in_col_v, dReal &out_depth) +{ + dReal dp = 0.0; + dReal contact_elt_length; + + DEPTH(dp, in_CoplanarPt, in_v, in_n); + + if (dp >= 0.0) { + // if the penetration depth (calculated above) is more than + // the contact point's ELT, then we've chosen the wrong face + // and should switch faces + contact_elt_length = fabs(dDOT(in_elt, in_n)); + + if (dp == 0.0) + dp = dMin(DISTANCE_EPSILON, contact_elt_length); + + if ((contact_elt_length < SMALL_ELT) && (dp < EXPANDED_ELT_THRESH)) + dp = contact_elt_length; + + if ( (dp > 0.0) && (dp <= contact_elt_length)) { + // Add a contact + + if ( ExamineContactPoint(in_col_v, in_n, in_v) ) { + out_depth = dp; + return true; + } + } + } + + return false; +} +#endif + + + +// Generate a "unique" contact. A unique contact has a unique +// position or normal. If the potential contact has the same +// position and normal as an existing contact, but a larger +// penetration depth, this new depth is used instead +// +static void +GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, + dxTriMesh* in_TriMesh1, dxTriMesh* in_TriMesh2, + const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, + int& OutTriCount) +{ + if (in_Depth < 0.0) + return; + + if (OutTriCount == (in_Flags & 0x0ffff)) + return; // contacts are full! + + + dContactGeom* Contact; + dVector3 diff; + bool duplicate = false; + + for (int i=0; ipos); + if (dDOT(diff, diff) < ODE_EPSILON) + { + // same normal? + if (fabs(dDOT(in_Normal, Contact->normal)) > ((dReal)1) - ODE_EPSILON ) + { + if (in_Depth > Contact->depth) { + Contact->depth = in_Depth; + SMULT( Contact->normal, in_Normal, -1.0); + Contact->normal[3] = 0.0; + } + duplicate = true; + } + } + } + + + if (!duplicate) + { + // Add a new contact + Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); + + SET( Contact->pos, in_ContactPos ); + Contact->pos[3] = 0.0; + + SMULT( Contact->normal, in_Normal, -1.0); + Contact->normal[3] = 0.0; + + Contact->depth = in_Depth; + + Contact->g1 = in_TriMesh1; + Contact->g2 = in_TriMesh2; + + OutTriCount++; + } + + +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_util.cpp b/src/external/open_dynamics_engine-ef/ode/ode_collision_util.cpp new file mode 100644 index 00000000..17a22c27 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_util.cpp @@ -0,0 +1,596 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +some useful collision utility stuff. this includes some API utility +functions that are defined in the public header files. + +*/ + +#include "ode/ode_common.h" +#include "ode/ode_collision.h" +#include "ode/ode_math.h" +#include "ode/ode_collision_util.h" + +//**************************************************************************** + +int dCollideSpheres (dVector3 p1, dReal r1, + dVector3 p2, dReal r2, dContactGeom *c) +{ + // printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n", + // d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2); + + dReal d = dDISTANCE (p1,p2); + if (d > (r1 + r2)) return 0; + if (d <= 0) { + c->pos[0] = p1[0]; + c->pos[1] = p1[1]; + c->pos[2] = p1[2]; + c->normal[0] = 1; + c->normal[1] = 0; + c->normal[2] = 0; + c->depth = r1 + r2; + } + else { + dReal d1 = dRecip (d); + c->normal[0] = (p1[0]-p2[0])*d1; + c->normal[1] = (p1[1]-p2[1])*d1; + c->normal[2] = (p1[2]-p2[2])*d1; + dReal k = REAL(0.5) * (r2 - r1 - d); + c->pos[0] = p1[0] + c->normal[0]*k; + c->pos[1] = p1[1] + c->normal[1]*k; + c->pos[2] = p1[2] + c->normal[2]*k; + c->depth = r1 + r2 - d; + } + return 1; +} + + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta) +{ + dVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + dReal uaub = dDOT(ua,ub); + dReal q1 = dDOT(ua,p); + dReal q2 = -dDOT(ub,p); + dReal d = 1-uaub*uaub; + if (d <= REAL(0.0001)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = dRecip(d); + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + +// given two line segments A and B with endpoints a1-a2 and b1-b2, return the +// points on A and B that are closest to each other (in cp1 and cp2). +// in the case of parallel lines where there are multiple solutions, a +// solution involving the endpoint of at least one line will be returned. +// this will work correctly for zero length lines, e.g. if a1==a2 and/or +// b1==b2. +// +// the algorithm works by applying the voronoi clipping rule to the features +// of the line segments. the three features of each line segment are the two +// endpoints and the line between them. the voronoi clipping rule states that, +// for feature X on line A and feature Y on line B, the closest points PA and +// PB between X and Y are globally the closest points if PA is in V(Y) and +// PB is in V(X), where V(X) is the voronoi region of X. + +void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2, + const dVector3 b1, const dVector3 b2, + dVector3 cp1, dVector3 cp2) +{ + dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n; + dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det; + +#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2]; +#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; + + // check vertex-vertex features + + SET3 (a1a2,a2,-,a1); + SET3 (b1b2,b2,-,b1); + SET3 (a1b1,b1,-,a1); + da1 = dDOT(a1a2,a1b1); + db1 = dDOT(b1b2,a1b1); + if (da1 <= 0 && db1 >= 0) { + SET2 (cp1,a1); + SET2 (cp2,b1); + return; + } + + SET3 (a1b2,b2,-,a1); + da2 = dDOT(a1a2,a1b2); + db2 = dDOT(b1b2,a1b2); + if (da2 <= 0 && db2 <= 0) { + SET2 (cp1,a1); + SET2 (cp2,b2); + return; + } + + SET3 (a2b1,b1,-,a2); + da3 = dDOT(a1a2,a2b1); + db3 = dDOT(b1b2,a2b1); + if (da3 >= 0 && db3 >= 0) { + SET2 (cp1,a2); + SET2 (cp2,b1); + return; + } + + SET3 (a2b2,b2,-,a2); + da4 = dDOT(a1a2,a2b2); + db4 = dDOT(b1b2,a2b2); + if (da4 >= 0 && db4 <= 0) { + SET2 (cp1,a2); + SET2 (cp2,b2); + return; + } + + // check edge-vertex features. + // if one or both of the lines has zero length, we will never get to here, + // so we do not have to worry about the following divisions by zero. + + la = dDOT(a1a2,a1a2); + if (da1 >= 0 && da3 <= 0) { + k = da1 / la; + SET3 (n,a1b1,-,k*a1a2); + if (dDOT(b1b2,n) >= 0) { + SET3 (cp1,a1,+,k*a1a2); + SET2 (cp2,b1); + return; + } + } + + if (da2 >= 0 && da4 <= 0) { + k = da2 / la; + SET3 (n,a1b2,-,k*a1a2); + if (dDOT(b1b2,n) <= 0) { + SET3 (cp1,a1,+,k*a1a2); + SET2 (cp2,b2); + return; + } + } + + lb = dDOT(b1b2,b1b2); + if (db1 <= 0 && db2 >= 0) { + k = -db1 / lb; + SET3 (n,-a1b1,-,k*b1b2); + if (dDOT(a1a2,n) >= 0) { + SET2 (cp1,a1); + SET3 (cp2,b1,+,k*b1b2); + return; + } + } + + if (db3 <= 0 && db4 >= 0) { + k = -db3 / lb; + SET3 (n,-a2b1,-,k*b1b2); + if (dDOT(a1a2,n) <= 0) { + SET2 (cp1,a2); + SET3 (cp2,b1,+,k*b1b2); + return; + } + } + + // it must be edge-edge + + k = dDOT(a1a2,b1b2); + det = la*lb - k*k; + if (det <= 0) { + // this should never happen, but just in case... + SET2(cp1,a1); + SET2(cp2,b1); + return; + } + det = dRecip (det); + dReal alpha = (lb*da1 - k*db1) * det; + dReal beta = ( k*da1 - la*db1) * det; + SET3 (cp1,a1,+,alpha*a1a2); + SET3 (cp2,b1,+,beta*b1b2); + +# undef SET2 +# undef SET3 +} + + +// a simple root finding algorithm is used to find the value of 't' that +// satisfies: +// d|D(t)|^2/dt = 0 +// where: +// |D(t)| = |p(t)-b(t)| +// where p(t) is a point on the line parameterized by t: +// p(t) = p1 + t*(p2-p1) +// and b(t) is that same point clipped to the boundary of the box. in box- +// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components +// each of which looks like this: +// +// t_lo / +// ______/ -->t +// / t_hi +// / +// +// t_lo and t_hi are the t values where the line passes through the planes +// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt +// in a piecewise fashion from t=0 to t=1, stopping at the point where +// d|D(t)|^2/dt crosses from negative to positive. + +void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, + const dVector3 c, const dMatrix3 R, + const dVector3 side, + dVector3 lret, dVector3 bret) +{ + int i; + + // compute the start and delta of the line p1-p2 relative to the box. + // we will do all subsequent computations in this box-relative coordinate + // system. we have to do a translation and rotation for each point. + dVector3 tmp,s,v; + tmp[0] = p1[0] - c[0]; + tmp[1] = p1[1] - c[1]; + tmp[2] = p1[2] - c[2]; + dMULTIPLY1_331 (s,R,tmp); + tmp[0] = p2[0] - p1[0]; + tmp[1] = p2[1] - p1[1]; + tmp[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (v,R,tmp); + + // mirror the line so that v has all components >= 0 + dVector3 sign; + for (i=0; i<3; i++) { + if (v[i] < 0) { + s[i] = -s[i]; + v[i] = -v[i]; + sign[i] = -1; + } + else sign[i] = 1; + } + + // compute v^2 + dVector3 v2; + v2[0] = v[0]*v[0]; + v2[1] = v[1]*v[1]; + v2[2] = v[2]*v[2]; + + // compute the half-sides of the box + dReal h[3]; + h[0] = REAL(0.5) * side[0]; + h[1] = REAL(0.5) * side[1]; + h[2] = REAL(0.5) * side[2]; + + // region is -1,0,+1 depending on which side of the box planes each + // coordinate is on. tanchor is the next t value at which there is a + // transition, or the last one if there are no more. + int region[3]; + dReal tanchor[3]; + + // find the region and tanchor values for p1 + for (i=0; i<3; i++) { + if (v[i] > 0) { + if (s[i] < -h[i]) { + region[i] = -1; + tanchor[i] = (-h[i]-s[i])/v[i]; + } + else { + region[i] = (s[i] > h[i]); + tanchor[i] = (h[i]-s[i])/v[i]; + } + } + else { + region[i] = 0; + tanchor[i] = 2; // this will never be a valid tanchor + } + } + + // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point + dReal t=0; + dReal dd2dt = 0; + for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i]; + if (dd2dt >= 0) goto got_answer; + + do { + // find the point on the line that is at the next clip plane boundary + dReal next_t = 1; + for (i=0; i<3; i++) { + if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t) + next_t = tanchor[i]; + } + + // compute d|d|^2/dt for the next t + dReal next_dd2dt = 0; + for (i=0; i<3; i++) { + next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]); + } + + // if the sign of d|d|^2/dt has changed, solution = the crossover point + if (next_dd2dt >= 0) { + dReal m = (next_dd2dt-dd2dt)/(next_t - t); + t -= dd2dt/m; + goto got_answer; + } + + // advance to the next anchor point / region + for (i=0; i<3; i++) { + if (tanchor[i] == next_t) { + tanchor[i] = (h[i]-s[i])/v[i]; + region[i]++; + } + } + t = next_t; + dd2dt = next_dd2dt; + } + while (t < 1); + t = 1; + + got_answer: + + // compute closest point on the line + for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1 + + // compute closest point on the box + for (i=0; i<3; i++) { + tmp[i] = sign[i] * (s[i] + t*v[i]); + if (tmp[i] < -h[i]) tmp[i] = -h[i]; + else if (tmp[i] > h[i]) tmp[i] = h[i]; + } + dMULTIPLY0_331 (s,R,tmp); + for (i=0; i<3; i++) bret[i] = s[i] + c[i]; +} + + +// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect +// or 0 if not. + +int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1, + const dVector3 side1, const dVector3 p2, + const dMatrix3 R2, const dVector3 side2) +{ + // two boxes are disjoint if (and only if) there is a separating axis + // perpendicular to a face from one box or perpendicular to an edge from + // either box. the following tests are derived from: + // "OBB Tree: A Hierarchical Structure for Rapid Interference Detection", + // S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996. + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2. + // Qij is abs(Rij) + dVector3 p,pp; + dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5); + B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5); + + // for the following tests, excluding computation of Rij, in the worst case, + // 15 compares, 60 adds, 81 multiplies, and 24 absolutes. + // notation: R1=[u1 u2 u3], R2=[v1 v2 v3] + + // separating axis = u1,u2,u3 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); + if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0; + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); + if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0; + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); + if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0; + + // separating axis = v1,v2,v3 + if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0; + if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0; + if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0; + + // separating axis = u1 x (v1,v2,v3) + if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0; + if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0; + if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0; + + // separating axis = u2 x (v1,v2,v3) + if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0; + if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0; + if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0; + + // separating axis = u3 x (v1,v2,v3) + if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0; + if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0; + if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0; + + return 1; +} + +//**************************************************************************** +// other utility functions + +void dInfiniteAABB (dxGeom *geom, dReal aabb[6]) +{ + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; +} + + +//////////// ADDED CYLINDER CODE +// clip polygon with plane and generate new polygon points + +//**************************************************************************** +// Helpers for Croteam's collider - by Nguyen Binh + +int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) +{ + // calculate distance of edge points to plane + dReal fDistance0 = dPointPlaneDistance( vEpnt0 ,plPlane ); + dReal fDistance1 = dPointPlaneDistance( vEpnt1 ,plPlane ); + + // if both points are behind the plane + if ( fDistance0 < 0 && fDistance1 < 0 ) + { + // do nothing + return 0; + // if both points in front of the plane + } + else if ( fDistance0 > 0 && fDistance1 > 0 ) + { + // accept them + return 1; + // if we have edge/plane intersection + } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); + + // clamp correct edge to intersection point + if ( fDistance0 < 0 ) + { + dVector3Copy(vIntersectionPoint,vEpnt0); + } else + { + dVector3Copy(vIntersectionPoint,vEpnt1); + } + return 1; + } + return 1; +} + +void dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1= 0 ) { + // emit point + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - + (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - + (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - + (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + +} + +void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ,dReal fRadius) +{ + // start with no output points + ctOut = 0; + + int i0 = ctIn-1; + + // for each edge in input polygon + for (int i1=0; i1= 0 ) + { + // emit point + if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius) + { + avArrayOut[ctOut][0] = avArrayIn[i0][0]; + avArrayOut[ctOut][1] = avArrayIn[i0][1]; + avArrayOut[ctOut][2] = avArrayIn[i0][2]; + ctOut++; + } + } + + // if points are on different sides + if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) + { + + // find intersection point of edge and plane + dVector3 vIntersectionPoint; + vIntersectionPoint[0]= avArrayIn[i0][0] - + (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[1]= avArrayIn[i0][1] - + (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1); + vIntersectionPoint[2]= avArrayIn[i0][2] - + (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1); + + // emit intersection point + if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius) + { + avArrayOut[ctOut][0] = vIntersectionPoint[0]; + avArrayOut[ctOut][1] = vIntersectionPoint[1]; + avArrayOut[ctOut][2] = vIntersectionPoint[2]; + ctOut++; + } + } + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_collision_util.h b/src/external/open_dynamics_engine-ef/ode/ode_collision_util.h new file mode 100644 index 00000000..ee8d7895 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_collision_util.h @@ -0,0 +1,344 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +some useful collision utility stuff. + +*/ + +#ifndef _ODE_COLLISION_UTIL_H_ +#define _ODE_COLLISION_UTIL_H_ + +#include "ode/ode_common.h" +#include "ode/ode_contact.h" +#include "ode/ode_math.h" +#include "ode/ode_rotation.h" + +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + + +// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and +// return 1, else return 0. + +int dCollideSpheres (dVector3 p1, dReal r1, + dVector3 p2, dReal r2, dContactGeom *c); + + +// given two lines +// qa = pa + alpha* ua +// qb = pb + beta * ub +// where pa,pb are two points, ua,ub are two unit length vectors, and alpha, +// beta go from [-inf,inf], return alpha and beta such that qa and qb are +// as close as possible + +void dLineClosestApproach (const dVector3 pa, const dVector3 ua, + const dVector3 pb, const dVector3 ub, + dReal *alpha, dReal *beta); + + +// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length +// vector 'side'), compute the points of closest approach between the box +// and the line. return these points in 'lret' (the point on the line) and +// 'bret' (the point on the box). if the line actually penetrates the box +// then the solution is not unique, but only one solution will be returned. +// in this case the solution points will coincide. + +void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, + const dVector3 c, const dMatrix3 R, + const dVector3 side, + dVector3 lret, dVector3 bret); + + +/// CYLINDER ADDITION +// 20 Apr 2004 +// Start code by Nguyen Binh +int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); +// clip polygon with plane and generate new polygon points +void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ); + +void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, + dVector3 avArrayOut[], int &ctOut, + const dVector4 &plPlane ,dReal fRadius); + + +// Some vector math +inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c) +{ + c[0] = a[0] - b[0]; + c[1] = a[1] - b[1]; + c[2] = a[2] - b[2]; +} + +// Some vector math +inline void dVector3Scale(dVector3& a,dReal nScale) +{ + a[0] *= nScale ; + a[1] *= nScale ; + a[2] *= nScale ; +} + +inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c) +{ + c[0] = a[0] + b[0]; + c[1] = a[1] + b[1]; + c[2] = a[2] + b[2]; +} + +inline void dVector3Copy(const dVector3& a,dVector3& c) +{ + c[0] = a[0]; + c[1] = a[1]; + c[2] = a[2]; +} + +inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c) +{ + dCROSS(c,=,a,b); +} + +inline dReal dVector3Length(const dVector3& a) +{ + return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); +} + +inline dReal dVector3Dot(const dVector3& a,const dVector3& b) +{ + return dDOT(a,b); +} + +inline void dVector3Inv(dVector3& a) +{ + a[0] = -a[0]; + a[1] = -a[1]; + a[2] = -a[2]; +} + +inline dReal dVector3Length2(const dVector3& a) +{ + return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); +} + +inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v) +{ + v[0] = m[col + 0]; + v[1] = m[col + 4]; + v[2] = m[col + 8]; +} + +inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) +{ + r[0] = v[1] * m[2*4 + col] - v[2] * m[1*4 + col]; + r[1] = v[2] * m[0*4 + col] - v[0] * m[2*4 + col]; + r[2] = v[0] * m[1*4 + col] - v[1] * m[0*4 + col]; +} + +inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) +{ + r[0] = v[2] * m[1*4 + col] - v[1] * m[2*4 + col]; + r[1] = v[0] * m[2*4 + col] - v[2] * m[0*4 + col]; + r[2] = v[1] * m[0*4 + col] - v[0] * m[1*4 + col]; +} + +inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r) +{ + dMULTIPLY0_331(r,m,v); +} + +inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane) +{ + return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]); +} + +inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane) +{ + plane[0] = normal[0]; + plane[1] = normal[1]; + plane[2] = normal[2]; + plane[3] = distance; +} + +inline void dMatrix3Copy(const dReal* source,dMatrix3& dest) +{ + dest[0] = source[0]; + dest[1] = source[1]; + dest[2] = source[2]; + + dest[4] = source[4]; + dest[5] = source[5]; + dest[6] = source[6]; + + dest[8] = source[8]; + dest[9] = source[9]; + dest[10]= source[10]; +} + +inline dReal dMatrix3Det( const dMatrix3& mat ) +{ + dReal det; + + det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) + - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) + + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); + + return( det ); +} + + +inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst ) +{ + dReal det = dMatrix3Det( ma ); + + if ( dFabs( det ) < REAL(0.0005) ) + { + dRSetIdentity( dst ); + return; + } + + dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det; + dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det; + dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det; + + dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det; + dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det; + dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det; + + dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det; + dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det; + dst[10] = ma[0]*ma[5] - ma[1]*ma[4] / det; +} + +inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) +{ + + // Nguyen Binh : this code seem to be the fastest. + dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3]; + dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1]; + dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2]; + dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3]; + + dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1; + dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2; + dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0; + + /* + // nVidia SDK implementation + dVector3 uv, uuv; + dVector3 qvec; + qvec[0] = quat[1]; + qvec[1] = quat[2]; + qvec[2] = quat[3]; + + dVector3Cross(qvec,source,uv); + dVector3Cross(qvec,uv,uuv); + + dVector3Scale(uv,REAL(2.0)*quat[0]); + dVector3Scale(uuv,REAL(2.0)); + + dest[0] = source[0] + uv[0] + uuv[0]; + dest[1] = source[1] + uv[1] + uuv[1]; + dest[2] = source[2] + uv[2] + uuv[2]; + */ +} + +inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) +{ + + dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; + + if (norm > REAL(0.0)) + { + dQuaternion invQuat; + invQuat[0] = quat[0] / norm; + invQuat[1] = -quat[1] / norm; + invQuat[2] = -quat[2] / norm; + invQuat[3] = -quat[3] / norm; + + dQuatTransform(invQuat,source,dest); + + } + else + { + // Singular -> return identity + dVector3Copy(source,dest); + } +} + +inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ) +{ + rY = asin(mRot[0 * 4 + 2]); + if (rY < M_PI /2) + { + if (rY > -M_PI /2) + { + rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]); + rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]); + } + else + { + // not unique + rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]); + rZ = REAL(0.0); + } + } + else + { + // not unique + rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]); + rZ = REAL(0.0); + } +} + +inline void dQuatInv(const dQuaternion& source, dQuaternion& dest) +{ + dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3]; + + if (norm > 0.0f) + { + dest[0] = source[0] / norm; + dest[1] = -source[1] / norm; + dest[2] = -source[2] / norm; + dest[3] = -source[3] / norm; + } + else + { + // Singular -> return identity + dest[0] = REAL(1.0); + dest[1] = REAL(0.0); + dest[2] = REAL(0.0); + dest[3] = REAL(0.0); + } +} + +// Fetches a contact +inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ + dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff)); + return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); +} + + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_common.h b/src/external/open_dynamics_engine-ef/ode/ode_common.h new file mode 100644 index 00000000..ef33df2f --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_common.h @@ -0,0 +1,357 @@ +/************************************************************************* + * * + * 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_COMMON_H_ +#define _ODE_COMMON_H_ + +#include "ode/ode_config.h" +#include "ode/ode_error.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +/* configuration stuff */ + +/* the efficient alignment. most platforms align data structures to some + * number of bytes, but this is not always the most efficient alignment. + * for example, many x86 compilers align to 4 bytes, but on a pentium it + * is important to align doubles to 8 byte boundaries (for speed), and + * the 4 floats in a SIMD register to 16 byte boundaries. many other + * platforms have similar behavior. setting a larger alignment can waste + * a (very) small amount of memory. NOTE: this number must be a power of + * two. this is set to 16 by default. + */ +#define EFFICIENT_ALIGNMENT 16 + + +/* constants */ + +/* pi and 1/sqrt(2) are defined here if necessary because they don't get + * defined in on some platforms (like MS-Windows) + */ + +#ifndef M_PI +#define M_PI REAL(3.1415926535897932384626433832795029) +#endif +#ifndef M_SQRT1_2 +#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490) +#endif + + +/* debugging: + * IASSERT is an internal assertion, i.e. a consistency check. if it fails + * we want to know where. + * UASSERT is a user assertion, i.e. if it fails a nice error message + * should be printed for the user. + * AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)" + * is printed. + * DEBUGMSG just prints out a message + */ + +#ifndef dNODEBUG +#ifdef __GNUC__ +#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__) +#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__) +#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__) +#else +#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__); +#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ + msg " (%s:%d)", __FILE__,__LINE__); +#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ + msg " (%s:%d)", __FILE__,__LINE__); +#endif +#else +#define dIASSERT(a) ; +#define dUASSERT(a,msg) ; +#define dDEBUGMSG(msg) ; +#endif +#define dAASSERT(a) dUASSERT(a,"Bad argument(s)") + +/* floating point data type, vector, matrix and quaternion types */ + +#if defined(dSINGLE) +typedef float dReal; +#elif defined(dDOUBLE) +typedef double dReal; +#else +#error You must #define dSINGLE or dDOUBLE +#endif + +//epsilon +#if defined(dSINGLE) + #define ODE_EPSILON FLT_EPSILON +#else + #define ODE_EPSILON DBL_EPSILON +#endif + + +/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified + * (used to compute matrix leading dimensions) + */ +#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a)) + +/* these types are mainly just used in headers */ +typedef dReal dVector3[4]; +typedef dReal dVector4[4]; +typedef dReal dMatrix3[4*3]; +typedef dReal dMatrix4[4*4]; +typedef dReal dMatrix6[8*6]; +typedef dReal dQuaternion[4]; + + +/* precision dependent scalar math functions */ + +#if defined(dSINGLE) + +#define REAL(x) (x ## f) /* form a constant */ +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ +#define dSqrt(x) ((float)sqrtf(float(x))) /* square root */ +#define dRecipSqrt(x) ((float)(1.0f/sqrtf(float(x)))) /* reciprocal square root */ +#define dSin(x) ((float)sinf(float(x))) /* sine */ +#define dCos(x) ((float)cosf(float(x))) /* cosine */ +#define dFabs(x) ((float)fabsf(float(x))) /* absolute value */ +#define dAtan2(y,x) ((float)atan2f(float(y),float(x))) /* arc tangent with 2 args */ +#define dFMod(a,b) ((float)fmodf(float(a),float(b))) /* modulo */ +// #ifdef WIN32 // ericf added +#if 0 +#define dCopySign(a,b) ((float)copysign(float(a),float(b))) +#else +#define dCopySign(a,b) ((float)copysignf(float(a),float(b))) +#endif + +#elif defined(dDOUBLE) + +#define REAL(x) (x) +#define dRecip(x) (1.0/(x)) +#define dSqrt(x) sqrt(x) +#define dRecipSqrt(x) (1.0/sqrt(x)) +#define dSin(x) sin(x) +#define dCos(x) cos(x) +#define dFabs(x) fabs(x) +#define dAtan2(y,x) atan2((y),(x)) +#define dFMod(a,b) (fmod((a),(b))) +#define dCopySign(a,b) (copysign((a),(b))) + +#else +#error You must #define dSINGLE or dDOUBLE +#endif + + +/* utility */ + + +/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */ + +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) + + +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + + +// Use the error-checking memory allocation system. Becuase this system uses heap +// (malloc) instead of stack (alloca), it is slower. However, it allows you to +// simulate larger scenes, as well as handle out-of-memory errors in a somewhat +// graceful manner + +// #define dUSE_MALLOC_FOR_ALLOCA + +#ifdef dUSE_MALLOC_FOR_ALLOCA +enum { + d_MEMORY_OK = 0, /* no memory errors */ + d_MEMORY_OUT_OF_MEMORY /* malloc failed due to out of memory error */ +}; + +#endif + + + +/* internal object types (all prefixed with `dx') */ + +struct dxWorld; /* dynamics world */ +struct dxSpace; /* collision space */ +struct dxBody; /* rigid body (dynamics object) */ +struct dxGeom; /* geometry (collision object) */ +struct dxJoint; +struct dxJointNode; +struct dxJointGroup; + +typedef struct dxWorld *dWorldID; +typedef struct dxSpace *dSpaceID; +typedef struct dxBody *dBodyID; +typedef struct dxGeom *dGeomID; +typedef struct dxJoint *dJointID; +typedef struct dxJointGroup *dJointGroupID; + + +/* error numbers */ + +enum { + d_ERR_UNKNOWN = 0, /* unknown error */ + d_ERR_IASSERT, /* internal assertion failed */ + d_ERR_UASSERT, /* user assertion failed */ + d_ERR_LCP /* user assertion failed */ +}; + + +/* joint type numbers */ + +enum { + dJointTypeNone = 0, /* or "unknown" */ + dJointTypeBall, + dJointTypeHinge, + dJointTypeSlider, + dJointTypeContact, + dJointTypeUniversal, + dJointTypeHinge2, + dJointTypeFixed, + dJointTypeNull, + dJointTypeAMotor +}; + + +/* an alternative way of setting joint parameters, using joint parameter + * structures and member constants. we don't actually do this yet. + */ + +/* +typedef struct dLimot { + int mode; + dReal lostop, histop; + dReal vel, fmax; + dReal fudge_factor; + dReal bounce, soft; + dReal suspension_erp, suspension_cfm; +} dLimot; + +enum { + dLimotLoStop = 0x0001, + dLimotHiStop = 0x0002, + dLimotVel = 0x0004, + dLimotFMax = 0x0008, + dLimotFudgeFactor = 0x0010, + dLimotBounce = 0x0020, + dLimotSoft = 0x0040 +}; +*/ + + +/* standard joint parameter names. why are these here? - because we don't want + * to include all the joint function definitions in joint.cpp. hmmmm. + * MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument, + * which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and + * paste between these two. + */ + +#define D_ALL_PARAM_NAMES(start) \ + /* parameters for limits and motors */ \ + dParamLoStop = start, \ + dParamHiStop, \ + dParamVel, \ + dParamFMax, \ + dParamFudgeFactor, \ + dParamBounce, \ + dParamCFM, \ + dParamStopERP, \ + dParamStopCFM, \ + /* parameters for suspension */ \ + dParamSuspensionERP, \ + dParamSuspensionCFM, \ + dParamStiffness, \ + dParamDamping, \ + dParamActive, \ + dParamLinearStiffness, \ + dParamLinearDamping, \ + dParamAngularStiffness, \ + dParamAngularDamping, + +#define D_ALL_PARAM_NAMES_X(start,x) \ + /* parameters for limits and motors */ \ + dParamLoStop ## x = start, \ + dParamHiStop ## x, \ + dParamVel ## x, \ + dParamFMax ## x, \ + dParamFudgeFactor ## x, \ + dParamBounce ## x, \ + dParamCFM ## x, \ + dParamStopERP ## x, \ + dParamStopCFM ## x, \ + /* parameters for suspension */ \ + dParamSuspensionERP ## x, \ + dParamSuspensionCFM ## x, + +enum { + D_ALL_PARAM_NAMES(0) + D_ALL_PARAM_NAMES_X(0x100,2) + D_ALL_PARAM_NAMES_X(0x200,3) + + /* add a multiple of this constant to the basic parameter numbers to get + * the parameters for the second, third etc axes. + */ + dParamGroup=0x100 +}; + + +/* angular motor mode numbers */ + +enum{ + dAMotorUser = 0, + dAMotorEuler = 1 +}; + + +/* joint force feedback information */ + +typedef struct dJointFeedback { + dVector3 f1; /* force applied to body 1 */ + dVector3 t1; /* torque applied to body 1 */ + dVector3 f2; /* force applied to body 2 */ + dVector3 t2; /* torque applied to body 2 */ +} dJointFeedback; + + +/* private functions that must be implemented by the collision library: + * (1) indicate that a geom has moved, (2) get the next geom in a body list. + * these functions are called whenever the position of geoms connected to a + * body have changed, e.g. with dBodySetPosition(), dBodySetRotation(), or + * when the ODE step function updates the body state. + */ + +void dGeomMoved (dGeomID); +dGeomID dGeomGetBodyNext (dGeomID); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_compatibility.h b/src/external/open_dynamics_engine-ef/ode/ode_compatibility.h new file mode 100644 index 00000000..b3709866 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_compatibility.h @@ -0,0 +1,40 @@ +/************************************************************************* + * * + * 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_COMPATIBILITY_H_ +#define _ODE_COMPATIBILITY_H_ + +/* + * ODE's backward compatibility system ensures that as ODE's API + * evolves, user code will not break. + */ + +/* + * These new rotation function names are more consistent with the + * rest of the API. + */ +#define dQtoR(q,R) dRfromQ((R),(q)) +#define dRtoQ(R,q) dQfromR((q),(R)) +#define dWtoDQ(w,q,dq) dDQfromW((dq),(w),(q)) + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_config.h b/src/external/open_dynamics_engine-ef/ode/ode_config.h new file mode 100644 index 00000000..adce621d --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_config.h @@ -0,0 +1,77 @@ +/* per-machine configuration. this file is automatically generated. */ + +#ifndef _ODE_CONFIG_H_ +#define _ODE_CONFIG_H_ + +#if HAVE_CONFIG_H +# include +#endif + +/* standard system headers */ +#include +#include +#include +#include +#include +#include + +#if BA_OSTYPE_WINDOWS +//#include +#else +#include +#endif + +//#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* is this a pentium on a gcc-based platform? */ +// moved to makefile +//#define PENTIUM 1 + +/* #if BA_PROCTYPE_X86 */ +// moved to makefile +/* # define PENTIUM 1 */ +/* #endif */ + +/* #if BA_PROCTYPE_X86_64 */ +/* # define PENTIUM 1 */ +/* # define SYS64bits 1 */ +/* #endif */ + +/* #if !BA_DEBUG_BUILD */ +/* #endif */ + +/* is this a 64bit system on a gcc-based platform? */ +// #define SYS64bits 1 + +/* integer types (we assume int >= 32 bits) */ +typedef int8_t int8; +typedef uint8_t uint8; +typedef int16_t int16; +typedef uint16_t uint16; +typedef int32_t int32; +typedef uint32_t uint32; + +/* an integer type that we can safely cast a pointer to and + * from without loss of bits. + */ +typedef uintptr_t intP; + +/* select the base floating point type */ +//#define dDOUBLE 1 +#define dSINGLE 1 + +/* the floating point infinity */ +//#define dInfinity DBL_MAX +#define dInfinity FLT_MAX + +/* available functions */ + +#ifdef __cplusplus +} +#endif +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_contact.h b/src/external/open_dynamics_engine-ef/ode/ode_contact.h new file mode 100644 index 00000000..3561a41b --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_contact.h @@ -0,0 +1,91 @@ +/************************************************************************* + * * + * 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_CONTACT_H_ +#define _ODE_CONTACT_H_ + +#include "ode/ode_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +enum { + dContactMu2 = 0x001, + dContactFDir1 = 0x002, + dContactBounce = 0x004, + dContactSoftERP = 0x008, + dContactSoftCFM = 0x010, + dContactMotion1 = 0x020, + dContactMotion2 = 0x040, + dContactSlip1 = 0x080, + dContactSlip2 = 0x100, + dContactMotionN = 0x200, + + dContactApprox0 = 0x0000, + dContactApprox1_1 = 0x1000, + dContactApprox1_2 = 0x2000, + dContactApprox1 = 0x3000 +}; + + +typedef struct dSurfaceParameters { + /* must always be defined */ + int mode; + dReal mu; + + /* only defined if the corresponding flag is set in mode */ + dReal mu2; + dReal bounce; + dReal bounce_vel; + dReal soft_erp; + dReal soft_cfm; + dReal motion1,motion2,motionN; + dReal slip1,slip2; +} dSurfaceParameters; + + +/* contact info set by collision functions */ + +typedef struct dContactGeom { + dVector3 pos; + dVector3 normal; + dReal depth; + dGeomID g1,g2; +} dContactGeom; + + +/* contact info used by contact joint */ + +typedef struct dContact { + dSurfaceParameters surface; + dContactGeom geom; + dVector3 fdir1; +} dContact; + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_drawstuff.h b/src/external/open_dynamics_engine-ef/ode/ode_drawstuff.h new file mode 100644 index 00000000..2d28263d --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_drawstuff.h @@ -0,0 +1,164 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +/* + +Draw Stuff +---------- + +this is a library for rendering simple 3D objects in a virtual environment. + +NOTES +----- + +in the virtual world, the z axis is "up" and z=0 is the floor. + +the user is able to click+drag in the main window to move the camera: + * left button - pan and tilt. + * right button - forward and sideways. + * left + right button (or middle button) - sideways and up. + +*/ + + +#ifndef __DRAWSTUFF_H__ +#define __DRAWSTUFF_H__ + +#ifdef __cplusplus +extern "C" { +#endif + + +#include "ode/ode_drawstuff_version.h" + + +/* texture numbers */ +#define DS_NONE 0 /* uses the current color instead of a texture */ +#define DS_WOOD 1 + + +typedef struct dsFunctions { + int version; /* put DS_VERSION here */ + /* version 1 data */ + void (*start)(); /* called before sim loop starts */ + void (*step) (int pause); /* called before every frame */ + void (*command) (int cmd); /* called if a command key is pressed */ + void (*stop)(); /* called after sim loop exits */ + /* version 2 data */ + char *path_to_textures; /* if nonzero, path to texture files */ +} dsFunctions; + + +/* the main() function should fill in the dsFunctions structure then + * call this. + */ +void dsSimulationLoop (int argc, char **argv, + int window_width, int window_height, + struct dsFunctions *fn); + +/* these functions display an error message then exit. they take arguments + * in the same way as printf(), except you do not have to add a terminating + * '\n'. Debug() tries to dump core or start the debugger. + */ +void dsError (char *msg, ...); +void dsDebug (char *msg, ...); + +/* dsPrint() prints out a message. it takes arguments in the same way as + * printf() (i.e. you must add a '\n' at the end of every line). + */ +void dsPrint (char *msg, ...); + +/* set and get the camera position. xyz is the cameria position (x,y,z). + * hpr contains heading, pitch and roll numbers in degrees. heading=0 + * points along the x axis, pitch=0 is looking towards the horizon, and + * roll 0 is "unrotated". + */ +void dsSetViewpoint (float xyz[3], float hpr[3]); +void dsGetViewpoint (float xyz[3], float hpr[3]); + +/* stop the simulation loop. calling this from within dsSimulationLoop() + * will cause it to exit and return to the caller. it is the same as if the + * user used the exit command. using this outside the loop will have no + * effect. + */ +void dsStop(); + +/* change the way objects are drawn. these changes will apply to all further + * dsDrawXXX() functions. the texture number must be a DS_xxx texture + * constant. the red, green, and blue number are between 0 and 1. + * alpha is between 0 and 1 - if alpha is not specified it's assubed to be 1. + * the current texture is colored according to the current color. + * at the start of each frame, the texture is reset to none and the color is + * reset to white. + */ +void dsSetTexture (int texture_number); +void dsSetColor (float red, float green, float blue); +void dsSetColorAlpha (float red, float green, float blue, float alpha); + +/* draw objects. + * - pos[] is the x,y,z of the center of the object. + * - R[] is a 3x3 rotation matrix for the object, stored by row like this: + * [ R11 R12 R13 0 ] + * [ R21 R22 R23 0 ] + * [ R31 R32 R33 0 ] + * - sides[] is an array of x,y,z side lengths. + * - all cylinders are aligned along the z axis. + */ +void dsDrawBox (const float pos[3], const float R[12], const float sides[3]); +void dsDrawSphere (const float pos[3], const float R[12], float radius); +void dsDrawTriangle (const float pos[3], const float R[12], + const float *v0, const float *v1, const float *v2, int solid); +void dsDrawCylinder (const float pos[3], const float R[12], + float length, float radius); +void dsDrawCappedCylinder (const float pos[3], const float R[12], + float length, float radius); +void dsDrawLine (const float pos1[3], const float pos2[3]); + +/* these drawing functions are identical to the ones above, except they take + * double arrays for `pos' and `R'. + */ +void dsDrawBoxD (const double pos[3], const double R[12], + const double sides[3]); +void dsDrawSphereD (const double pos[3], const double R[12], + const float radius); +void dsDrawTriangleD (const double pos[3], const double R[12], + const double *v0, const double *v1, const double *v2, int solid); +void dsDrawCylinderD (const double pos[3], const double R[12], + float length, float radius); +void dsDrawCappedCylinderD (const double pos[3], const double R[12], + float length, float radius); +void dsDrawLineD (const double pos1[3], const double pos2[3]); + +/* Set the drawn quality of the objects. Higher numbers are higher quality, + * but slower to draw. This must be set before the first objects are drawn to + * be effective. + */ +void dsSetSphereQuality (int n); /* default = 1 */ +void dsSetCappedCylinderQuality (int n); /* default = 3 */ + + +/* closing bracket for extern "C" */ +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_drawstuff_version.h b/src/external/open_dynamics_engine-ef/ode/ode_drawstuff_version.h new file mode 100644 index 00000000..71d95f46 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_drawstuff_version.h @@ -0,0 +1,29 @@ +/************************************************************************* + * * + * 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 __VERSION_H +#define __VERSION_H + +/* high byte is major version, low byte is minor version */ +#define DS_VERSION 0x0002 + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_error.cpp b/src/external/open_dynamics_engine-ef/ode/ode_error.cpp new file mode 100644 index 00000000..4fa3b98b --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_error.cpp @@ -0,0 +1,175 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_config.h" +#include "ode/ode_error.h" + + +static dMessageFunction *error_function = 0; +static dMessageFunction *debug_function = 0; +static dMessageFunction *message_function = 0; + + +extern "C" void dSetErrorHandler (dMessageFunction *fn) +{ + error_function = fn; +} + + +extern "C" void dSetDebugHandler (dMessageFunction *fn) +{ + debug_function = fn; +} + + +extern "C" void dSetMessageHandler (dMessageFunction *fn) +{ + message_function = fn; +} + + +extern "C" dMessageFunction *dGetErrorHandler() +{ + return error_function; +} + + +extern "C" dMessageFunction *dGetDebugHandler() +{ + return debug_function; +} + + +extern "C" dMessageFunction *dGetMessageHandler() +{ + return message_function; +} + + +static void printMessage (int num, const char *msg1, const char *msg2, + va_list ap) +{ + fflush (stderr); + fflush (stdout); + if (num) fprintf (stderr,"\n%s %d: ",msg1,num); + else fprintf (stderr,"\n%s: ",msg1); + vfprintf (stderr,msg2,ap); + fprintf (stderr,"\n"); + fflush (stderr); +} + +//**************************************************************************** +// unix + +#ifndef WIN32 + +extern "C" void dError (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (error_function) error_function (num,msg,ap); + else printMessage (num,"ODE Error",msg,ap); + exit (1); +} + + +extern "C" void dDebug (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (debug_function) debug_function (num,msg,ap); + else printMessage (num,"ODE INTERNAL ERROR",msg,ap); + // *((char *)0) = 0; ... commit SEGVicide + abort(); +} + + +extern "C" void dMessage (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (message_function) message_function (num,msg,ap); + else printMessage (num,"ODE Message",msg,ap); +} + +#endif + +//**************************************************************************** +// windows + +#ifdef WIN32 + +// isn't cygwin annoying! +#ifdef CYGWIN +#define _snprintf snprintf +#define _vsnprintf vsnprintf +#endif + + +#include "windows.h" + + +extern "C" void dError (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (error_function) error_function (num,msg,ap); + else { + char s[1000],title[100]; + _snprintf (title,sizeof(title),"ODE Error %d",num); + _vsnprintf (s,sizeof(s),msg,ap); + s[sizeof(s)-1] = 0; + //printf("FIXME dError NEEDS UPDATING\n"); + //cout << "FIXME DERROR BROKEN" << endl; + //MessageBox(0,s,title,MB_OK | MB_ICONWARNING); + } + exit (1); +} + + +extern "C" void dDebug (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (debug_function) debug_function (num,msg,ap); + else { + char s[1000],title[100]; + _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num); + _vsnprintf (s,sizeof(s),msg,ap); + s[sizeof(s)-1] = 0; + printf("FIXME dDebug NEEDS UPDATING\n"); + //MessageBox(0,s,title,MB_OK | MB_ICONSTOP); + } + abort(); +} + + +extern "C" void dMessage (int num, const char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + if (message_function) message_function (num,msg,ap); + else printMessage (num,"ODE Message",msg,ap); +} + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_error.h b/src/external/open_dynamics_engine-ef/ode/ode_error.h new file mode 100644 index 00000000..dd8e604a --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_error.h @@ -0,0 +1,63 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* this comes from the `reuse' library. copy any changes back to the source */ + +#ifndef _ODE_ERROR_H_ +#define _ODE_ERROR_H_ + +#include "ode/ode_config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* all user defined error functions have this type. error and debug functions + * should not return. + */ +typedef void dMessageFunction (int errnum, const char *msg, va_list ap); + +/* set a new error, debug or warning handler. if fn is 0, the default handlers + * are used. + */ +void dSetErrorHandler (dMessageFunction *fn); +void dSetDebugHandler (dMessageFunction *fn); +void dSetMessageHandler (dMessageFunction *fn); + +/* return the current error, debug or warning handler. if the return value is + * 0, the default handlers are in place. + */ +dMessageFunction *dGetErrorHandler(void); +dMessageFunction *dGetDebugHandler(void); +dMessageFunction *dGetMessageHandler(void); + +/* generate a fatal error, debug trap or a message. */ +void dError (int num, const char *msg, ...); +void dDebug (int num, const char *msg, ...); +void dMessage (int num, const char *msg, ...); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_export-dif.h b/src/external/open_dynamics_engine-ef/ode/ode_export-dif.h new file mode 100644 index 00000000..313d8727 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_export-dif.h @@ -0,0 +1,32 @@ +/************************************************************************* + * * + * 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_EXPORT_DIF_ +#define _ODE_EXPORT_DIF_ + +#include "ode/ode_common.h" + + +void dWorldExportDIF (dWorldID w, FILE *file, const char *world_name); + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_export-diff.cpp b/src/external/open_dynamics_engine-ef/ode/ode_export-diff.cpp new file mode 100644 index 00000000..935bcee8 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_export-diff.cpp @@ -0,0 +1,533 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + * Export a DIF (Dynamics Interchange Format) file. + */ + + +// @@@ TODO: +// * export all spaces, and geoms in spaces, not just ones attached to bodies +// (separate export function?) +// * say the space each geom is in, so reader can construct space heirarchy +// * limot --> separate out into limits and motors? +// * make sure ODE-specific parameters divided out + + +#include "ode/ode.h" +#include "ode/ode_objects_private.h" +#include "ode/ode_joint.h" +#include "ode/ode_collision_kernel.h" + +//*************************************************************************** +// utility + +struct PrintingContext { + FILE *file; // file to write to + int precision; // digits of precision to print + int indent; // number of levels of indent + + void printIndent(); + void printReal (dReal x); + void print (const char *name, int x); + void print (const char *name, dReal x); + void print (const char *name, const dReal *x, int n=3); + void print (const char *name, const char *x=0); + void printNonzero (const char *name, dReal x); + void printNonzero (const char *name, const dReal x[3]); +}; + + +void PrintingContext::printIndent() +{ + for (int i=0; i= 0) { + c.printIndent(); + fprintf (c.file,"limit%d = {\n",num); + } + else { + c.print ("limit = {"); + } + c.indent++; + c.print ("low_stop",limot.lostop); + c.print ("high_stop",limot.histop); + c.printNonzero ("bounce",limot.bounce); + c.print ("ODE = {"); + c.indent++; + c.printNonzero ("stop_erp",limot.stop_erp); + c.printNonzero ("stop_cfm",limot.stop_cfm); + c.indent--; + c.print ("},"); + c.indent--; + c.print ("},"); + + if (num >= 0) { + c.printIndent(); + fprintf (c.file,"motor%d = {\n",num); + } + else { + c.print ("motor = {"); + } + c.indent++; + c.printNonzero ("vel",limot.vel); + c.printNonzero ("fmax",limot.fmax); + c.print ("ODE = {"); + c.indent++; + c.printNonzero ("fudge_factor",limot.fudge_factor); + c.printNonzero ("normal_cfm",limot.normal_cfm); + c.indent--; + c.print ("},"); + c.indent--; + c.print ("},"); +} + + +static const char *getJointName (dxJoint *j) +{ + switch (j->vtable->typenum) { + case dJointTypeBall: return "ball"; + case dJointTypeHinge: return "hinge"; + case dJointTypeSlider: return "slider"; + case dJointTypeContact: return "contact"; + case dJointTypeUniversal: return "universal"; + case dJointTypeHinge2: return "ODE_hinge2"; + case dJointTypeFixed: return "fixed"; + case dJointTypeNull: return "null"; + case dJointTypeAMotor: return "ODE_angular_motor"; + } + return "unknown"; +} + + +static void printBall (PrintingContext &c, dxJoint *j) +{ + dxJointBall *b = (dxJointBall*) j; + c.print ("anchor1",b->anchor1); + c.print ("anchor2",b->anchor2); +} + + +static void printHinge (PrintingContext &c, dxJoint *j) +{ + dxJointHinge *h = (dxJointHinge*) j; + c.print ("anchor1",h->anchor1); + c.print ("anchor2",h->anchor2); + c.print ("axis1",h->axis1); + c.print ("axis2",h->axis2); + c.print ("qrel",h->qrel,4); + printLimot (c,h->limot,-1); +} + + +static void printSlider (PrintingContext &c, dxJoint *j) +{ + dxJointSlider *s = (dxJointSlider*) j; + c.print ("axis1",s->axis1); + c.print ("qrel",s->qrel,4); + c.print ("offset",s->offset); + printLimot (c,s->limot,-1); +} + + +static void printContact (PrintingContext &c, dxJoint *j) +{ + dxJointContact *ct = (dxJointContact*) j; + int mode = ct->contact.surface.mode; + c.print ("pos",ct->contact.geom.pos); + c.print ("normal",ct->contact.geom.normal); + c.print ("depth",ct->contact.geom.depth); + //@@@ may want to write the geoms g1 and g2 that are involved, for debugging. + // to do this we must have written out all geoms in all spaces, not just + // geoms that are attached to bodies. + c.print ("mu",ct->contact.surface.mu); + if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2); + if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce); + if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel); + if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp); + if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm); + if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1); + if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2); + if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1); + if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2); + int fa = 0; // friction approximation code + if (mode & dContactApprox1_1) fa |= 1; + if (mode & dContactApprox1_2) fa |= 2; + if (fa) c.print ("friction_approximation",fa); + if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1); +} + + +static void printUniversal (PrintingContext &c, dxJoint *j) +{ + dxJointUniversal *u = (dxJointUniversal*) j; + c.print ("anchor1",u->anchor1); + c.print ("anchor2",u->anchor2); + c.print ("axis1",u->axis1); + c.print ("axis2",u->axis2); + c.print ("qrel1",u->qrel1,4); + c.print ("qrel2",u->qrel2,4); + printLimot (c,u->limot1,1); + printLimot (c,u->limot2,2); +} + + +static void printHinge2 (PrintingContext &c, dxJoint *j) +{ + dxJointHinge2 *h = (dxJointHinge2*) j; + c.print ("anchor1",h->anchor1); + c.print ("anchor2",h->anchor2); + c.print ("axis1",h->axis1); + c.print ("axis2",h->axis2); + c.print ("v1",h->v1); //@@@ much better to write out 'qrel' here, if it's available + c.print ("v2",h->v2); + c.print ("susp_erp",h->susp_erp); + c.print ("susp_cfm",h->susp_cfm); + printLimot (c,h->limot1,1); + printLimot (c,h->limot2,2); +} + + +static void printFixed (PrintingContext &c, dxJoint *j) +{ + dxJointFixed *f = (dxJointFixed*) j; + c.print ("qrel",f->qrel); + c.print ("offset",f->offset); +} + + +static void printAMotor (PrintingContext &c, dxJoint *j) +{ + dxJointAMotor *a = (dxJointAMotor*) j; + c.print ("num",a->num); + c.print ("mode",a->mode); + c.printIndent(); + fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]); + c.print ("axis1",a->axis[0]); + c.print ("axis2",a->axis[1]); + c.print ("axis3",a->axis[2]); + for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1); + c.print ("angle1",a->angle[0]); + c.print ("angle2",a->angle[1]); + c.print ("angle3",a->angle[2]); +} + +//*************************************************************************** +// geometry + +static void printGeom (PrintingContext &c, dxGeom *g); + +static void printSphere (PrintingContext &c, dxGeom *g) +{ + c.print ("type","sphere"); + c.print ("radius",dGeomSphereGetRadius (g)); +} + + +static void printBox (PrintingContext &c, dxGeom *g) +{ + dVector3 sides; + dGeomBoxGetLengths (g,sides); + c.print ("type","box"); + c.print ("sides",sides); +} + + + +static void printCCylinder (PrintingContext &c, dxGeom *g) +{ + dReal radius,length; + dGeomCCylinderGetParams (g,&radius,&length); + c.print ("type","capsule"); + c.print ("radius",radius); + c.print ("length",length); +} + + +static void printPlane (PrintingContext &c, dxGeom *g) +{ + dVector4 e; + dGeomPlaneGetParams (g,e); + c.print ("type","plane"); + c.print ("normal",e); + c.print ("d",e[3]); +} + + + +static void printRay (PrintingContext &c, dxGeom *g) +{ + dReal length = dGeomRayGetLength (g); + c.print ("type","ray"); + c.print ("length",length); +} + + + +static void printGeomTransform (PrintingContext &c, dxGeom *g) +{ + dxGeom *g2 = dGeomTransformGetGeom (g); + const dReal *pos = dGeomGetPosition (g2); + dQuaternion q; + dGeomGetQuaternion (g2,q); + c.print ("type","transform"); + c.print ("pos",pos); + c.print ("q",q,4); + c.print ("geometry = {"); + c.indent++; + printGeom (c,g2); + c.indent--; + c.print ("}"); +} + + + +static void printTriMesh (PrintingContext &c, dxGeom *g) +{ + c.print ("type","trimesh"); + //@@@ i don't think that the trimesh accessor functions are really + // sufficient to read out all the triangle data, and anyway we + // should have a method of not duplicating trimesh data that is + // shared. +} + + +static void printGeom (PrintingContext &c, dxGeom *g) +{ + unsigned long category = dGeomGetCategoryBits (g); + if (category != (unsigned long)(~0)) { + c.printIndent(); + fprintf (c.file,"category_bits = %lu\n",category); + } + unsigned long collide = dGeomGetCollideBits (g); + if (collide != (unsigned long)(~0)) { + c.printIndent(); + fprintf (c.file,"collide_bits = %lu\n",collide); + } + if (!dGeomIsEnabled (g)) { + c.print ("disabled",1); + } + switch (g->type) { + case dSphereClass: printSphere (c,g); break; + case dBoxClass: printBox (c,g); break; + case dCCylinderClass: printCCylinder (c,g); break; + case dPlaneClass: printPlane (c,g); break; + case dRayClass: printRay (c,g); break; + case dGeomTransformClass: printGeomTransform (c,g); break; + case dTriMeshClass: printTriMesh (c,g); break; + } +} + +//*************************************************************************** +// world + +void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix) +{ + PrintingContext c; + c.file = file; +#if defined(dSINGLE) + c.precision = 7; +#else + c.precision = 15; +#endif + c.indent = 1; + + fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix); + c.print ("gravity",w->gravity); + c.print ("ODE = {"); + c.indent++; + c.print ("ERP",w->global_erp); + c.print ("CFM",w->global_cfm); + c.print ("auto_disable = {"); + c.indent++; + c.print ("linear_threshold",w->adis.linear_threshold); + c.print ("angular_threshold",w->adis.angular_threshold); + c.print ("idle_time",w->adis.idle_time); + c.print ("idle_steps",w->adis.idle_steps); + fprintf (file,"\t\t},\n\t},\n}\n"); + c.indent -= 3; + + // bodies + int num = 0; + fprintf (file,"%sbody = {}\n",prefix); + for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) { + b->tag = num; + fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix); + c.indent++; + c.print ("pos",b->pos); + c.print ("q",b->q,4); + c.print ("lvel",b->lvel); + c.print ("avel",b->avel); + c.print ("mass",b->mass.mass); + fprintf (file,"\tI = {{"); + for (int i=0; i<3; i++) { + for (int j=0; j<3; j++) { + c.printReal (b->mass.I[i*4+j]); + if (j < 2) fputc (',',file); + } + if (i < 2) fprintf (file,"},{"); + } + fprintf (file,"}},\n"); + c.printNonzero ("com",b->mass.c); + c.print ("ODE = {"); + c.indent++; + if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1); + if (b->flags & dxBodyDisabled) c.print ("disabled",1); + if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1); + if (b->flags & dxBodyAutoDisable) { + c.print ("auto_disable = {"); + c.indent++; + c.print ("linear_threshold",b->adis.linear_threshold); + c.print ("angular_threshold",b->adis.angular_threshold); + c.print ("idle_time",b->adis.idle_time); + c.print ("idle_steps",b->adis.idle_steps); + c.print ("time_left",b->adis_timeleft); + c.print ("steps_left",b->adis_stepsleft); + c.indent--; + c.print ("},"); + } + c.printNonzero ("facc",b->facc); + c.printNonzero ("tacc",b->tacc); + if (b->flags & dxBodyFlagFiniteRotationAxis) { + c.print ("finite_rotation_axis",b->finite_rot_axis); + } + c.indent--; + c.print ("},"); + if (b->geom) { + c.print ("geometry = {"); + c.indent++; + for (dxGeom *g=b->geom; g; g=g->body_next) { + c.print ("{"); + c.indent++; + printGeom (c,g); + c.indent--; + c.print ("},"); + } + c.indent--; + c.print ("},"); + } + c.indent--; + c.print ("}"); + num++; + } + + // joints + num = 0; + fprintf (file,"%sjoint = {}\n",prefix); + for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) { + c.indent++; + const char *name = getJointName (j); + fprintf (file, + "%sjoint[%d] = dynamics.%s_joint {\n" + "\tworld = %sworld,\n" + "\tbody = {%sbody[%d]" + ,prefix,num,name,prefix,prefix,j->node[0].body->tag); + if (j->node[1].body) fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag); + fprintf (file,"},\n"); + switch (j->vtable->typenum) { + case dJointTypeBall: printBall (c,j); break; + case dJointTypeHinge: printHinge (c,j); break; + case dJointTypeSlider: printSlider (c,j); break; + case dJointTypeContact: printContact (c,j); break; + case dJointTypeUniversal: printUniversal (c,j); break; + case dJointTypeHinge2: printHinge2 (c,j); break; + case dJointTypeFixed: printFixed (c,j); break; + case dJointTypeAMotor: printAMotor (c,j); break; + } + c.indent--; + c.print ("}"); + num++; + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_fastdot.cpp b/src/external/open_dynamics_engine-ef/ode/ode_fastdot.cpp new file mode 100644 index 00000000..8fa792eb --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_fastdot.cpp @@ -0,0 +1,30 @@ +/* generated code, do not edit. */ + +#include "ode/ode_matrix.h" + + +dReal dDot (const dReal *a, const dReal *b, int n) +{ + dReal p0,q0,m0,p1,q1,m1,sum; + sum = 0; + n -= 2; + while (n >= 0) { + p0 = a[0]; q0 = b[0]; + m0 = p0 * q0; + p1 = a[1]; q1 = b[1]; + m1 = p1 * q1; + sum += m0; + sum += m1; + a += 2; + b += 2; + n -= 2; + } + n += 2; + while (n > 0) { + sum += (*a) * (*b); + a++; + b++; + n--; + } + return sum; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_fastldlt.cpp b/src/external/open_dynamics_engine-ef/ode/ode_fastldlt.cpp new file mode 100644 index 00000000..895d125b --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_fastldlt.cpp @@ -0,0 +1,383 @@ +/* generated code, do not edit. */ + +#include "ode/ode_matrix.h" + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ + +static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,m11,Z21,m21,p1,q1,p2,*ex; + const dReal *ell; + int i,j; + /* compute all 2 x 1 blocks of X */ + for (i=0; i < n; i+=2) { + /* compute all 2 x 1 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-2; j >= 0; j -= 2) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + p2=ell[lskip1]; + m21 = p2 * q1; + Z11 += m11; + Z21 += m21; + /* compute outer product and add it to the Z matrix */ + p1=ell[1]; + q1=ex[1]; + m11 = p1 * q1; + p2=ell[1+lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z21 += m21; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + p2=ell[lskip1]; + m21 = p2 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z21 += m21; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + /* end of outer loop */ + } +} + +/* solve L*X=B, with B containing 2 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*2 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 2*2. + * if this is in the factorizer source file, n must be a multiple of 2. + */ + +static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex; + const dReal *ell; + int i,j; + /* compute all 2 x 2 blocks of X */ + for (i=0; i < n; i+=2) { + /* compute all 2 x 2 block of X, from rows i..i+2-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z12=0; + Z21=0; + Z22=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-2; j >= 0; j -= 2) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + q2=ex[lskip1]; + m12 = p1 * q2; + p2=ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* compute outer product and add it to the Z matrix */ + p1=ell[1]; + q1=ex[1]; + m11 = p1 * q1; + q2=ex[1+lskip1]; + m12 = p1 * q2; + p2=ell[1+lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 2; + ex += 2; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 2; + for (; j > 0; j--) { + /* compute outer product and add it to the Z matrix */ + p1=ell[0]; + q1=ex[0]; + m11 = p1 * q1; + q2=ex[lskip1]; + m12 = p1 * q2; + p2=ell[lskip1]; + m21 = p2 * q1; + m22 = p2 * q2; + /* advance pointers */ + ell += 1; + ex += 1; + Z11 += m11; + Z12 += m12; + Z21 += m21; + Z22 += m22; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + Z12 = ex[lskip1] - Z12; + ex[lskip1] = Z12; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + Z22 = ex[1+lskip1] - Z22 - p1*Z12; + ex[1+lskip1] = Z22; + /* end of outer loop */ + } +} + + +void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1) +{ + int i,j; + dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22; + if (n < 1) return; + + for (i=0; i<=n-2; i += 2) { + /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ + dSolveL1_2 (A,A+i*nskip1,i,nskip1); + /* scale the elements in a 2 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + Z21 = 0; + Z22 = 0; + ell = A+i*nskip1; + dee = d; + for (j=i-6; j >= 0; j -= 6) { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1*dd; + q2 = p2*dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[1]; + p2 = ell[1+nskip1]; + dd = dee[1]; + q1 = p1*dd; + q2 = p2*dd; + ell[1] = q1; + ell[1+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[2]; + p2 = ell[2+nskip1]; + dd = dee[2]; + q1 = p1*dd; + q2 = p2*dd; + ell[2] = q1; + ell[2+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[3]; + p2 = ell[3+nskip1]; + dd = dee[3]; + q1 = p1*dd; + q2 = p2*dd; + ell[3] = q1; + ell[3+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[4]; + p2 = ell[4+nskip1]; + dd = dee[4]; + q1 = p1*dd; + q2 = p2*dd; + ell[4] = q1; + ell[4+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + p1 = ell[5]; + p2 = ell[5+nskip1]; + dd = dee[5]; + q1 = p1*dd; + q2 = p2*dd; + ell[5] = q1; + ell[5+nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) { + p1 = ell[0]; + p2 = ell[nskip1]; + dd = dee[0]; + q1 = p1*dd; + q2 = p2*dd; + ell[0] = q1; + ell[nskip1] = q2; + m11 = p1*q1; + m21 = p2*q1; + m22 = p2*q2; + Z11 += m11; + Z21 += m21; + Z22 += m22; + ell++; + dee++; + } + /* solve for diagonal 2 x 2 block at A(i,i) */ + Z11 = ell[0] - Z11; + Z21 = ell[nskip1] - Z21; + Z22 = ell[1+nskip1] - Z22; + dee = d + i; + /* factorize 2 x 2 block Z,dee */ + /* factorize row 1 */ + dee[0] = dRecip(Z11); + /* factorize row 2 */ + sum = 0; + q1 = Z21; + q2 = q1 * dee[0]; + Z21 = q2; + sum += q1*q2; + dee[1] = dRecip(Z22 - sum); + /* done factorizing 2 x 2 block */ + ell[nskip1] = Z21; + } + /* compute the (less than 2) rows at the bottom */ + switch (n-i) { + case 0: + break; + + case 1: + dSolveL1_1 (A,A+i*nskip1,i,nskip1); + /* scale the elements in a 1 x i block at A(i,0), and also */ + /* compute Z = the outer product matrix that we'll need. */ + Z11 = 0; + ell = A+i*nskip1; + dee = d; + for (j=i-6; j >= 0; j -= 6) { + p1 = ell[0]; + dd = dee[0]; + q1 = p1*dd; + ell[0] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[1]; + dd = dee[1]; + q1 = p1*dd; + ell[1] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[2]; + dd = dee[2]; + q1 = p1*dd; + ell[2] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[3]; + dd = dee[3]; + q1 = p1*dd; + ell[3] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[4]; + dd = dee[4]; + q1 = p1*dd; + ell[4] = q1; + m11 = p1*q1; + Z11 += m11; + p1 = ell[5]; + dd = dee[5]; + q1 = p1*dd; + ell[5] = q1; + m11 = p1*q1; + Z11 += m11; + ell += 6; + dee += 6; + } + /* compute left-over iterations */ + j += 6; + for (; j > 0; j--) { + p1 = ell[0]; + dd = dee[0]; + q1 = p1*dd; + ell[0] = q1; + m11 = p1*q1; + Z11 += m11; + ell++; + dee++; + } + /* solve for diagonal 1 x 1 block at A(i,i) */ + Z11 = ell[0] - Z11; + dee = d + i; + /* factorize 1 x 1 block Z,dee */ + /* factorize row 1 */ + dee[0] = dRecip(Z11); + /* done factorizing 1 x 1 block */ + break; + + default: + abort(); + //*((char*)0)=0; /* this should never happen! */ + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_fastlsolve.cpp b/src/external/open_dynamics_engine-ef/ode/ode_fastlsolve.cpp new file mode 100644 index 00000000..6ab348b5 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_fastlsolve.cpp @@ -0,0 +1,298 @@ +/* generated code, do not edit. */ + +#include "ode/ode_matrix.h" + +/* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * B is an n*1 matrix that contains the right hand sides. + * B is stored by columns and its leading dimension is also lskip. + * B is overwritten with X. + * this processes blocks of 4*4. + * if this is in the factorizer source file, n must be a multiple of 4. + */ + +void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex; + const dReal *ell; + int lskip2,lskip3,i,j; + /* compute lskip values */ + lskip2 = 2*lskip1; + lskip3 = 3*lskip1; + /* compute all 4 x 1 blocks of X */ + for (i=0; i <= n-4; i+=4) { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + Z31=0; + Z41=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-12; j >= 0; j -= 12) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[lskip1]; + p3=ell[lskip2]; + p4=ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[1]; + q1=ex[1]; + p2=ell[1+lskip1]; + p3=ell[1+lskip2]; + p4=ell[1+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[2]; + q1=ex[2]; + p2=ell[2+lskip1]; + p3=ell[2+lskip2]; + p4=ell[2+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[3]; + q1=ex[3]; + p2=ell[3+lskip1]; + p3=ell[3+lskip2]; + p4=ell[3+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[4]; + q1=ex[4]; + p2=ell[4+lskip1]; + p3=ell[4+lskip2]; + p4=ell[4+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[5]; + q1=ex[5]; + p2=ell[5+lskip1]; + p3=ell[5+lskip2]; + p4=ell[5+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[6]; + q1=ex[6]; + p2=ell[6+lskip1]; + p3=ell[6+lskip2]; + p4=ell[6+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[7]; + q1=ex[7]; + p2=ell[7+lskip1]; + p3=ell[7+lskip2]; + p4=ell[7+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[8]; + q1=ex[8]; + p2=ell[8+lskip1]; + p3=ell[8+lskip2]; + p4=ell[8+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[9]; + q1=ex[9]; + p2=ell[9+lskip1]; + p3=ell[9+lskip2]; + p4=ell[9+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[10]; + q1=ex[10]; + p2=ell[10+lskip1]; + p3=ell[10+lskip2]; + p4=ell[10+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* load p and q values */ + p1=ell[11]; + q1=ex[11]; + p2=ell[11+lskip1]; + p3=ell[11+lskip2]; + p4=ell[11+lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[lskip1]; + p3=ell[lskip2]; + p4=ell[lskip3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + Z21 += p2 * q1; + Z31 += p3 * q1; + Z41 += p4 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[lskip1]; + Z21 = ex[1] - Z21 - p1*Z11; + ex[1] = Z21; + p1 = ell[lskip2]; + p2 = ell[1+lskip2]; + Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21; + ex[2] = Z31; + p1 = ell[lskip3]; + p2 = ell[1+lskip3]; + p3 = ell[2+lskip3]; + Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; + ex[3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11=0; + ell = L + i*lskip1; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-12; j >= 0; j -= 12) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[1]; + q1=ex[1]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[2]; + q1=ex[2]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[3]; + q1=ex[3]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[4]; + q1=ex[4]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[5]; + q1=ex[5]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[6]; + q1=ex[6]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[7]; + q1=ex[7]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[8]; + q1=ex[8]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[9]; + q1=ex[9]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[10]; + q1=ex[10]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* load p and q values */ + p1=ell[11]; + q1=ex[11]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 12; + ex += 12; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 12; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + Z11 += p1 * q1; + /* advance pointers */ + ell += 1; + ex += 1; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_fastltsolve.cpp b/src/external/open_dynamics_engine-ef/ode/ode_fastltsolve.cpp new file mode 100644 index 00000000..43d93565 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_fastltsolve.cpp @@ -0,0 +1,199 @@ +/* generated code, do not edit. */ + +#include "ode/ode_matrix.h" + +/* solve L^T * x=b, with b containing 1 right hand side. + * L is an n*n lower triangular matrix with ones on the diagonal. + * L is stored by rows and its leading dimension is lskip. + * b is an n*1 matrix that contains the right hand side. + * b is overwritten with x. + * this processes blocks of 4. + */ + +void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1) +{ + /* declare variables - Z matrix, p and q vectors, etc */ + dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; + const dReal *ell; + int lskip2,lskip3,i,j; + /* special handling for L and B because we're solving L1 *transpose* */ + L = L + (n-1)*(lskip1+1); + B = B + n-1; + lskip1 = -lskip1; + /* compute lskip values */ + lskip2 = 2*lskip1; + lskip3 = 3*lskip1; + /* compute all 4 x 1 blocks of X */ + for (i=0; i <= n-4; i+=4) { + /* compute all 4 x 1 block of X, from rows i..i+4-1 */ + /* set the Z matrix to 0 */ + Z11=0; + Z21=0; + Z31=0; + Z41=0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-4; j >= 0; j -= 4) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-1]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-2]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* load p and q values */ + p1=ell[0]; + q1=ex[-3]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + p2=ell[-1]; + p3=ell[-2]; + p4=ell[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + m21 = p2 * q1; + m31 = p3 * q1; + m41 = p4 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + Z21 += m21; + Z31 += m31; + Z41 += m41; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + p1 = ell[-1]; + Z21 = ex[-1] - Z21 - p1*Z11; + ex[-1] = Z21; + p1 = ell[-2]; + p2 = ell[-2+lskip1]; + Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21; + ex[-2] = Z31; + p1 = ell[-3]; + p2 = ell[-3+lskip1]; + p3 = ell[-3+lskip2]; + Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; + ex[-3] = Z41; + /* end of outer loop */ + } + /* compute rows at end that are not a multiple of block size */ + for (; i < n; i++) { + /* compute all 1 x 1 block of X, from rows i..i+1-1 */ + /* set the Z matrix to 0 */ + Z11=0; + ell = L - i; + ex = B; + /* the inner loop that computes outer products and adds them to Z */ + for (j=i-4; j >= 0; j -= 4) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-1]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-2]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + Z11 += m11; + /* load p and q values */ + p1=ell[0]; + q1=ex[-3]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 4; + Z11 += m11; + /* end of inner loop */ + } + /* compute left-over iterations */ + j += 4; + for (; j > 0; j--) { + /* load p and q values */ + p1=ell[0]; + q1=ex[0]; + /* compute outer product and add it to the Z matrix */ + m11 = p1 * q1; + ell += lskip1; + ex -= 1; + Z11 += m11; + } + /* finish computing the X(i) block */ + Z11 = ex[0] - Z11; + ex[0] = Z11; + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_joint.cpp b/src/external/open_dynamics_engine-ef/ode/ode_joint.cpp new file mode 100644 index 00000000..193f5a20 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_joint.cpp @@ -0,0 +1,3247 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +design note: the general principle for giving a joint the option of connecting +to the static environment (i.e. the absolute frame) is to check the second +body (joint->node[1].body), and if it is zero then behave as if its body +transform is the identity. + +*/ + +#include "ode/ode_math.h" +#include "ode/ode_rotation.h" +#include "ode/ode_matrix.h" +#include "ode/ode_joint.h" +#include "ode/ode_util.h" + +// #include + +//**************************************************************************** +// externs + +extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); + +//**************************************************************************** +// utility + +// set three "ball-and-socket" rows in the constraint equation, and the +// corresponding right hand side. + +static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int s = info->rowskip; + +#if VALUE_TESTING + if (testLogging) + fprintf(f,"rowskip %d\n",s); +#endif + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dCROSSMAT (info->J1a,a1,s,-,+); + + if (joint->node[1].body) { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dCROSSMAT (info->J2a,a2,s,+,-); + + +#if VALUE_TESTING + if (testLogging){ + fprintf(f," %f ",info->J1a[1]); PRINT_DBL_HEX(info->J1a[1]); + fprintf(f," %f ",info->J1a[2]); PRINT_DBL_HEX(info->J1a[2]); + fprintf(f," %f ",info->J1a[s+0]); PRINT_DBL_HEX(info->J1a[s+0]); + fprintf(f," %f ",info->J1a[s+2]); PRINT_DBL_HEX(info->J1a[s+2]); + fprintf(f," %f ",info->J1a[2*s+0]); PRINT_DBL_HEX(info->J1a[2*s+0]); + fprintf(f," %f ",info->J1a[2*s+1]); PRINT_DBL_HEX(info->J1a[2*s+1]); + } +#endif + + +#if VALUE_TESTING + if (testLogging){ + fprintf(f," %f ",info->J2a[1]); PRINT_DBL_HEX(info->J2a[1]); + fprintf(f," %f ",info->J2a[2]); PRINT_DBL_HEX(info->J2a[2]); + fprintf(f," %f ",info->J2a[s+0]); PRINT_DBL_HEX(info->J2a[s+0]); + fprintf(f," %f ",info->J2a[s+2]); PRINT_DBL_HEX(info->J2a[s+2]); + fprintf(f," %f ",info->J2a[2*s+0]); PRINT_DBL_HEX(info->J2a[2*s+0]); + fprintf(f," %f ",info->J2a[2*s+1]); PRINT_DBL_HEX(info->J2a[2*s+1]); + } +#endif + + + } + + // set right hand side + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) { + info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] - + a1[j] - joint->node[0].body->pos[j]); + } + } + else { + for (int j=0; j<3; j++) { + info->c[j] = k * (anchor2[j] - a1[j] - + joint->node[0].body->pos[j]); + } + } +} + + +// this is like setBall(), except that `axis' is a unit length vector +// (in global coordinates) that should be used for the first jacobian +// position row (the other two row vectors will be derived from this). +// `erp1' is the erp value to use along the axis. + +static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2, + dVector3 axis, dReal erp1) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int i,s = info->rowskip; + + // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], + // [0 1 0] and [0 0 1], which makes everything much easier. + dVector3 q1,q2; + dPlaneSpace (axis,q1,q2); + + // set jacobian + for (i=0; i<3; i++) info->J1l[i] = axis[i]; + for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; + for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; + dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dCROSS (info->J1a,=,a1,axis); + dCROSS (info->J1a+s,=,a1,q1); + dCROSS (info->J1a+2*s,=,a1,q2); + if (joint->node[1].body) { + for (i=0; i<3; i++) info->J2l[i] = -axis[i]; + for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; + for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; + dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dCROSS (info->J2a,= -,a2,axis); + dCROSS (info->J2a+s,= -,a2,q1); + dCROSS (info->J2a+2*s,= -,a2,q2); + } + + // set right hand side - measure error along (axis,q1,q2) + dReal k1 = info->fps * erp1; + dReal k = info->fps * info->erp; + + for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; + if (joint->node[1].body) { + for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; + info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[0] %f ",info->c[0]); PRINT_DBL_HEX(info->c[0]); + fprintf(f,"c[1] %f ",info->c[1]); PRINT_DBL_HEX(info->c[1]); + fprintf(f,"c[2] %f ",info->c[2]); PRINT_DBL_HEX(info->c[2]); + } +#endif + + } + else { + info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[0] %f ",info->c[0]); PRINT_DBL_HEX(info->c[0]); + fprintf(f,"c[1] %f ",info->c[1]); PRINT_DBL_HEX(info->c[1]); + fprintf(f,"c[2] %f ",info->c[2]); PRINT_DBL_HEX(info->c[2]); + } +#endif + + } +} + + +// set three orientation rows in the constraint equation, and the +// corresponding right hand side. + +static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) +{ + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->J1a[start_index] = 1; + info->J1a[start_index + s + 1] = 1; + info->J1a[start_index + s*2+2] = 1; + if (joint->node[1].body) { + info->J2a[start_index] = -1; + info->J2a[start_index + s+1] = -1; + info->J2a[start_index + s*2+2] = -1; + } + + // compute the right hand side. the first three elements will result in + // relative angular velocity of the two bodies - this is set to bring them + // back into alignment. the correcting angular velocity is + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * u + // = (erp*fps) * theta * u + // where rotation along unit length axis u by theta brings body 2's frame + // to qrel with respect to body 1's frame. using a small angle approximation + // for sin(), this gives + // angular_velocity = (erp*fps) * 2 * v + // where the quaternion of the relative rotation between the two bodies is + // q = [cos(theta/2) sin(theta/2)*u] = [s v] + + // get qerr = relative rotation (rotation error) between two bodies + dQuaternion qerr,e; + if (joint->node[1].body) { + dQuaternion qq; + dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); + dQMultiply2 (qerr,qq,qrel); + } + else { + dQMultiply3 (qerr,joint->node[0].body->q,qrel); + } + if (qerr[0] < 0) { + qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small + qerr[2] = -qerr[2]; + qerr[3] = -qerr[3]; + } + dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! + dReal k = info->fps * info->erp; + info->c[start_row] = 2*k * e[0]; + info->c[start_row+1] = 2*k * e[1]; + info->c[start_row+2] = 2*k * e[2]; + +} + + + +// compute anchor points relative to bodies + +static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 anchor1, dVector3 anchor2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x - j->node[0].body->pos[0]; + q[1] = y - j->node[0].body->pos[1]; + q[2] = z - j->node[0].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); + if (j->node[1].body) { + q[0] = x - j->node[1].body->pos[0]; + q[1] = y - j->node[1].body->pos[1]; + q[2] = z - j->node[1].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); + } + else { + anchor2[0] = x; + anchor2[1] = y; + anchor2[2] = z; + } + } + anchor1[3] = 0; + anchor2[3] = 0; +} + +// added by Eric Froemling - sets anchor 2 in absolute position +// for manually moving the target point for a single-body fixed constraint +static void setAnchorsAbs (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 anchor1, dVector3 anchor2) +{ + if (j->node[0].body) { +// dReal q[4]; +// q[0] = x - j->node[0].body->pos[0]; +// q[1] = y - j->node[0].body->pos[1]; +// q[2] = z - j->node[0].body->pos[2]; +// q[3] = 0; +// dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); +// if (j->node[1].body) { +// q[0] = x - j->node[1].body->pos[0]; +// q[1] = y - j->node[1].body->pos[1]; +// q[2] = z - j->node[1].body->pos[2]; +// q[3] = 0; +// dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); +// } +// else { +// dReal q[4]; +// q[0] = x; +// q[1] = y; +// q[2] = z; +// q[3] = 0; +// dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); + anchor2[0] = x; + anchor2[1] = y; + anchor2[2] = z; +// } + } +// anchor1[3] = 0; +// anchor2[3] = 0; +} + + +// compute axes relative to bodies. either axis1 or axis2 can be 0. + +static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 axis1, dVector3 axis2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + if (axis1) { + dMULTIPLY1_331 (axis1,j->node[0].body->R,q); + axis1[3] = 0; + } + if (axis2) { + if (j->node[1].body) { + dMULTIPLY1_331 (axis2,j->node[1].body->R,q); + } + else { + axis2[0] = x; + axis2[1] = y; + axis2[2] = z; + } + axis2[3] = 0; + } + } +} + + +static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->R,anchor1); + result[0] += j->node[0].body->pos[0]; + result[1] += j->node[0].body->pos[1]; + result[2] += j->node[0].body->pos[2]; + } +} + + +static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->R,anchor2); + result[0] += j->node[1].body->pos[0]; + result[1] += j->node[1].body->pos[1]; + result[2] += j->node[1].body->pos[2]; + } + else { + result[0] = anchor2[0]; + result[1] = anchor2[1]; + result[2] = anchor2[2]; + } +} + + +static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->R,axis1); + } +} + + +static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->R,axis2); + } + else { + result[0] = axis2[0]; + result[1] = axis2[1]; + result[2] = axis2[2]; + } +} + + +static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) +{ + // the angle between the two bodies is extracted from the quaternion that + // represents the relative rotation between them. recall that a quaternion + // q is: + // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] + // where s is a scalar and v is a 3-vector. u is a unit length axis and + // theta is a rotation along that axis. we can get theta/2 by: + // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) + // but we can't get sin(theta/2) directly, only its absolute value, i.e.: + // |v| = |sin(theta/2)| * |u| + // = |sin(theta/2)| + // using this value will have a strange effect. recall that there are two + // quaternion representations of a given rotation, q and -q. typically as + // a body rotates along the axis it will go through a complete cycle using + // one representation and then the next cycle will use the other + // representation. this corresponds to u pointing in the direction of the + // hinge axis and then in the opposite direction. the result is that theta + // will appear to go "backwards" every other cycle. here is a fix: if u + // points "away" from the direction of the hinge (motor) axis (i.e. more + // than 90 degrees) then use -q instead of q. this represents the same + // rotation, but results in the cos(theta/2) value being sign inverted. + + // extract the angle from the quaternion. cost2 = cos(theta/2), + // sint2 = |sin(theta/2)| + dReal cost2 = qrel[0]; + dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); + dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions + (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis + (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction + + // the angle we get will be between 0..2*pi, but we want to return angles + // between -pi..pi + if (theta > M_PI) theta -= 2*M_PI; + + // the angle we've just extracted has the wrong sign + theta = -theta; + + return theta; +} + + +// given two bodies (body1,body2), the hinge axis that they are connected by +// w.r.t. body1 (axis), and the initial relative orientation between them +// (q_initial), return the relative rotation angle. the initial relative +// orientation corresponds to an angle of zero. if body2 is 0 then measure the +// angle between body1 and the static frame. +// +// this will not return the correct angle if the bodies rotate along any axis +// other than the given hinge axis. + +static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, + dQuaternion q_initial) +{ + // get qrel = relative rotation between the two bodies + dQuaternion qrel; + if (body2) { + dQuaternion qq; + dQMultiply1 (qq,body1->q,body2->q); + dQMultiply2 (qrel,qq,q_initial); + } + else { + // pretend body2->q is the identity + dQMultiply3 (qrel,body1->q,q_initial); + } + + return getHingeAngleFromRelativeQuat (qrel,axis); +} + +//**************************************************************************** +// dxJointLimitMotor + +void dxJointLimitMotor::init (dxWorld *world) +{ + vel = 0; + fmax = 0; + lostop = -dInfinity; + histop = dInfinity; + fudge_factor = 1; + normal_cfm = world->global_cfm; + stop_erp = world->global_erp; + stop_cfm = world->global_cfm; + bounce = 0; + limit = 0; + limit_err = 0; +} + + +void dxJointLimitMotor::set (int num, dReal value) +{ + switch (num) { + case dParamLoStop: + if (value <= histop) lostop = value; + break; + case dParamHiStop: + if (value >= lostop) histop = value; + break; + case dParamVel: + //if (testLogging){ + //printf("dParamVel value %f ",value); PRINT_DBL_HEX_STDOUT(value); + //} + vel = value; + //if (testLogging){ + //printf("dParamVel valueb %f ",vel); PRINT_DBL_HEX_STDOUT(vel); + //} + break; + case dParamFMax: + if (value >= 0) fmax = value; + break; + case dParamFudgeFactor: + if (value >= 0 && value <= 1) fudge_factor = value; + break; + case dParamBounce: + bounce = value; + break; + case dParamCFM: + normal_cfm = value; + break; + case dParamStopERP: + stop_erp = value; + break; + case dParamStopCFM: + stop_cfm = value; + break; + } +} + + +dReal dxJointLimitMotor::get (int num) +{ + switch (num) { + case dParamLoStop: return lostop; + case dParamHiStop: return histop; + case dParamVel: return vel; + case dParamFMax: return fmax; + case dParamFudgeFactor: return fudge_factor; + case dParamBounce: return bounce; + case dParamCFM: return normal_cfm; + case dParamStopERP: return stop_erp; + case dParamStopCFM: return stop_cfm; + default: return 0; + } +} + + +int dxJointLimitMotor::testRotationalLimit (dReal angle) +{ + if (angle <= lostop) { + limit = 1; + limit_err = angle - lostop; + return 1; + } + else if (angle >= histop) { + limit = 2; + limit_err = angle - histop; + return 1; + } + else { + limit = 0; + return 0; + } +} + + +int dxJointLimitMotor::addLimot (dxJoint *joint, + dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational) +{ + int srow = row * info->rowskip; + + // if the joint is powered, or has joint limits, add in the extra row + int powered = fmax > 0; + if (powered || limit) { + dReal *J1 = rotational ? info->J1a : info->J1l; + dReal *J2 = rotational ? info->J2a : info->J2l; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (joint->node[1].body) { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"J1[srow+0] %f ",J1[srow+0]); PRINT_DBL_HEX(J1[srow+0]); + fprintf(f,"J1[srow+1] %f ",J1[srow+1]); PRINT_DBL_HEX(J1[srow+1]); + fprintf(f,"J1[srow+2] %f ",J1[srow+2]); PRINT_DBL_HEX(J1[srow+2]); + + fprintf(f,"J2[srow+0] %f ",J2[srow+0]); PRINT_DBL_HEX(J2[srow+0]); + fprintf(f,"J2[srow+1] %f ",J2[srow+1]); PRINT_DBL_HEX(J2[srow+1]); + fprintf(f,"J2[srow+2] %f ",J2[srow+2]); PRINT_DBL_HEX(J2[srow+2]); + } +#endif + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && joint->node[1].body) { + dVector3 c; + c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]); + dCROSS (ltd,=,c,ax1); + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[srow+2] = ltd[2]; + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"info->J1a[srow+0] %f ",info->J1a[srow+0]); PRINT_DBL_HEX(info->J1a[srow+0]); + fprintf(f,"info->J1a[srow+1] %f ",info->J1a[srow+1]); PRINT_DBL_HEX(info->J1a[srow+1]); + fprintf(f,"info->J1a[srow+2] %f ",info->J1a[srow+2]); PRINT_DBL_HEX(info->J1a[srow+2]); + + fprintf(f,"info->J2a[srow+0] %f ",info->J2a[srow+0]); PRINT_DBL_HEX(info->J2a[srow+0]); + fprintf(f,"info->J2a[srow+1] %f ",info->J2a[srow+1]); PRINT_DBL_HEX(info->J2a[srow+1]); + fprintf(f,"info->J2a[srow+2] %f ",info->J2a[srow+2]); PRINT_DBL_HEX(info->J2a[srow+2]); + } +#endif + + + } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (lostop == histop)) powered = 0; + + if (powered) { + info->cfm[row] = normal_cfm; + + if (! limit) { + info->c[row] = vel; +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"vel %f ",vel); PRINT_DBL_HEX(vel); + //fprintf(f,"c[row] 1 %f ",info->c[row]); PRINT_DBL_HEX(info->c[row]); + } +#endif + + info->lo[row] = -fmax; + info->hi[row] = fmax; + } + else { + // the joint is at a limit, AND is being powered. if the joint is + // being powered into the limit then we apply the maximum motor force + // in that direction, because the motor is working against the + // immovable limit. if the joint is being powered away from the limit + // then we have problems because actually we need *two* lcp + // constraints to handle this case. so we fake it and apply some + // fraction of the maximum force. the fraction to use can be set as + // a fudge factor. + + dReal fm = fmax; + if (vel > 0) fm = -fm; + + // if we're powering away from the limit, apply the fudge factor + if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; + + if (rotational) { + dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], + -fm*ax1[2]); + if (joint->node[1].body) + dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + } + else { + dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); + if (joint->node[1].body) { + dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + + // linear limot torque decoupling step: refer to above discussion + dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + } + } + } + } + + if (limit) { + dReal k = info->fps * stop_erp; + info->c[row] = -k * limit_err; +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[row] 2 %f ",info->c[row]); PRINT_DBL_HEX(info->c[row]); + } +#endif + info->cfm[row] = stop_cfm; + + if (lostop == histop) { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else { + if (limit == 1) { + // low limit + info->lo[row] = 0; + info->hi[row] = dInfinity; + } + else { + // high limit + info->lo[row] = -dInfinity; + info->hi[row] = 0; + } + + // deal with bounce + if (bounce > 0) { + // calculate joint velocity + dReal vel; + if (rotational) { + vel = dDOT(joint->node[0].body->avel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->avel,ax1); + } + else { + vel = dDOT(joint->node[0].body->lvel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->lvel,ax1); + } + + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) { + // low limit + if (vel < 0) { + dReal newc = -bounce * vel; + if (newc > info->c[row]){ + info->c[row] = newc; +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[row] 3 %f ",info->c[row]); PRINT_DBL_HEX(info->c[row]); + } +#endif + } + } + } + else { + // high limit - all those computations are reversed + if (vel > 0) { + dReal newc = -bounce * vel; + if (newc < info->c[row]){ + info->c[row] = newc; +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[row] 4 %f ",info->c[row]); PRINT_DBL_HEX(info->c[row]); + } +#endif + } + } + } + } + } + } + return 1; + } + else return 0; +} + +//**************************************************************************** +// ball and socket + +static void ballInit (dxJointBall *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + j->damping = 0; + j->stiffness = 0; + j->springMode = false; +} + + +static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) +{ + info->m = 3; + info->nub = 3; +} + +static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) +{ + dReal origERP = info->erp; + + + dReal linearERP = 0; + dReal linearCFM = 0; + + if (joint->springMode){ + dReal linearStiffness = joint->stiffness; + dReal linearDamping = joint->damping; + + dReal stepSize = 1.0/info->fps; + if ((linearStiffness < 0.00001) && (linearDamping < 0.00001)){ + linearDamping = 0.00001; + } + linearERP = (linearStiffness * stepSize)/((linearStiffness * stepSize) + linearDamping); + linearCFM = 1.0/(stepSize*linearStiffness + linearDamping); + info->erp = linearERP; + } + + setBall (joint,info,joint->anchor1,joint->anchor2); + + if (joint->springMode){ + info->cfm[0] = linearCFM; + info->cfm[1] = linearCFM; + info->cfm[2] = linearCFM; + } + info->erp = origERP; +} + +extern "C" void dJointSetBallSpringMode (dxJointBall *joint, + int enable) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + joint->springMode = enable; +} +extern "C" void dJointSetBallParam (dxJointBall *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + switch (parameter) { + case dParamStiffness: + joint->stiffness = value; + break; + case dParamDamping: + joint->damping = value; + break; + } + +} + +extern "C" void dJointSetBallAnchor (dxJointBall *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); +} + + +extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +dxJoint::Vtable __dball_vtable = { + sizeof(dxJointBall), + (dxJoint::init_fn*) ballInit, + (dxJoint::getInfo1_fn*) ballGetInfo1, + (dxJoint::getInfo2_fn*) ballGetInfo2, + dJointTypeBall}; + +//**************************************************************************** +// hinge + +static void hingeInit (dxJointHinge *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[0] = 1; + dSetZero (j->qrel,4); + j->limot.init (j->world); +} + + +static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered hinge needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && + j->limot.lostop <= j->limot.histop) { + dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, + j->qrel); + if (j->limot.testRotationalLimit (angle)) info->m = 6; + } +} + + +static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) +{ + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"(hinge joint)\n"); + } +#endif + + + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the two hinge rows. the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body + dVector3 p,q; // plane space vectors for ax1 + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dPlaneSpace (ax1,p,q); + + int s3=3*info->rowskip; + int s4=4*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + info->J1a[s4+0] = q[0]; + info->J1a[s4+1] = q[1]; + info->J1a[s4+2] = q[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + info->J2a[s4+0] = -q[0]; + info->J2a[s4+1] = -q[1]; + info->J2a[s4+2] = -q[2]; + } + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"info->J1a[s3+0] %f ",info->J1a[s3+0]); PRINT_DBL_HEX(info->J1a[s3+0]); + fprintf(f,"info->J1a[s3+1] %f ",info->J1a[s3+1]); PRINT_DBL_HEX(info->J1a[s3+1]); + fprintf(f,"info->J1a[s3+2] %f ",info->J1a[s3+2]); PRINT_DBL_HEX(info->J1a[s3+2]); + fprintf(f,"info->J1a[s4+0] %f ",info->J1a[s4+0]); PRINT_DBL_HEX(info->J1a[s4+0]); + fprintf(f,"info->J1a[s4+1] %f ",info->J1a[s4+1]); PRINT_DBL_HEX(info->J1a[s4+1]); + fprintf(f,"info->J1a[s4+2] %f ",info->J1a[s4+2]); PRINT_DBL_HEX(info->J1a[s4+2]); + + fprintf(f,"info->J2a[s3+0] %f ",info->J2a[s3+0]); PRINT_DBL_HEX(info->J2a[s3+0]); + fprintf(f,"info->J2a[s3+1] %f ",info->J2a[s3+1]); PRINT_DBL_HEX(info->J2a[s3+1]); + fprintf(f,"info->J2a[s3+2] %f ",info->J2a[s3+2]); PRINT_DBL_HEX(info->J2a[s3+2]); + fprintf(f,"info->J2a[s4+0] %f ",info->J2a[s4+0]); PRINT_DBL_HEX(info->J2a[s4+0]); + fprintf(f,"info->J2a[s4+1] %f ",info->J2a[s4+1]); PRINT_DBL_HEX(info->J2a[s4+1]); + fprintf(f,"info->J2a[s4+2] %f ",info->J2a[s4+2]); PRINT_DBL_HEX(info->J2a[s4+2]); + //fprintf(f,"J2a[] %f ",i,J[i]); PRINT_DBL_HEX(J[i]);m + } +#endif + + + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + + dVector3 ax2,b; + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } + dCROSS (b,=,ax1,ax2); + dReal k = info->fps * info->erp; + info->c[3] = k * dDOT(b,p); + info->c[4] = k * dDOT(b,q); +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[3] %f ",info->c[3]); PRINT_DBL_HEX(info->c[3]); + fprintf(f,"c[4] %f ",info->c[4]); PRINT_DBL_HEX(info->c[4]); + } +#endif + + // if the hinge is powered, or has joint limits, add in the stuff + joint->limot.addLimot (joint,info,5,ax1,1); +} + + +// compute initial relative rotation body1 -> body2, or env -> body1 + +static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) +{ + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + } + else { + // set joint->qrel to the transpose of the first body q + joint->qrel[0] = joint->node[0].body->q[0]; + for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + } + } +} + + +extern "C" void dJointSetHingeAnchor (dxJointHinge *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointSetHingeAnchorDelta (dxJointHinge *joint, + dReal x, dReal y, dReal z, + dReal dx, dReal dy, dReal dz) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + + if (joint->node[0].body) { + dReal q[4]; + q[0] = x - joint->node[0].body->pos[0]; + q[1] = y - joint->node[0].body->pos[1]; + q[2] = z - joint->node[0].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (joint->anchor1,joint->node[0].body->R,q); + + if (joint->node[1].body) { + q[0] = x - joint->node[1].body->pos[0]; + q[1] = y - joint->node[1].body->pos[1]; + q[2] = z - joint->node[1].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (joint->anchor2,joint->node[1].body->R,q); + } + else { + // Move the relative displacement between the passive body and the + // anchor in the same direction as the passive body has just moved + joint->anchor2[0] = x + dx; + joint->anchor2[1] = y + dy; + joint->anchor2[2] = z + dz; + } + } + joint->anchor1[3] = 0; + joint->anchor2[3] = 0; + + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointSetHingeAxis (dxJointHinge *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAxes (joint,x,y,z,joint->axis1,joint->axis2); + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointSetHingeParam (dxJointHinge *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + joint->limot.set (parameter,value); +} + + +extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + return joint->limot.get (parameter); +} + + +extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->node[0].body) { + dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, + joint->qrel); + if (joint->flags & dJOINT_REVERSE) + return -ang; + else + return ang; + } + else return 0; +} + + +extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + if (joint->flags & dJOINT_REVERSE) rate = - rate; + return rate; + } + else return 0; +} + + +extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque) +{ + dVector3 axis; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + + if (joint->flags & dJOINT_REVERSE) + torque = -torque; + + getAxis (joint,axis,joint->axis1); + axis[0] *= torque; + axis[1] *= torque; + axis[2] *= torque; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dhinge_vtable = { + sizeof(dxJointHinge), + (dxJoint::init_fn*) hingeInit, + (dxJoint::getInfo1_fn*) hingeGetInfo1, + (dxJoint::getInfo2_fn*) hingeGetInfo2, + dJointTypeHinge}; + +//**************************************************************************** +// slider + +static void sliderInit (dxJointSlider *j) +{ + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->qrel,4); + dSetZero (j->offset,4); + j->limot.init (j->world); +} + + +extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1,q; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + // get body2 + offset point in global coordinates + dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset); + for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - + joint->node[1].body->pos[i]; + } + else { + for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - + joint->offset[i]; + + } + return dDOT(ax1,q); +} + + +extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + return dDOT(ax1,joint->node[0].body->lvel) - + dDOT(ax1,joint->node[1].body->lvel); + } + else { + return dDOT(ax1,joint->node[0].body->lvel); + } +} + + +static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered slider needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + j->limot.limit = 0; + if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && + j->limot.lostop <= j->limot.histop) { + // measure joint position + dReal pos = dJointGetSliderPosition (j); + if (pos <= j->limot.lostop) { + j->limot.limit = 1; + j->limot.limit_err = pos - j->limot.lostop; + info->m = 6; + } + else if (pos >= j->limot.histop) { + j->limot.limit = 2; + j->limot.limit_err = pos - j->limot.histop; + info->m = 6; + } + } +} + + +static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s3=3*s,s4=4*s; + + // pull out pos and R for both bodies. also get the `connection' + // vector pos2-pos1. + + dReal *pos1,*pos2,*R1,*R2; + dVector3 c; + pos1 = joint->node[0].body->pos; + R1 = joint->node[0].body->R; + if (joint->node[1].body) { + pos2 = joint->node[1].body->pos; + R2 = joint->node[1].body->R; + for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; + } + else { + pos2 = 0; + R2 = 0; + } + + // 3 rows to make body rotations equal + setFixedOrientation(joint, info, joint->qrel, 0); + + // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + + dVector3 ax1; // joint axis in global coordinates (unit length) + dVector3 p,q; // plane space of ax1 + dMULTIPLY0_331 (ax1,R1,joint->axis1); + dPlaneSpace (ax1,p,q); + if (joint->node[1].body) { + dVector3 tmp; + dCROSS (tmp, = REAL(0.5) * ,c,p); + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + dCROSS (tmp, = REAL(0.5) * ,c,q); + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; + for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; + } + for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; + for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; + + // compute last two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + dVector3 ofs; // offset point in global coordinates + dMULTIPLY0_331 (ofs,R2,joint->offset); + for (i=0; i<3; i++) c[i] += ofs[i]; + info->c[3] = k * dDOT(p,c); + info->c[4] = k * dDOT(q,c); +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[3] %f ",info->c[3]); PRINT_DBL_HEX(info->c[3]); + fprintf(f,"c[4] %f ",info->c[4]); PRINT_DBL_HEX(info->c[4]); + } +#endif + + } + else { + dVector3 ofs; // offset point in global coordinates + for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; + info->c[3] = k * dDOT(p,ofs); + info->c[4] = k * dDOT(q,ofs); +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[3] %f ",info->c[3]); PRINT_DBL_HEX(info->c[3]); + fprintf(f,"c[4] %f ",info->c[4]); PRINT_DBL_HEX(info->c[4]); + } +#endif + } + + // if the slider is powered, or has joint limits, add in the extra row + joint->limot.addLimot (joint,info,5,ax1,0); +} + + +extern "C" void dJointSetSliderAxis (dxJointSlider *joint, + dReal x, dReal y, dReal z) +{ + int i; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + setAxes (joint,x,y,z,joint->axis1,0); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dVector3 c; + for (i=0; i<3; i++) + c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i]; + } +} + + +extern "C" void dJointSetSliderAxisDelta (dxJointSlider *joint, + dReal x, dReal y, dReal z, + dReal dx, dReal dy, dReal dz) +{ + int i; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + setAxes (joint,x,y,z,joint->axis1,0); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dVector3 c; + for (i=0; i<3; i++) + c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + + for (i=1; i<4; i++) + joint->qrel[i] = -joint->node[0].body->q[i]; + + joint->offset[0] = joint->node[0].body->pos[0] + dx; + joint->offset[1] = joint->node[0].body->pos[1] + dy; + joint->offset[2] = joint->node[0].body->pos[2] + dz; + } +} + + + +extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointSetSliderParam (dxJointSlider *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + joint->limot.set (parameter,value); +} + + +extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + return joint->limot.get (parameter); +} + + +extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force) +{ + dVector3 axis; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + if (joint->flags & dJOINT_REVERSE) + force -= force; + + getAxis (joint,axis,joint->axis1); + axis[0] *= force; + axis[1] *= force; + axis[2] *= force; + + if (joint->node[0].body != 0) + dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); + if (joint->node[1].body != 0) + dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dslider_vtable = { + sizeof(dxJointSlider), + (dxJoint::init_fn*) sliderInit, + (dxJoint::getInfo1_fn*) sliderGetInfo1, + (dxJoint::getInfo2_fn*) sliderGetInfo2, + dJointTypeSlider}; + +//**************************************************************************** +// contact + +static void contactInit (dxJointContact *j) +{ + // default frictionless contact. hmmm, this info gets overwritten straight + // away anyway, so why bother? +#if 0 /* so don't bother ;) */ + j->contact.surface.mode = 0; + j->contact.surface.mu = 0; + dSetZero (j->contact.geom.pos,4); + dSetZero (j->contact.geom.normal,4); + j->contact.geom.depth = 0; +#endif +} + + +static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) +{ + // make sure mu's >= 0, then calculate number of constraint rows and number + // of unbounded rows. + int m = 1, nub=0; + if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; + if (j->contact.surface.mode & dContactMu2) { + if (j->contact.surface.mu > 0) m++; + if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; + if (j->contact.surface.mu2 > 0) m++; + if (j->contact.surface.mu == dInfinity) nub ++; + if (j->contact.surface.mu2 == dInfinity) nub ++; + } + else { + if (j->contact.surface.mu > 0) m += 2; + if (j->contact.surface.mu == dInfinity) nub += 2; + } + + j->the_m = m; + info->m = m; + info->nub = nub; +} + + +static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s2 = 2*s; + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"(contact joint)\n"); + } +#endif + + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + if (j->flags & dJOINT_REVERSE) { + normal[0] = - j->contact.geom.normal[0]; + normal[1] = - j->contact.geom.normal[1]; + normal[2] = - j->contact.geom.normal[2]; + } + else { + normal[0] = j->contact.geom.normal[0]; + normal[1] = j->contact.geom.normal[1]; + normal[2] = j->contact.geom.normal[2]; + } + normal[3] = 0; // @@@ hmmm + + // c1,c2 = contact points with respect to body PORs + dVector3 c1,c2; + for (i=0; i<3; i++){ + c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; + + } + + // set jacobian for normal + info->J1l[0] = normal[0]; + info->J1l[1] = normal[1]; + info->J1l[2] = normal[2]; + dCROSS (info->J1a,=,c1,normal); + + if (j->node[1].body) { + for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - + j->node[1].body->pos[i]; + info->J2l[0] = -normal[0]; + info->J2l[1] = -normal[1]; + info->J2l[2] = -normal[2]; + dCROSS (info->J2a,= -,c2,normal); + } + + // set right hand side and cfm value for normal + dReal erp = info->erp; + if (j->contact.surface.mode & dContactSoftERP) + erp = j->contact.surface.soft_erp; + dReal k = info->fps * erp; + dReal depth = j->contact.geom.depth - j->world->contactp.min_depth; + if (depth < 0) depth = 0; + dReal maxvel = j->world->contactp.max_vel; + + // added by ericf - motion along normal - for simulating moving bodies' velocity along + // the normal using static geoms and such + dReal motionN = 0; + if (j->contact.surface.mode & dContactMotionN){ + motionN = j->contact.surface.motionN; + } + dReal pushout = k*depth + motionN; + if (pushout > maxvel) + info->c[0] = maxvel; + else + info->c[0] = pushout; + +// if (k*depth > maxvel){ +// info->c[0] = maxvel; +// } +// else{ +// info->c[0] = k*depth; +// } + + if (j->contact.surface.mode & dContactSoftCFM) + info->cfm[0] = j->contact.surface.soft_cfm; + + + + + // deal with bounce + if (j->contact.surface.mode & dContactBounce) { + // calculate outgoing velocity (-ve for incoming contact) + dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + + dDOT(info->J1a,j->node[0].body->avel); + if (j->node[1].body) { + outgoing += dDOT(info->J2l,j->node[1].body->lvel) + + dDOT(info->J2a,j->node[1].body->avel); + } + // only apply bounce if the outgoing velocity is greater than the + // threshold, and if the resulting c[0] exceeds what we already have. + if (j->contact.surface.bounce_vel >= 0 && + (-outgoing) > j->contact.surface.bounce_vel) { + //motionN added by ericf + dReal newc = - j->contact.surface.bounce * outgoing + motionN; + if (newc > info->c[0]){ + info->c[0] = newc; + } + } + } + + // set LCP limits for normal + info->lo[0] = 0; + info->hi[0] = dInfinity; + + // now do jacobian for tangential forces + dVector3 t1,t2; // two vectors tangential to normal + + // first friction direction + if (j->the_m >= 2) { + if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? + t1[0] = j->contact.fdir1[0]; + t1[1] = j->contact.fdir1[1]; + t1[2] = j->contact.fdir1[2]; + dCROSS (t2,=,normal,t1); + } + else { + dPlaneSpace (normal,t1,t2); + } + info->J1l[s+0] = t1[0]; + info->J1l[s+1] = t1[1]; + info->J1l[s+2] = t1[2]; + dCROSS (info->J1a+s,=,c1,t1); + + if (j->node[1].body) { + info->J2l[s+0] = -t1[0]; + info->J2l[s+1] = -t1[1]; + info->J2l[s+2] = -t1[2]; + dCROSS (info->J2a+s,= -,c2,t1); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion1) { + info->c[1] = j->contact.surface.motion1; + } + + // set LCP bounds and friction index. this depends on the approximation + // mode + info->lo[1] = -j->contact.surface.mu; + info->hi[1] = j->contact.surface.mu; + if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip1){ + info->cfm[1] = j->contact.surface.slip1; + } + } + + // second friction direction + if (j->the_m >= 3) { + info->J1l[s2+0] = t2[0]; + info->J1l[s2+1] = t2[1]; + info->J1l[s2+2] = t2[2]; + dCROSS (info->J1a+s2,=,c1,t2); + if (j->node[1].body) { + info->J2l[s2+0] = -t2[0]; + info->J2l[s2+1] = -t2[1]; + info->J2l[s2+2] = -t2[2]; + dCROSS (info->J2a+s2,= -,c2,t2); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion2) { + info->c[2] = j->contact.surface.motion2; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + if (j->contact.surface.mode & dContactMu2) { + info->lo[2] = -j->contact.surface.mu2; + info->hi[2] = j->contact.surface.mu2; + } + else { + info->lo[2] = -j->contact.surface.mu; + info->hi[2] = j->contact.surface.mu; + } + if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip2){ + info->cfm[2] = j->contact.surface.slip2; + } + } + +} + +dxJoint::Vtable __dcontact_vtable = { + sizeof(dxJointContact), + (dxJoint::init_fn*) contactInit, + (dxJoint::getInfo1_fn*) contactGetInfo1, + (dxJoint::getInfo2_fn*) contactGetInfo2, + dJointTypeContact}; + +//**************************************************************************** +// hinge 2. note that this joint must be attached to two bodies for it to work + +static dReal measureHinge2Angle (dxJointHinge2 *joint) +{ + dVector3 a1,a2; + dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2); + dMULTIPLY1_331 (a2,joint->node[0].body->R,a1); + dReal x = dDOT(joint->v1,a2); + dReal y = dDOT(joint->v2,a2); + return -dAtan2 (y,x); +} + + +static void hinge2Init (dxJointHinge2 *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + j->c0 = 0; + j->s0 = 0; + + dSetZero (j->v1,4); + j->v1[0] = 1; + dSetZero (j->v2,4); + j->v2[1] = 1; + + j->limot1.init (j->world); + j->limot2.init (j->world); + + j->susp_erp = j->world->global_erp; + j->susp_cfm = j->world->global_cfm; + + j->flags |= dJOINT_TWOBODIES; +} + + +static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) +{ + info->m = 4; + info->nub = 4; + + // see if we're powered or at a joint limit for axis 1 + int atlimit=0; + if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop) { + dReal angle = measureHinge2Angle (j); + if (j->limot1.testRotationalLimit (angle)) atlimit = 1; + } + if (atlimit || j->limot1.fmax > 0) info->m++; + + // see if we're powering axis 2 (we currently never limit this axis) + j->limot2.limit = 0; + if (j->limot2.fmax > 0) info->m++; +} + + +// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are +// relative to body 1 and 2 initially) and then computes the constrained +// rotational axis as the cross product of ax1 and ax2. +// the sin and cos of the angle between axis 1 and 2 is computed, this comes +// from dot and cross product rules. + +#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ + dVector3 ax1,ax2; \ + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \ + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \ + dCROSS (axis,=,ax1,ax2); \ + sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ + cos_angle = dDOT (ax1,ax2); + + +static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) +{ + // get information we need to set the hinge row + dReal s,c; + dVector3 q; + HINGE2_GET_AXIS_INFO (q,s,c); + dNormalize3 (q); // @@@ quicker: divide q by s ? + + // set the three ball-and-socket rows (aligned to the suspension axis ax1) + setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); + + // set the hinge row + int s3=3*info->rowskip; + info->J1a[s3+0] = q[0]; + info->J1a[s3+1] = q[1]; + info->J1a[s3+2] = q[2]; + if (joint->node[1].body) { + info->J2a[s3+0] = -q[0]; + info->J2a[s3+1] = -q[1]; + info->J2a[s3+2] = -q[2]; + } + + // compute the right hand side for the constrained rotational DOF. + // axis 1 and axis 2 are separated by an angle `theta'. the desired + // separation angle is theta0. sin(theta0) and cos(theta0) are recorded + // in the joint structure. the correcting angular velocity is: + // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize + // = (erp*fps) * (theta0-theta) + // (theta0-theta) can be computed using the following small-angle-difference + // approximation: + // theta0-theta ~= tan(theta0-theta) + // = sin(theta0-theta)/cos(theta0-theta) + // = (c*s0 - s*c0) / (c*c0 + s*s0) + // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 + // where c = cos(theta), s = sin(theta) + // c0 = cos(theta0), s0 = sin(theta0) + + dReal k = info->fps * info->erp; + info->c[3] = k * (joint->c0 * s - joint->s0 * c); + + // if the axis1 hinge is powered, or has joint limits, add in more stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the axis2 hinge is powered, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); + + // set parameter for the suspension + info->cfm[0] = joint->susp_cfm; +} + + +// compute vectors v1 and v2 (embedded in body1), used to measure angle +// between body 1 and body 2 + +static void makeHinge2V1andV2 (dxJointHinge2 *joint) +{ + if (joint->node[0].body) { + // get axis 1 and 2 in global coords + dVector3 ax1,ax2,v; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + + // don't do anything if the axis1 or axis2 vectors are zero or the same + if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || + (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || + (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; + + // modify axis 2 so it's perpendicular to axis 1 + dReal k = dDOT(ax1,ax2); + for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; + dNormalize3 (ax2); + + // make v1 = modified axis2, v2 = axis1 x (modified axis2) + dCROSS (v,=,ax1,ax2); + dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2); + dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v); + } +} + + +extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + if (parameter == dParamSuspensionERP) joint->susp_erp = value; + else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; + else joint->limot1.set (parameter,value); + } +} + + +extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1); + } +} + + +extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2); + } +} + + +extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + if (parameter == dParamSuspensionERP) return joint->susp_erp; + else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; + else return joint->limot1.get (parameter); + } +} + + +extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) return measureHinge2Angle (joint); + else return 0; +} + + +extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body && joint->node[1].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2) +{ + dVector3 axis1, axis2; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + + if (joint->node[0].body && joint->node[1].body) { + dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); + } +} + + +dxJoint::Vtable __dhinge2_vtable = { + sizeof(dxJointHinge2), + (dxJoint::init_fn*) hinge2Init, + (dxJoint::getInfo1_fn*) hinge2GetInfo1, + (dxJoint::getInfo2_fn*) hinge2GetInfo2, + dJointTypeHinge2}; + +//**************************************************************************** +// universal + +// I just realized that the universal joint is equivalent to a hinge 2 joint with +// perfectly stiff suspension. By comparing the hinge 2 implementation to +// the universal implementation, you may be able to improve this +// implementation (or, less likely, the hinge2 implementation). + +static void universalInit (dxJointUniversal *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + dSetZero(j->qrel1,4); + dSetZero(j->qrel2,4); + j->limot1.init (j->world); + j->limot2.init (j->world); +} + + +static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) +{ + // This says "ax1 = joint->node[0].body->R * joint->axis1" + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } +} + + +static dReal getUniversalAngle1(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ (R,qcross); + + // This code is essential the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1 (qq,joint->node[0].body->q,qcross); + dQMultiply2 (qrel,qq,joint->qrel1); + + return getHingeAngleFromRelativeQuat(qrel, joint->axis1); + } + return 0; +} + + +static dReal getUniversalAngle2(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + + if (joint->node[1].body) { + dQMultiply1 (qq, joint->node[1].body->q, qcross); + dQMultiply2 (qrel,qq,joint->qrel2); + } + else { + // pretend joint->node[1].body->q is the identity + dQMultiply2 (qrel,qcross, joint->qrel2); + } + + return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); + } + return 0; +} + + +static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) +{ + info->nub = 4; + info->m = 4; + + // see if we're powered or at a joint limit. + bool constraint1 = j->limot1.fmax > 0; + bool constraint2 = j->limot2.fmax > 0; + + bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop; + bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && + j->limot2.lostop <= j->limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + if (limiting1 || limiting2) { + dReal angle1, angle2; + angle1 = getUniversalAngle1(j); + angle2 = getUniversalAngle2(j); + if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; + if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; + } + if (constraint1) + info->m++; + if (constraint2) + info->m++; +} + + +static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) +{ +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"(universal joint)\n"); + } +#endif + + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + + // set the universal joint row. the angular velocity about an axis + // perpendicular to both joint axes should be equal. thus the constraint + // equation is + // p*w1 - p*w2 = 0 + // where p is a vector normal to both joint axes, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dVector3 ax2_temp; + // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate + // about this. + dVector3 p; + dReal k; + + getUniversalAxes(joint, ax1, ax2); + k = dDOT(ax1, ax2); + ax2_temp[0] = ax2[0] - k*ax1[0]; + ax2_temp[1] = ax2[1] - k*ax1[1]; + ax2_temp[2] = ax2[2] - k*ax1[2]; + dCROSS(p, =, ax1, ax2_temp); + dNormalize3(p); + + int s3=3*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + } + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"info->J1a[s3+0] %f ",info->J1a[s3+0]); PRINT_DBL_HEX(info->J1a[s3+0]); + fprintf(f,"info->J1a[s3+1] %f ",info->J1a[s3+1]); PRINT_DBL_HEX(info->J1a[s3+1]); + fprintf(f,"info->J1a[s3+2] %f ",info->J1a[s3+2]); PRINT_DBL_HEX(info->J1a[s3+2]); + + fprintf(f,"info->J2a[s3+0] %f ",info->J2a[s3+0]); PRINT_DBL_HEX(info->J2a[s3+0]); + fprintf(f,"info->J2a[s3+1] %f ",info->J2a[s3+1]); PRINT_DBL_HEX(info->J2a[s3+1]); + fprintf(f,"info->J2a[s3+2] %f ",info->J2a[s3+2]); PRINT_DBL_HEX(info->J2a[s3+2]); + } +#endif + // compute the right hand side of the constraint equation. set relative + // body velocities along p to bring the axes back to perpendicular. + // If ax1, ax2 are unit length joint axes as computed from body1 and + // body2, we need to rotate both bodies along the axis p. If theta + // is the angle between ax1 and ax2, we need an angular velocity + // along p to cover the angle erp * (theta - Pi/2) in one step: + // + // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize + // = (erp*fps) * (theta - Pi/2) + // + // if theta is close to Pi/2, + // theta - Pi/2 ~= cos(theta), so + // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) + + info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"c[3] %f ",info->c[3]); PRINT_DBL_HEX(info->c[3]); + } +#endif + + + // if the first angle is powered, or has joint limits, add in the stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the second angle is powered, or has joint limits, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); +} + + +static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) +{ + if (joint->node[0].body) { + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross; + + getUniversalAxes(joint, ax1, ax2); + + // Axis 1. + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ(R, qcross); + dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); + + // Axis 2. + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + if (joint->node[1].body) { + dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); + } + else { + // set joint->qrel to qcross + for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; + } + } +} + + +extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,NULL,joint->axis2); + else + setAxes (joint,x,y,z,joint->axis1,NULL); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,joint->axis1,NULL); + else + setAxes (joint,x,y,z,NULL,joint->axis2); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,result,joint->axis2); + else + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,result,joint->axis1); + else + getAxis2 (joint,result,joint->axis2); +} + + +extern "C" void dJointSetUniversalParam (dxJointUniversal *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + joint->limot1.set (parameter,value); + } +} + + +extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + return joint->limot1.get (parameter); + } +} + + +extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle2 (joint); + else + return getUniversalAngle1 (joint); +} + + +extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle1 (joint); + else + return getUniversalAngle2 (joint); +} + + +extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,axis,joint->axis2); + else + getAxis (joint,axis,joint->axis1); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,axis,joint->axis1); + else + getAxis2 (joint,axis,joint->axis2); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2) +{ + dVector3 axis1, axis2; + dAASSERT(joint); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->flags & dJOINT_REVERSE) { + dReal temp = torque1; + torque1 = - torque2; + torque2 = - temp; + } + + getAxis (joint,axis1,joint->axis1); + getAxis2 (joint,axis2,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); +} + + + + + +dxJoint::Vtable __duniversal_vtable = { + sizeof(dxJointUniversal), + (dxJoint::init_fn*) universalInit, + (dxJoint::getInfo1_fn*) universalGetInfo1, + (dxJoint::getInfo2_fn*) universalGetInfo2, + dJointTypeUniversal}; + +//**************************************************************************** +// angular motor + +static void amotorInit (dxJointAMotor *j) +{ + int i; + j->num = 0; + j->mode = dAMotorUser; + for (i=0; i<3; i++) { + j->rel[i] = 0; + dSetZero (j->axis[i],4); + j->limot[i].init (j->world); + j->angle[i] = 0; + } + dSetZero (j->reference1,4); + dSetZero (j->reference2,4); +} + + +// compute the 3 axes in global coordinates + +static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) +{ + if (joint->mode == dAMotorEuler) { + // special handling for euler mode + dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]); + if (joint->node[1].body) { + dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]); + } + else { + ax[2][0] = joint->axis[2][0]; + ax[2][1] = joint->axis[2][1]; + ax[2][2] = joint->axis[2][2]; + } + dCROSS (ax[1],=,ax[2],ax[0]); + dNormalize3 (ax[1]); + } + else { + for (int i=0; i < joint->num; i++) { + if (joint->rel[i] == 1) { + // relative to b1 + dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); + } + else if (joint->rel[i] == 2) { + // relative to b2 + if (joint->node[1].body) { // jds: don't assert, just ignore + dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); + } + } + else { + // global - just copy it + ax[i][0] = joint->axis[i][0]; + ax[i][1] = joint->axis[i][1]; + ax[i][2] = joint->axis[i][2]; + } + } + } +} + + +static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) +{ + // assumptions: + // global axes already calculated --> ax + // axis[0] is relative to body 1 --> global ax[0] + // axis[2] is relative to body 2 --> global ax[2] + // ax[1] = ax[2] x ax[0] + // original ax[0] and ax[2] are perpendicular + // reference1 is perpendicular to ax[0] (in body 1 frame) + // reference2 is perpendicular to ax[2] (in body 2 frame) + // all ax[] and reference vectors are unit length + + // calculate references in global frame + dVector3 ref1,ref2; + dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1); + if (joint->node[1].body) { + dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2); + } + else { + ref2[0] = joint->reference2[0]; + ref2[1] = joint->reference2[1]; + ref2[2] = joint->reference2[2]; + } + + // get q perpendicular to both ax[0] and ref1, get first euler angle + dVector3 q; + dCROSS (q,=,ax[0],ref1); + joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); + + // get q perpendicular to both ax[0] and ax[1], get second euler angle + dCROSS (q,=,ax[0],ax[1]); + joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); + + // get q perpendicular to both ax[1] and ax[2], get third euler angle + dCROSS (q,=,ax[1],ax[2]); + joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); +} + + +// set the reference vectors as follows: +// * reference1 = current axis[2] relative to body 1 +// * reference2 = current axis[0] relative to body 2 +// this assumes that: +// * axis[0] is relative to body 1 +// * axis[2] is relative to body 2 + +static void amotorSetEulerReferenceVectors (dxJointAMotor *j) +{ + if (j->node[0].body && j->node[1].body) { + dVector3 r; // axis[2] and axis[0] in global coordinates + dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]); + dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); + dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); + dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r); + } + + else { // jds + // else if (j->node[0].body) { + // dMULTIPLY1_331 (j->reference1,j->node[0].body->R,j->axis[2]); + // dMULTIPLY0_331 (j->reference2,j->node[0].body->R,j->axis[0]); + + // We want to handle angular motors attached to passive geoms + dVector3 r; // axis[2] and axis[0] in global coordinates + r[0] = j->axis[2][0]; r[1] = j->axis[2][1]; r[2] = j->axis[2][2]; r[3] = j->axis[2][3]; + dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); + dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); + j->reference2[0] += r[0]; j->reference2[1] += r[1]; + j->reference2[2] += r[2]; j->reference2[3] += r[3]; + } +} + + +static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; + + // compute the axes and angles, if in euler mode + if (j->mode == dAMotorEuler) { + dVector3 ax[3]; + amotorComputeGlobalAxes (j,ax); + amotorComputeEulerAngles (j,ax); + } + + // see if we're powered or at a joint limit for each axis + for (int i=0; i < j->num; i++) { + if (j->limot[i].testRotationalLimit (j->angle[i]) || + j->limot[i].fmax > 0) { + info->m++; + } + } +} + + +static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) +{ + int i; + +#if VALUE_TESTING + if (testLogging){ + fprintf(f,"(amotor joint)\n"); + } +#endif + + + // compute the axes (if not global) + dVector3 ax[3]; + amotorComputeGlobalAxes (joint,ax); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + dVector3 *axptr[3]; + axptr[0] = &ax[0]; + axptr[1] = &ax[1]; + axptr[2] = &ax[2]; + + dVector3 ax0_cross_ax1; + dVector3 ax1_cross_ax2; + if (joint->mode == dAMotorEuler) { + dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); + axptr[2] = &ax0_cross_ax1; + dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); + axptr[0] = &ax1_cross_ax2; + } + + int row=0; + for (i=0; i < joint->num; i++) { + row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); + } +} + + +extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num) +{ + dAASSERT(joint && num >= 0 && num <= 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorEuler) { + joint->num = 3; + } + else { + if (num < 0) num = 0; + if (num > 3) num = 3; + joint->num = num; + } +} + + +extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, + dReal x, dReal y, dReal z) +{ + dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + dUASSERT(!(!joint->node[1].body && (joint->flags & dJOINT_REVERSE) && rel == 1),"no first body, can't set axis rel=1"); + dUASSERT(!(!joint->node[1].body && !(joint->flags & dJOINT_REVERSE) && rel == 2),"no second body, can't set axis rel=2"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + + // adjust rel to match the internal body order + if (!joint->node[1].body && rel==2) rel = 1; + + joint->rel[anum] = rel; + + // x,y,z is always in global coordinates regardless of rel, so we may have + // to convert it to be relative to a body + dVector3 r; + r[0] = x; + r[1] = y; + r[2] = z; + r[3] = 0; + if (rel > 0) { + if (rel==1) { + dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r); + } + else { + // don't assert; handle the case of attachment to a bodiless geom + if (joint->node[1].body) { // jds + dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r); + } + else { + joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3]; + } + } + } + else { + joint->axis[anum][0] = r[0]; + joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; + } + dNormalize3 (joint->axis[anum]); + if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); +} + + +extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum, + dReal angle) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorUser) { + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + joint->angle[anum] = angle; + } +} + + +extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter, + dReal value) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + joint->limot[anum].set (parameter, value); +} + + +extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + joint->mode = mode; + if (joint->mode == dAMotorEuler) { + joint->num = 3; + amotorSetEulerReferenceVectors (joint); + } +} + + +extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->num; +} + + +extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, + dVector3 result) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + if (joint->rel[anum] > 0) { + if (joint->rel[anum]==1) { + dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]); + } + else { + if (joint->node[1].body) { // jds + dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]); + } + else { + result[0] = joint->axis[anum][0]; result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; result[3] = joint->axis[anum][3]; + } + } + } + else { + result[0] = joint->axis[anum][0]; + result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; + } +} + + +extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + return joint->rel[anum]; +} + + +extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + + // compute the axes and angles, if in euler mode + if (joint->mode == dAMotorEuler) { + dVector3 ax[3]; + amotorComputeGlobalAxes (joint,ax); + amotorComputeEulerAngles (joint,ax); + } + + return joint->angle[anum]; +} + + +extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum) +{ + // @@@ + dDebug (0,"not yet implemented"); + return 0; +} + + +extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + return joint->limot[anum].get (parameter); +} + + +extern "C" int dJointGetAMotorMode (dxJointAMotor *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->mode; +} + + +extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3) +{ + dVector3 axes[3]; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + + if (joint->num == 0) + return; + dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); + + amotorComputeGlobalAxes (joint,axes); + axes[0][0] *= torque1; + axes[0][1] *= torque1; + axes[0][2] *= torque1; + if (joint->num >= 2) { + axes[0][0] += axes[1][0] * torque2; + axes[0][1] += axes[1][0] * torque2; + axes[0][2] += axes[1][0] * torque2; + if (joint->num >= 3) { + axes[0][0] += axes[2][0] * torque3; + axes[0][1] += axes[2][0] * torque3; + axes[0][2] += axes[2][0] * torque3; + } + } + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); +} + + +dxJoint::Vtable __damotor_vtable = { + sizeof(dxJointAMotor), + (dxJoint::init_fn*) amotorInit, + (dxJoint::getInfo1_fn*) amotorGetInfo1, + (dxJoint::getInfo2_fn*) amotorGetInfo2, + dJointTypeAMotor}; + +//**************************************************************************** +// fixed joint + +static void fixedInit (dxJointFixed *j) +{ + dSetZero (j->offset,4); + dSetZero (j->qrel,4); + dSetZero (j->anchor1,3); + dSetZero (j->anchor2,3); + j->linearStiffness = 0.0; + j->linearDamping = 0.0; + j->angularStiffness = 0.0; + j->angularDamping = 0.0; + j->active = true; + j->linearSpringMode = false; + j->angularSpringMode = false; + j->customAnchorPoint = false; +} + + +static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) +{ + if (j->active){ + info->m = 6; + info->nub = 6; + } + else{ + info->m = 0; + info->nub = 0; + } +} + + +static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) +{ + if (!joint->active) + return; + + dReal origERP = info->erp; + + int s = info->rowskip; + + dReal linearStiffness = joint->linearStiffness; + dReal linearDamping = joint->linearDamping; + + dReal rotateStiffness = joint->angularStiffness; + dReal rotateDamping = joint->angularDamping; + + dReal stepSize = 1.0/info->fps; + + dReal linearERP = 0; + dReal linearCFM = 0; + dReal rotateERP = 0; + dReal rotateCFM = 0; + + if (joint->linearSpringMode){ + //printf("LSP %f %f\n",linearStiffness,linearDamping); + if ((linearStiffness < 0.00001) && (linearDamping < 0.00001)){ + linearDamping = 0.00001; + } + linearERP = (linearStiffness * stepSize)/((linearStiffness * stepSize) + linearDamping); + linearCFM = 1.0/(stepSize*linearStiffness + linearDamping); + } + if (joint->angularSpringMode){ + //printf("ASP %f %f\n",rotateStiffness,rotateDamping); + if ((rotateStiffness < 0.00001) && (rotateDamping < 0.00001)){ + rotateDamping = 0.00001; + } + rotateERP = (rotateStiffness * stepSize)/((rotateStiffness * stepSize) + rotateDamping); + rotateCFM = 1.0/(stepSize*rotateStiffness + rotateDamping); + info->erp = rotateERP; + } + + // Three rows for orientation + setFixedOrientation(joint, info, joint->qrel, 3); + + if (joint->linearSpringMode){ + info->erp = linearERP; + } + + + // we worry about our anchor point + if (joint->customAnchorPoint){ + setBall(joint,info,joint->anchor1,joint->anchor2); + } + //original fixed code - just use the offset between the two + // (the anchor is effectively at one of the bodies) + else{ + // Three rows for position. + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + dVector3 ofs; + dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); + if (joint->node[1].body) { + dCROSSMAT (info->J1a,ofs,s,+,-); + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + } + + // set right hand side for the first three rows (linear) + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->node[1].body->pos[j] - + joint->node[0].body->pos[j] + ofs[j]); + } + else { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); + } + } + + if (joint->linearSpringMode){ + info->cfm[0] = linearCFM; + info->cfm[1] = linearCFM; + info->cfm[2] = linearCFM; + } + if (joint->angularSpringMode){ + info->cfm[3] = rotateCFM; + info->cfm[4] = rotateCFM; + info->cfm[5] = rotateCFM; + } + + info->erp = origERP; +} + + +extern "C" void dJointSetFixed (dxJointFixed *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); + int i; + + // This code is taken from sJointSetSliderAxis(), we should really put the + // common code in its own function. + // compute the offset between the bodies + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dReal ofs[4]; + for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; + for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; + } + for (i=0; i<4; i++) joint->qrelOrig[i] = joint->qrel[i]; + } +} + + +//if abs is true, the second anchor is set to abs coords +extern "C" void dJointSetFixedAnchor (dxJointFixed *joint, + dReal x, dReal y, dReal z, int abs) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed"); + if (abs) + setAnchorsAbs(joint,x,y,z,joint->anchor1,joint->anchor2); + else + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); +} + +extern "C" void dJointSetFixedAnchorRotation (dxJointFixed *joint, const dQuaternion q) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed"); + //cout << "would set rot" << endl; +// dQuaternion q; +// dRtoQ(R,q); + +// for (int i=1; i<4; i++) q[i] = -q[i]; + //dQMultiply0(joint->qrel,joint->qrelOrig,q); + //dQuaternion q2; + //for (int i = 0; i < 4; i++) q2[i] = joint->qrelOrig[i]; + //dQMultiply0 (q2,joint->qrelOrig,joint->node[0].body->q); + dQMultiply0 (joint->qrel,joint->qrelOrig,q); + //for (int i = 0; i < 4; i++)joint->qrel[i] = q2[i]; +} + + +extern "C" void dJointSetFixedSpringMode (dxJointFixed *joint, + int linearEnable, + int angularEnable, + int customAnchorPointEnable) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not a fixed"); + joint->linearSpringMode = linearEnable; + joint->angularSpringMode = angularEnable; + joint->customAnchorPoint = customAnchorPointEnable; +} + +extern "C" void dJointSetFixedParam (dxJointFixed *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); + switch (parameter) { + case dParamActive: + joint->active = value; + break; + case dParamLinearStiffness: + joint->linearStiffness = value; + break; + case dParamLinearDamping: + joint->linearDamping = value; + break; + case dParamAngularStiffness: + joint->angularStiffness = value; + break; + case dParamAngularDamping: + joint->angularDamping = value; + break; + } +} + + +dxJoint::Vtable __dfixed_vtable = { + sizeof(dxJointFixed), + (dxJoint::init_fn*) fixedInit, + (dxJoint::getInfo1_fn*) fixedGetInfo1, + (dxJoint::getInfo2_fn*) fixedGetInfo2, + dJointTypeFixed}; + +//**************************************************************************** +// null joint + +static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; +} + + +static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) +{ + dDebug (0,"this should never get called"); +} + + +dxJoint::Vtable __dnull_vtable = { + sizeof(dxJointNull), + (dxJoint::init_fn*) 0, + (dxJoint::getInfo1_fn*) nullGetInfo1, + (dxJoint::getInfo2_fn*) nullGetInfo2, + dJointTypeNull}; diff --git a/src/external/open_dynamics_engine-ef/ode/ode_joint.h b/src/external/open_dynamics_engine-ef/ode/ode_joint.h new file mode 100644 index 00000000..9eec9a84 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_joint.h @@ -0,0 +1,295 @@ +/************************************************************************* + * * + * 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_JOINT_H_ +#define _ODE_JOINT_H_ + + +#include "ode/ode_objects_private.h" +#include "ode/ode_contact.h" +#include "ode/ode_obstack.h" + + +// joint flags +enum { + // if this flag is set, the joint was allocated in a joint group + dJOINT_INGROUP = 1, + + // if this flag is set, the joint was attached with arguments (0,body). + // our convention is to treat all attaches as (body,0), i.e. so node[0].body + // is always nonzero, so this flag records the fact that the arguments were + // swapped. + dJOINT_REVERSE = 2, + + // if this flag is set, the joint can not have just one body attached to it, + // it must have either zero or two bodies attached. + dJOINT_TWOBODIES = 4 +}; + + +// there are two of these nodes in the joint, one for each connection to a +// body. these are node of a linked list kept by each body of it's connecting +// joints. but note that the body pointer in each node points to the body that +// makes use of the *other* node, not this node. this trick makes it a bit +// easier to traverse the body/joint graph. + +struct dxJointNode { + dxJoint *joint; // pointer to enclosing dxJoint object + dxBody *body; // *other* body this joint is connected to + dxJointNode *next; // next node in body's list of connected joints +}; + + +// ericf temp +struct dxJointFixed; +struct dxJointContact; +struct dxJointAMotor; +struct JointFixedEF; + +struct dxJoint : public dObject { + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + struct Info1 { + int m,nub; + }; + + // info returned by getInfo2 function + + struct Info2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + dReal fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + dReal *J1l,*J1a,*J2l,*J2a; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + dReal *c,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + dReal *lo,*hi; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + // virtual function table: size of the joint structure, function pointers. + // we do it this way instead of using C++ virtual functions because + // sometimes we need to allocate joints ourself within a memory pool. + + typedef void init_fn (dxJoint *joint); + typedef void getInfo1_fn (dxJoint *joint, Info1 *info); + typedef void getInfo2_fn (dxJoint *joint, Info2 *info); + struct Vtable { + int size; + init_fn *init; + getInfo1_fn *getInfo1; + getInfo2_fn *getInfo2; + int typenum; // a dJointTypeXXX type number + }; + + Vtable *vtable; // virtual function table + int flags; // dJOINT_xxx flags + dxJointNode node[2]; // connections to bodies. node[1].body can be 0 + dJointFeedback *feedback; // optional feedback structure + dReal lambda[6]; // lambda generated by last step + + // TEMP ericf + #ifndef dNODEBUG + dxJointFixed *jFixed; + dxJointContact *jContact; + dxJointAMotor *jMotor; + JointFixedEF *jFixedEricf; + #endif +}; + + +// joint group. NOTE: any joints in the group that have their world destroyed +// will have their world pointer set to 0. + +struct dxJointGroup : public dBase { + int num; // number of joints on the stack + dObStack stack; // a stack of (possibly differently sized) dxJoint +}; // objects. + + +// common limit and motor information for a single joint axis of movement +struct dxJointLimitMotor { + dReal vel,fmax; // powered joint: velocity, max force + dReal lostop,histop; // joint limits, relative to initial position + dReal fudge_factor; // when powering away from joint limits + dReal normal_cfm; // cfm to use when not at a stop + dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit + dReal bounce; // restitution factor + // variables used between getInfo1() and getInfo2() + int limit; // 0=free, 1=at lo limit, 2=at hi limit + dReal limit_err; // if at limit, amount over limit + + void init (dxWorld *); + void set (int num, dReal value); + dReal get (int num); + int testRotationalLimit (dReal angle); + int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational); +}; + + +// ball and socket + +struct dxJointBall : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dReal stiffness; + dReal damping; + int springMode; +}; +extern struct dxJoint::Vtable __dball_vtable; + + +// hinge + +struct dxJointHinge : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dhinge_vtable; + + +// universal + +struct dxJointUniversal : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece + dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 + dxJointLimitMotor limot1; // limit and motor information for axis1 + dxJointLimitMotor limot2; // limit and motor information for axis2 +}; +extern struct dxJoint::Vtable __duniversal_vtable; + + +// slider. if body2 is 0 then qrel is the absolute rotation of body1 and +// offset is the position of body1 center along axis1. + +struct dxJointSlider : public dxJoint { + dVector3 axis1; // axis w.r.t first body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // point relative to body2 that should be + // aligned with body1 center along axis1 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dslider_vtable; + + +// contact + +struct dxJointContact : public dxJoint { + int the_m; // number of rows computed by getInfo1 + dContact contact; +}; +extern struct dxJoint::Vtable __dcontact_vtable; + + +// hinge 2 + +struct dxJointHinge2 : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis 1 w.r.t first body + dVector3 axis2; // axis 2 w.r.t second body + dReal c0,s0; // cos,sin of desired angle between axis 1,2 + dVector3 v1,v2; // angle ref vectors embedded in first body + dxJointLimitMotor limot1; // limit+motor info for axis 1 + dxJointLimitMotor limot2; // limit+motor info for axis 2 + dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) +}; +extern struct dxJoint::Vtable __dhinge2_vtable; + + +// angular motor + +struct dxJointAMotor : public dxJoint { + int num; // number of axes (0..3) + int mode; // a dAMotorXXX constant + int rel[3]; // what the axes are relative to (global,b1,b2) + dVector3 axis[3]; // three axes + dxJointLimitMotor limot[3]; // limit+motor info for axes + dReal angle[3]; // user-supplied angles for axes + // these vectors are used for calculating euler angles + dVector3 reference1; // original axis[2], relative to body 1 + dVector3 reference2; // original axis[0], relative to body 2 +}; +extern struct dxJoint::Vtable __damotor_vtable; + + +// fixed + +struct dxJointFixed : public dxJoint { + dQuaternion qrel; // relative rotation body1 -> body2 + dQuaternion qrelOrig; // initial relative rotation used for single body constrains that get modified + dVector3 offset; // relative offset between the bodies + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dReal linearStiffness; //stiffness when in spring mode + dReal linearDamping; //damping when in spring mode + dReal angularStiffness; //stiffness when in spring mode + dReal angularDamping; //damping when in spring mode + bool active; //whether the joint works at all + bool linearSpringMode; + bool angularSpringMode; + bool customAnchorPoint; +}; +extern struct dxJoint::Vtable __dfixed_vtable; + + +// null joint, for testing only + +struct dxJointNull : public dxJoint { +}; +extern struct dxJoint::Vtable __dnull_vtable; + + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_lcp.cpp b/src/external/open_dynamics_engine-ef/ode/ode_lcp.cpp new file mode 100644 index 00000000..798f473b --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_lcp.cpp @@ -0,0 +1,2020 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + + +THE ALGORITHM +------------- + +solve A*x = b+w, with x and w subject to certain LCP conditions. +each x(i),w(i) must lie on one of the three line segments in the following +diagram. each line segment corresponds to one index set : + + w(i) + /|\ | : + | | : + | |i in N : + w>0 | |state[i]=0 : + | | : + | | : i in C + w=0 + +-----------------------+ + | : | + | : | + w<0 | : |i in N + | : |state[i]=1 + | : | + | : | + +-------|-----------|-----------|----------> x(i) + lo 0 hi + +the Dantzig algorithm proceeds as follows: + for i=1:n + * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or + negative towards the line. as this is done, the other (x(j),w(j)) + for j= 0. this makes the algorithm a bit +simpler, because the starting point for x(i),w(i) is always on the dotted +line x=0 and x will only ever increase in one direction, so it can only hit +two out of the three line segments. + + +NOTES +----- + +this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m". +the implementation is split into an LCP problem object (dLCP) and an LCP +driver function. most optimization occurs in the dLCP object. + +a naive implementation of the algorithm requires either a lot of data motion +or a lot of permutation-array lookup, because we are constantly re-ordering +rows and columns. to avoid this and make a more optimized algorithm, a +non-trivial data structure is used to represent the matrix A (this is +implemented in the fast version of the dLCP object). + +during execution of this algorithm, some indexes in A are clamped (set C), +some are non-clamped (set N), and some are "don't care" (where x=0). +A,x,b,w (and other problem vectors) are permuted such that the clamped +indexes are first, the unclamped indexes are next, and the don't-care +indexes are last. this permutation is recorded in the array `p'. +initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped, +the corresponding elements of p are swapped. + +because the C and N elements are grouped together in the rows of A, we can do +lots of work with a fast dot product function. if A,x,etc were not permuted +and we only had a permutation array, then those dot products would be much +slower as we would have a permutation array lookup in some inner loops. + +A is accessed through an array of row pointers, so that element (i,j) of the +permuted matrix is A[i][j]. this makes row swapping fast. for column swapping +we still have to actually move the data. + +during execution of this algorithm we maintain an L*D*L' factorization of +the clamped submatrix of A (call it `AC') which is the top left nC*nC +submatrix of A. there are two ways we could arrange the rows/columns in AC. + +(1) AC is always permuted such that L*D*L' = AC. this causes a problem + when a row/column is removed from C, because then all the rows/columns of A + between the deleted index and the end of C need to be rotated downward. + this results in a lot of data motion and slows things down. +(2) L*D*L' is actually a factorization of a *permutation* of AC (which is + itself a permutation of the underlying A). this is what we do - the + permutation is recorded in the vector C. call this permutation A[C,C]. + when a row/column is removed from C, all we have to do is swap two + rows/columns and manipulate C. + +*/ + +#include "ode/ode_common.h" +#include "ode/ode_lcp.h" +#include "ode/ode_matrix.h" +#include "ode/ode_misc.h" +#include "ode/ode_mat.h" // for testing +#include "ode/ode_timer.h" // for testing + +//*************************************************************************** +// code generation parameters + +// LCP debugging (mosty for fast dLCP) - this slows things down a lot +//#define DEBUG_LCP + +//#define dLCP_SLOW // use slow dLCP object +#define dLCP_FAST // use fast dLCP object + +// option 1 : matrix row pointers (less data copying) +#define ROWPTRS +#define ATYPE dReal ** +#define AROW(i) (A[i]) + +// option 2 : no matrix row pointers (slightly faster inner loops) +//#define NOROWPTRS +//#define ATYPE dReal * +//#define AROW(i) (A+(i)*nskip) + +// use protected, non-stack memory allocation system + +#ifdef dUSE_MALLOC_FOR_ALLOCA +extern unsigned int dMemoryFlag; + +#define ALLOCA(t,v,s) t* v = (t*) malloc(s) +#define UNALLOCA(t) free(t) + +#else + +#define ALLOCA(t,v,s) t* v =(t*)dALLOCA16(s) +#define UNALLOCA(t) /* nothing */ + +#endif + +//#define dDot myDot +#define NUB_OPTIMIZATIONS + +//*************************************************************************** + +// an alternative inline dot product, for speed comparisons + +#if 0 +static inline dReal myDot (dReal *a, dReal *b, int n) +{ + dReal sum=0; + while (n > 0) { + sum += (*a) * (*b); + a++; + b++; + n--; + } + return sum; +} +#endif // 0 + + +// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of +// A is nskip. this only references and swaps the lower triangle. +// if `do_fast_row_swaps' is nonzero and row pointers are being used, then +// rows will be swapped by exchanging row pointers. otherwise the data will +// be copied. + +static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip, + int do_fast_row_swaps) +{ + int i; + dAASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && + nskip >= n && i1 < i2); + +# ifdef ROWPTRS + for (i=i1+1; i 0) { + memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal)); + memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal)); + memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal)); + } + for (i=i1+1; i0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && + i1 <= i2); + if (i1==i2) return; + swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) + return; +#endif + tmp = x[i1]; + x[i1] = x[i2]; + x[i2] = tmp; + tmp = b[i1]; + b[i1] = b[i2]; + b[i2] = tmp; + tmp = w[i1]; + w[i1] = w[i2]; + w[i2] = tmp; + tmp = lo[i1]; + lo[i1] = lo[i2]; + lo[i2] = tmp; + tmp = hi[i1]; + hi[i1] = hi[i2]; + hi[i2] = tmp; + tmpi = p[i1]; + p[i1] = p[i2]; + p[i2] = tmpi; + tmpi = state[i1]; + state[i1] = state[i2]; + state[i2] = tmpi; + if (findex) { + tmpi = findex[i1]; + findex[i1] = findex[i2]; + findex[i2] = tmpi; + } +} + + +// for debugging - check that L,d is the factorization of A[C,C]. +// A[C,C] has size nC*nC and leading dimension nskip. +// L has size nC*nC and leading dimension nskip. +// d has size nC. + +#ifdef DEBUG_LCP + +static void checkFactorization (ATYPE A, dReal *_L, dReal *_d, + int nC, int *C, int nskip) +{ + int i,j; + if (nC==0) return; + + // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C] + dMatrix A1 (nC,nC); + for (i=0; i 1e-8) + dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff); +} + +#endif + + +// for debugging + +#ifdef DEBUG_LCP + +static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C) +{ + int j,k; + dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n); + for (k=0; k= 0 && p[k] < i); + for (k=i; k C,N; // index sets + int last_i_for_solve1; // last i value given to solve1 + + dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, + dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, + dReal *_Dell, dReal *_ell, dReal *_tmp, + int *_state, int *_findex, int *_p, int *_C, dReal **Arows); + // the constructor is given an initial problem description (A,x,b,w) and + // space for other working data (which the caller may allocate on the stack). + // some of this data is specific to the fast dLCP implementation. + // the matrices A and L have size n*n, vectors have size n*1. + // A represents a symmetric matrix but only the lower triangle is valid. + // `nub' is the number of unbounded indexes at the start. all the indexes + // 0..nub-1 will be put into C. + + ~dLCP(); + + int getNub() { return nub; } + // return the value of `nub'. the constructor may want to change it, + // so the caller should find out its new value. + + // transfer functions: transfer index i to the given set (C or N). indexes + // less than `nub' can never be given. A,x,b,w,etc may be permuted by these + // functions, the caller must be robust to this. + + void transfer_i_to_C (int i); + // this assumes C and N span 1:i-1. this also assumes that solve1() has + // been recently called for the same i without any other transfer + // functions in between (thereby allowing some data reuse for the fast + // implementation). + void transfer_i_to_N (int i); + // this assumes C and N span 1:i-1. + void transfer_i_from_N_to_C (int i); + void transfer_i_from_C_to_N (int i); + + int numC(); + int numN(); + // return the number of indexes in set C/N + + int indexC (int i); + int indexN (int i); + // return index i in set C/N. + + // accessor and arithmetic functions. Aij translates as A(i,j), etc. + // make sure that only the lower triangle of A is ever referenced. + + dReal Aii (int i); + dReal AiC_times_qC (int i, dReal *q); + dReal AiN_times_qN (int i, dReal *q); // for all Nj + void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj + void pN_plusequals_ANi (dReal *p, int i, int sign=1); + // for all Nj. sign = +1,-1. assumes i > maximum index in N. + void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q); + void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj + void solve1 (dReal *a, int i, int dir=1, int only_transfer=0); + // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1. + // the fast version of this function computes some data that is needed by + // transfer_i_to_C(). if only_transfer is nonzero then this function + // *only* computes that data, it does not set a(C). + + void unpermute(); + // call this at the end of the LCP function. if the x/w values have been + // permuted then this will unscramble them. +}; + + +dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, + dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, + dReal *_Dell, dReal *_ell, dReal *_tmp, + int *_state, int *_findex, int *_p, int *_C, dReal **Arows) +{ + dUASSERT (_findex==0,"slow dLCP object does not support findex array"); + + n = _n; + nub = _nub; + Adata = _Adata; + A = 0; + x = _x; + b = _b; + w = _w; + lo = _lo; + hi = _hi; + nskip = dPAD(n); + dSetZero (x,n); + last_i_for_solve1 = -1; + + int i,j; + C.setSize (n); + N.setSize (n); + for (int i=0; i0, put all indexes 0..nub-1 into C and solve for x + if (nub > 0) { + for (i=0; i= i) dDebug (0,"N assumption violated"); + if (sign > 0) { + for (k=0; k 0) { + for (ii=0; ii nub + if (nub < n) { + for (k=0; k<100; k++) { + int i1,i2; + do { + i1 = dRandInt(n-nub)+nub; + i2 = dRandInt(n-nub)+nub; + } + while (i1 > i2); + //printf ("--> %d %d\n",i1,i2); + swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0); + } + } + */ + + // permute the problem so that *all* the unbounded variables are at the + // start, i.e. look for unbounded variables not included in `nub'. we can + // potentially push up `nub' this way and get a bigger initial factorization. + // note that when we swap rows/cols here we must not just swap row pointers, + // as the initial factorization relies on the data being all in one chunk. + // variables that have findex >= 0 are *not* considered to be unbounded even + // if lo=-inf and hi=inf - this is because these limits may change during the + // solution process. + + for (k=nub; k= 0) continue; + if (lo[k]==-dInfinity && hi[k]==dInfinity) { + swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0); + nub++; + } + } + + // if there are unbounded variables at the start, factorize A up to that + // point and solve for x. this puts all indexes 0..nub-1 into C. + if (nub > 0) { + for (k=0; k nub such that all findex variables are at the end + if (findex) { + int num_at_end = 0; + for (k=n-1; k >= nub; k--) { + if (findex[k] >= 0) { + swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1); + num_at_end++; + } + } + } + + // print info about indexes + /* + for (k=0; k 0) { + // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) + for (j=0; j 0) { + dReal *aptr = AROW(i); +# ifdef NUB_OPTIMIZATIONS + // if nub>0, initial part of aptr unpermuted + for (j=0; j 0) { + for (int i=0; i 0) { + dReal *aptr = AROW(i); +# ifdef NUB_OPTIMIZATIONS + // if nub>0, initial part of aptr[] is guaranteed unpermuted + for (j=0; j 0) { + for (j=0; j0 && A && x && b && w && nub == 0); + + int i,k; + int nskip = dPAD(n); + ALLOCA (dReal,L,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (L == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,d,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (d == NULL) { + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_x,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_x == NULL) { + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_w,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_w == NULL) { + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,Dell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Dell == NULL) { + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,ell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ell == NULL) { + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,tmp,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (tmp == NULL) { + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal*,Arows,n*sizeof(dReal*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Arows == NULL) { + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,p,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (p == NULL) { + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,C,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (C == NULL) { + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,dummy,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dummy == NULL) { + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + + dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows); + nub = lcp.getNub(); + + for (i=0; i= 0) { + lcp.transfer_i_to_N (i); + } + else { + for (;;) { + // compute: delta_x(C) = -A(C,C)\A(C,i) + dSetZero (delta_x,n); + lcp.solve1 (delta_x,i); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + UNALLOCA(dummy); + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(tmp); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + return; + } +#endif + delta_x[i] = 1; + + // compute: delta_w = A*delta_x + dSetZero (delta_w,n); + lcp.pN_equals_ANC_times_qC (delta_w,delta_x); + lcp.pN_plusequals_ANi (delta_w,i); + delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i); + + // find index to switch + int si = i; // si = switch index + int si_in_N = 0; // set to 1 if si in N + dReal s = -w[i]/delta_w[i]; + + if (s <= 0) { + dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); + if (i < (n-1)) { + dSetZero (x+i,n-i); + dSetZero (w+i,n-i); + } + goto done; + } + + for (k=0; k < lcp.numN(); k++) { + if (delta_w[lcp.indexN(k)] < 0) { + dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)]; + if (s2 < s) { + s = s2; + si = lcp.indexN(k); + si_in_N = 1; + } + } + } + for (k=0; k < lcp.numC(); k++) { + if (delta_x[lcp.indexC(k)] < 0) { + dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)]; + if (s2 < s) { + s = s2; + si = lcp.indexC(k); + si_in_N = 0; + } + } + } + + // apply x = x + s * delta_x + lcp.pC_plusequals_s_times_qC (x,s,delta_x); + x[i] += s; + lcp.pN_plusequals_s_times_qN (w,s,delta_w); + w[i] += s * delta_w[i]; + + // switch indexes between sets if necessary + if (si==i) { + w[i] = 0; + lcp.transfer_i_to_C (i); + break; + } + if (si_in_N) { + w[si] = 0; + lcp.transfer_i_from_N_to_C (si); + } + else { + x[si] = 0; + lcp.transfer_i_from_C_to_N (si); + } + } + } + } + + done: + lcp.unpermute(); + + UNALLOCA (L); + UNALLOCA (d); + UNALLOCA (delta_x); + UNALLOCA (delta_w); + UNALLOCA (Dell); + UNALLOCA (ell); + UNALLOCA (tmp); + UNALLOCA (Arows); + UNALLOCA (p); + UNALLOCA (C); + UNALLOCA (dummy); +} + +//*************************************************************************** +// an optimized Dantzig LCP driver routine for the lo-hi LCP problem. + +void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, + dReal *w, int nub, dReal *lo, dReal *hi, int *findex) +{ + dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n); + + int i,k,hit_first_friction_index = 0; + int nskip = dPAD(n); + + // if all the variables are unbounded then we can just factor, solve, + // and return + if (nub >= n) { + dFactorLDLT (A,w,n,nskip); // use w for d + dSolveLDLT (A,w,b,n,nskip); + memcpy (x,b,n*sizeof(dReal)); + dSetZero (w,n); + + return; + } +# ifndef dNODEBUG + // check restrictions on lo and hi + for (k=0; k= 0); +# endif + ALLOCA (dReal,L,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (L == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,d,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (d == NULL) { + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_x,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_x == NULL) { + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,delta_w,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (delta_w == NULL) { + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,Dell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Dell == NULL) { + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,ell,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ell == NULL) { + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal*,Arows,n*sizeof(dReal*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (Arows == NULL) { + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,p,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (p == NULL) { + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (int,C,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (C == NULL) { + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + int dir; + dReal dirf; + + // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) + ALLOCA (int,state,n*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (state == NULL) { + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + // create LCP object. note that tmp is set to delta_w to save space, this + // optimization relies on knowledge of how tmp is used, so be careful! + dLCP *lcp=new dLCP(n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows); + nub = lcp->getNub(); + + // loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the + // LCP conditions then i is added to the appropriate index set. otherwise + // x(i),w(i) is driven either +ve or -ve to force it to the valid region. + // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. + // while driving x(i) we maintain the LCP conditions on the other variables + // 0..i-1. we do this by watching out for other x(i),w(i) values going + // outside the valid region, and then switching them between index sets + // when that happens. + + for (i=nub; i= 0) { + // un-permute x into delta_w, which is not being used at the moment + for (k=0; kAiC_times_qC (i,x) + lcp->AiN_times_qN (i,x) - b[i]; + + // if lo=hi=0 (which can happen for tangential friction when normals are + // 0) then the index will be assigned to set N with some state. however, + // set C's line has zero size, so the index will always remain in set N. + // with the "normal" switching logic, if w changed sign then the index + // would have to switch to set C and then back to set N with an inverted + // state. this is pointless, and also computationally expensive. to + // prevent this from happening, we use the rule that indexes with lo=hi=0 + // will never be checked for set changes. this means that the state for + // these indexes may be incorrect, but that doesn't matter. + + // see if x(i),w(i) is in a valid region + if (lo[i]==0 && w[i] >= 0) { + lcp->transfer_i_to_N (i); + state[i] = 0; + } + else if (hi[i]==0 && w[i] <= 0) { + lcp->transfer_i_to_N (i); + state[i] = 1; + } + else if (w[i]==0) { + // this is a degenerate case. by the time we get to this test we know + // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, + // and similarly that hi > 0. this means that the line segment + // corresponding to set C is at least finite in extent, and we are on it. + // NOTE: we must call lcp->solve1() before lcp->transfer_i_to_C() + lcp->solve1 (delta_x,i,0,1); + +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + UNALLOCA(state); + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + return; + } +#endif + + lcp->transfer_i_to_C (i); + } + else { + // we must push x(i) and w(i) + for (;;) { + // find direction to push on x(i) + if (w[i] <= 0) { + dir = 1; + dirf = REAL(1.0); + } + else { + dir = -1; + dirf = REAL(-1.0); + } + + // compute: delta_x(C) = -dir*A(C,C)\A(C,i) + lcp->solve1 (delta_x,i,dir); + +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + UNALLOCA(state); + UNALLOCA(C); + UNALLOCA(p); + UNALLOCA(Arows); + UNALLOCA(ell); + UNALLOCA(Dell); + UNALLOCA(delta_w); + UNALLOCA(delta_x); + UNALLOCA(d); + UNALLOCA(L); + return; + } +#endif + + // note that delta_x[i] = dirf, but we wont bother to set it + + // compute: delta_w = A*delta_x ... note we only care about + // delta_w(N) and delta_w(i), the rest is ignored + lcp->pN_equals_ANC_times_qC (delta_w,delta_x); + lcp->pN_plusequals_ANi (delta_w,i,dir); + delta_w[i] = lcp->AiC_times_qC (i,delta_x) + lcp->Aii(i)*dirf; + + // find largest step we can take (size=s), either to drive x(i),w(i) + // to the valid LCP region or to drive an already-valid variable + // outside the valid region. + + int cmd = 1; // index switching command + int si = 0; // si = index to switch if cmd>3 + dReal s = -w[i]/delta_w[i]; + if (dir > 0) { + if (hi[i] < dInfinity) { + dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i) + if (s2 < s) { + s = s2; + cmd = 3; + } + } + } + else { + if (lo[i] > -dInfinity) { + dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i) + if (s2 < s) { + s = s2; + cmd = 2; + } + } + } + + for (k=0; k < lcp->numN(); k++) { + if ((state[lcp->indexN(k)]==0 && delta_w[lcp->indexN(k)] < 0) || + (state[lcp->indexN(k)]!=0 && delta_w[lcp->indexN(k)] > 0)) { + // don't bother checking if lo=hi=0 + if (lo[lcp->indexN(k)] == 0 && hi[lcp->indexN(k)] == 0) continue; + dReal s2 = -w[lcp->indexN(k)] / delta_w[lcp->indexN(k)]; + if (s2 < s) { + s = s2; + cmd = 4; + si = lcp->indexN(k); + } + } + } + + for (k=nub; k < lcp->numC(); k++) { + if (delta_x[lcp->indexC(k)] < 0 && lo[lcp->indexC(k)] > -dInfinity) { + dReal s2 = (lo[lcp->indexC(k)]-x[lcp->indexC(k)]) / + delta_x[lcp->indexC(k)]; + if (s2 < s) { + s = s2; + cmd = 5; + si = lcp->indexC(k); + } + } + if (delta_x[lcp->indexC(k)] > 0 && hi[lcp->indexC(k)] < dInfinity) { + dReal s2 = (hi[lcp->indexC(k)]-x[lcp->indexC(k)]) / + delta_x[lcp->indexC(k)]; + if (s2 < s) { + s = s2; + cmd = 6; + si = lcp->indexC(k); + } + } + } + + //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", + // "C->NL","C->NH"}; + //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); + + // if s <= 0 then we've got a problem. if we just keep going then + // we're going to get stuck in an infinite loop. instead, just cross + // our fingers and exit with the current solution. + if (s <= 0) { + dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); + if (i < (n-1)) { + dSetZero (x+i,n-i); + dSetZero (w+i,n-i); + } + goto done; + } + + // apply x = x + s * delta_x + lcp->pC_plusequals_s_times_qC (x,s,delta_x); + x[i] += s * dirf; + + // apply w = w + s * delta_w + lcp->pN_plusequals_s_times_qN (w,s,delta_w); + w[i] += s * delta_w[i]; + + // switch indexes between sets if necessary + switch (cmd) { + case 1: // done + w[i] = 0; + lcp->transfer_i_to_C (i); + break; + case 2: // done + x[i] = lo[i]; + state[i] = 0; + lcp->transfer_i_to_N (i); + break; + case 3: // done + x[i] = hi[i]; + state[i] = 1; + lcp->transfer_i_to_N (i); + break; + case 4: // keep going + w[si] = 0; + lcp->transfer_i_from_N_to_C (si); + break; + case 5: // keep going + x[si] = lo[si]; + state[si] = 0; + lcp->transfer_i_from_C_to_N (si); + break; + case 6: // keep going + x[si] = hi[si]; + state[si] = 1; + lcp->transfer_i_from_C_to_N (si); + break; + } + + if (cmd <= 3) break; + } + } + } + + done: + lcp->unpermute(); + delete lcp; + + UNALLOCA (L); + UNALLOCA (d); + UNALLOCA (delta_x); + UNALLOCA (delta_w); + UNALLOCA (Dell); + UNALLOCA (ell); + UNALLOCA (Arows); + UNALLOCA (p); + UNALLOCA (C); + UNALLOCA (state); +} + +//*************************************************************************** +// accuracy and timing test + +extern "C" void dTestSolveLCP() +{ + int n = 100; + int i,nskip = dPAD(n); + const dReal tol = REAL(1e-9); + printf ("dTestSolveLCP()\n"); + + ALLOCA (dReal,A,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,x,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (x == NULL) { + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,b,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (b == NULL) { + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,w,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (w == NULL) { + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,lo,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo == NULL) { + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,hi,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi == NULL) { + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + ALLOCA (dReal,A2,n*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A2 == NULL) { + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,b2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (b2 == NULL) { + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,lo2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo2 == NULL) { + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,hi2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi2 == NULL) { + UNALLOCA (lo2); + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,tmp1,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (tmp1 == NULL) { + UNALLOCA (hi2); + UNALLOCA (lo2); + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA (dReal,tmp2,n*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (tmp2 == NULL) { + UNALLOCA (tmp1); + UNALLOCA (hi2); + UNALLOCA (lo2); + UNALLOCA (b2); + UNALLOCA (A2); + UNALLOCA (hi); + UNALLOCA (lo); + UNALLOCA (w); + UNALLOCA (b); + UNALLOCA (x); + UNALLOCA (A); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + double total_time = 0; + for (int count=0; count < 1000; count++) { + + // form (A,b) = a random positive definite LCP problem + dMakeRandomMatrix (A2,n,n,1.0); + dMultiply2 (A,A2,A2,n,n,n); + dMakeRandomMatrix (x,n,1,1.0); + dMultiply0 (b,A,x,n,n,1); + for (i=0; i tol ? "FAILED" : "passed"); + if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff); + int n1=0,n2=0,n3=0; + for (i=0; i= 0) { + n1++; // ok + } + else if (x[i]==hi[i] && w[i] <= 0) { + n2++; // ok + } + else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) { + n3++; // ok + } + else { + dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i, + x[i],w[i],lo[i],hi[i]); + } + } + + // pacifier + printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3); + printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average); + } + + UNALLOCA (A); + UNALLOCA (x); + UNALLOCA (b); + UNALLOCA (w); + UNALLOCA (lo); + UNALLOCA (hi); + UNALLOCA (A2); + UNALLOCA (b2); + UNALLOCA (lo2); + UNALLOCA (hi2); + UNALLOCA (tmp1); + UNALLOCA (tmp2); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_lcp.h b/src/external/open_dynamics_engine-ef/ode/ode_lcp.h new file mode 100644 index 00000000..484902c1 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_lcp.h @@ -0,0 +1,58 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i) +satisfies one of + (1) x = lo, w >= 0 + (2) x = hi, w <= 0 + (3) lo < x < hi, w = 0 +A is a matrix of dimension n*n, everything else is a vector of size n*1. +lo and hi can be +/- dInfinity as needed. the first `nub' variables are +unbounded, i.e. hi and lo are assumed to be +/- dInfinity. + +we restrict lo(i) <= 0 and hi(i) >= 0. + +the original data (A,b) may be modified by this function. + +if the `findex' (friction index) parameter is nonzero, it points to an array +of index values. in this case constraints that have findex[i] >= 0 are +special. all non-special constraints are solved for, then the lo and hi values +for the special constraints are set: + hi[i] = abs( hi[i] * x[findex[i]] ) + lo[i] = -hi[i] +and the solution continues. this mechanism allows a friction approximation +to be implemented. the first `nub' variables are assumed to have findex < 0. + +*/ + + +#ifndef _ODE_LCP_H_ +#define _ODE_LCP_H_ + + +void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w, + int nub, dReal *lo, dReal *hi, int *findex); + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_mass.cpp b/src/external/open_dynamics_engine-ef/ode/ode_mass.cpp new file mode 100644 index 00000000..36508b24 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_mass.cpp @@ -0,0 +1,313 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_config.h" +#include "ode/ode_mass.h" +#include "ode/ode_math.h" +#include "ode/ode_matrix.h" + + +#define _I(i,j) I[(i)*4+(j)] + + +// return 1 if ok, 0 if bad + +static int checkMass (dMass *m) +{ + int i; + + if (m->mass <= 0) { + dDEBUGMSG ("mass must be > 0"); + return 0; + } + if (!dIsPositiveDefinite (m->I,3)) { + dDEBUGMSG ("inertia must be positive definite"); + return 0; + } + + // verify that the center of mass position is consistent with the mass + // and inertia matrix. this is done by checking that the inertia around + // the center of mass is also positive definite. from the comment in + // dMassTranslate(), if the body is translated so that its center of mass + // is at the point of reference, then the new inertia is: + // I + mass*crossmat(c)^2 + // note that requiring this to be positive definite is exactly equivalent + // to requiring that the spatial inertia matrix + // [ mass*eye(3,3) M*crossmat(c)^T ] + // [ M*crossmat(c) I ] + // is positive definite, given that I is PD and mass>0. see the theorem + // about partitioned PD matrices for proof. + + dMatrix3 I2,chat; + dSetZero (chat,12); + dCROSSMAT (chat,m->c,4,+,-); + dMULTIPLY0_333 (I2,chat,chat); + for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i]; + for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i]; + for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i]; + if (!dIsPositiveDefinite (I2,3)) { + dDEBUGMSG ("center of mass inconsistent with mass parameters"); + return 0; + } + return 1; +} + + +void dMassSetZero (dMass *m) +{ + dAASSERT (m); + m->mass = REAL(0.0); + dSetZero (m->c,sizeof(m->c) / sizeof(dReal)); + dSetZero (m->I,sizeof(m->I) / sizeof(dReal)); +} + + +void dMassSetParameters (dMass *m, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal I11, dReal I22, dReal I33, + dReal I12, dReal I13, dReal I23) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = themass; + m->c[0] = cgx; + m->c[1] = cgy; + m->c[2] = cgz; + m->_I(0,0) = I11; + m->_I(1,1) = I22; + m->_I(2,2) = I33; + m->_I(0,1) = I12; + m->_I(0,2) = I13; + m->_I(1,2) = I23; + m->_I(1,0) = I12; + m->_I(2,0) = I13; + m->_I(2,1) = I23; + checkMass (m); +} + + +void dMassSetSphere (dMass *m, dReal density, dReal radius) +{ + dMassSetSphereTotal (m, (REAL(4.0)/REAL(3.0)) * M_PI * + radius*radius*radius * density, radius); +} + + +void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = total_mass; + dReal II = REAL(0.4) * total_mass * radius*radius; + m->_I(0,0) = II; + m->_I(1,1) = II; + m->_I(2,2) = II; + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassSetCappedCylinder (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dReal M1,M2,Ia,Ib; + dAASSERT (m); + dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); + dMassSetZero (m); + M1 = M_PI*radius*radius*length*density; // cylinder mass + M2 = (REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density; // total cap mass + m->mass = M1+M2; + Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) + + M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length); + Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius; + m->_I(0,0) = Ia; + m->_I(1,1) = Ia; + m->_I(2,2) = Ia; + m->_I(direction-1,direction-1) = Ib; + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassSetCappedCylinderTotal (dMass *m, dReal total_mass, int direction, + dReal a, dReal b) +{ + dMassSetCappedCylinder (m, 1.0, direction, a, b); + dMassAdjust (m, total_mass); +} + + +void dMassSetCylinder (dMass *m, dReal density, int direction, + dReal radius, dReal length) +{ + dMassSetCylinderTotal (m, M_PI*radius*radius*length*density, + direction, radius, length); +} + +void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction, + dReal radius, dReal length) +{ + dReal r2,I; + dAASSERT (m); + dMassSetZero (m); + r2 = radius*radius; + m->mass = total_mass; + I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length); + m->_I(0,0) = I; + m->_I(1,1) = I; + m->_I(2,2) = I; + m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2; + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassSetBox (dMass *m, dReal density, + dReal lx, dReal ly, dReal lz) +{ + dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz); +} + + +void dMassSetBoxTotal (dMass *m, dReal total_mass, + dReal lx, dReal ly, dReal lz) +{ + dAASSERT (m); + dMassSetZero (m); + m->mass = total_mass; + m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz); + m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz); + m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly); + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassAdjust (dMass *m, dReal newmass) +{ + dAASSERT (m); + dReal scale = newmass / m->mass; + m->mass = newmass; + for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale; + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassTranslate (dMass *m, dReal x, dReal y, dReal z) +{ + // if the body is translated by `a' relative to its point of reference, + // the new inertia about the point of reference is: + // + // I + mass*(crossmat(c)^2 - crossmat(c+a)^2) + // + // where c is the existing center of mass and I is the old inertia. + + int i,j; + dMatrix3 ahat,chat,t1,t2; + dReal a[3]; + + dAASSERT (m); + + // adjust inertia matrix + dSetZero (chat,12); + dCROSSMAT (chat,m->c,4,+,-); + a[0] = x + m->c[0]; + a[1] = y + m->c[1]; + a[2] = z + m->c[2]; + dSetZero (ahat,12); + dCROSSMAT (ahat,a,4,+,-); + dMULTIPLY0_333 (t1,ahat,ahat); + dMULTIPLY0_333 (t2,chat,chat); + for (i=0; i<3; i++) for (j=0; j<3; j++) + m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]); + + // ensure perfect symmetry + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); + + // adjust center of mass + m->c[0] += x; + m->c[1] += y; + m->c[2] += z; + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassRotate (dMass *m, const dMatrix3 R) +{ + // if the body is rotated by `R' relative to its point of reference, + // the new inertia about the point of reference is: + // + // R * I * R' + // + // where I is the old inertia. + + dMatrix3 t1; + dReal t2[3]; + + dAASSERT (m); + + // rotate inertia matrix + dMULTIPLY2_333 (t1,m->I,R); + dMULTIPLY0_333 (m->I,R,t1); + + // ensure perfect symmetry + m->_I(1,0) = m->_I(0,1); + m->_I(2,0) = m->_I(0,2); + m->_I(2,1) = m->_I(1,2); + + // rotate center of mass + dMULTIPLY0_331 (t2,R,m->c); + m->c[0] = t2[0]; + m->c[1] = t2[1]; + m->c[2] = t2[2]; + +# ifndef dNODEBUG + checkMass (m); +# endif +} + + +void dMassAdd (dMass *a, const dMass *b) +{ + int i; + dAASSERT (a && b); + dReal denom = dRecip (a->mass + b->mass); + for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom; + a->mass += b->mass; + for (i=0; i<12; i++) a->I[i] += b->I[i]; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_mass.h b/src/external/open_dynamics_engine-ef/ode/ode_mass.h new file mode 100644 index 00000000..e2dfd184 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_mass.h @@ -0,0 +1,107 @@ +/************************************************************************* + * * + * 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_MASS_H_ +#define _ODE_MASS_H_ + +#include "ode/ode_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct dMass; +typedef struct dMass dMass; + + +void dMassSetZero (dMass *); + +void dMassSetParameters (dMass *, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal I11, dReal I22, dReal I33, + dReal I12, dReal I13, dReal I23); + +void dMassSetSphere (dMass *, dReal density, dReal radius); +void dMassSetSphereTotal (dMass *, dReal total_mass, dReal radius); + +void dMassSetCappedCylinder (dMass *, dReal density, int direction, + dReal radius, dReal length); +void dMassSetCappedCylinderTotal (dMass *, dReal total_mass, int direction, + dReal radius, dReal length); + +void dMassSetCylinder (dMass *, dReal density, int direction, + dReal radius, dReal length); +void dMassSetCylinderTotal (dMass *, dReal total_mass, int direction, + dReal radius, dReal length); + +void dMassSetBox (dMass *, dReal density, + dReal lx, dReal ly, dReal lz); +void dMassSetBoxTotal (dMass *, dReal total_mass, + dReal lx, dReal ly, dReal lz); + +void dMassAdjust (dMass *, dReal newmass); + +void dMassTranslate (dMass *, dReal x, dReal y, dReal z); + +void dMassRotate (dMass *, const dMatrix3 R); + +void dMassAdd (dMass *a, const dMass *b); + + + +struct dMass { + dReal mass; + dVector4 c; + dMatrix3 I; + +#ifdef __cplusplus + dMass() + { dMassSetZero (this); } + void setZero() + { dMassSetZero (this); } + void setParameters (dReal themass, dReal cgx, dReal cgy, dReal cgz, + dReal I11, dReal I22, dReal I33, + dReal I12, dReal I13, dReal I23) + { dMassSetParameters (this,themass,cgx,cgy,cgz,I11,I22,I33,I12,I13,I23); } + void setSphere (dReal density, dReal radius) + { dMassSetSphere (this,density,radius); } + void setCappedCylinder (dReal density, int direction, dReal a, dReal b) + { dMassSetCappedCylinder (this,density,direction,a,b); } + void setBox (dReal density, dReal lx, dReal ly, dReal lz) + { dMassSetBox (this,density,lx,ly,lz); } + void adjust (dReal newmass) + { dMassAdjust (this,newmass); } + void translate (dReal x, dReal y, dReal z) + { dMassTranslate (this,x,y,z); } + void rotate (const dMatrix3 R) + { dMassRotate (this,R); } + void add (const dMass *b) + { dMassAdd (this,b); } +#endif +}; + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_mat.cpp b/src/external/open_dynamics_engine-ef/ode/ode_mat.cpp new file mode 100644 index 00000000..13c19f3b --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_mat.cpp @@ -0,0 +1,230 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_config.h" +#include "ode/ode_misc.h" +#include "ode/ode_matrix.h" +#include "ode/ode_error.h" +#include "ode/ode_memory.h" +#include "ode/ode_mat.h" + + +dMatrix::dMatrix() +{ + n = 0; + m = 0; + data = 0; +} + + +dMatrix::dMatrix (int rows, int cols) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + dSetZero (data,n*m); +} + + +dMatrix::dMatrix (const dMatrix &a) +{ + n = a.n; + m = a.m; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); +} + + +dMatrix::dMatrix (int rows, int cols, + dReal *_data, int rowskip, int colskip) +{ + if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); + n = rows; + m = cols; + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + for (int i=0; i= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)"); + return data [i*m+j]; +} + + +void dMatrix::operator= (const dMatrix &a) +{ + if (data) dFree (data,n*m*sizeof(dReal)); + n = a.n; + m = a.m; + if (n > 0 && m > 0) { + data = (dReal*) dAlloc (n*m*sizeof(dReal)); + memcpy (data,a.data,n*m*sizeof(dReal)); + } + else data = 0; +} + + +void dMatrix::operator= (dReal a) +{ + for (int i=0; i= n || q[i] < 0 || q[i] >= m) + dDebug (0,"Matrix select, bad index arrays"); + r.data[i*nq+j] = data[p[i]*m+q[j]]; + } + } + return r; +} + + +dMatrix dMatrix::operator + (const dMatrix &a) +{ + if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes"); + dMatrix r (n,m); + for (int i=0; i max) max = diff; + } + } + return max; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_mat.h b/src/external/open_dynamics_engine-ef/ode/ode_mat.h new file mode 100644 index 00000000..0b5a07fd --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_mat.h @@ -0,0 +1,71 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// matrix class. this is mostly for convenience in the testing code, it is +// not optimized at all. correctness is much more importance here. + +#ifndef _ODE_MAT_H_ +#define _ODE_MAT_H_ + +#include "ode/ode_common.h" + + +class dMatrix { + int n,m; // matrix dimension, n,m >= 0 + dReal *data; // if nonzero, n*m elements allocated on the heap + +public: + // constructors, destructors + dMatrix(); // make default 0x0 matrix + dMatrix (int rows, int cols); // construct zero matrix of given size + dMatrix (const dMatrix &); // construct copy of given matrix + // create copy of given data - element (i,j) is data[i*rowskip+j*colskip] + dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip); + ~dMatrix(); // destructor + + // data movement + dReal & operator () (int i, int j); // reference an element + void operator= (const dMatrix &); // matrix = matrix + void operator= (dReal); // matrix = scalar + dMatrix transpose(); // return transposed matrix + // return a permuted submatrix of this matrix, made up of the rows in p + // and the columns in q. p has np elements, q has nq elements. + dMatrix select (int np, int *p, int nq, int *q); + + // operators + dMatrix operator + (const dMatrix &); + dMatrix operator - (const dMatrix &); + dMatrix operator - (); + dMatrix operator * (const dMatrix &); + void operator += (const dMatrix &); + void operator -= (const dMatrix &); + + // utility + void clearUpperTriangle(); + void clearLowerTriangle(); + void makeRandom (dReal range); + void print (const char *fmt = "%10.4f ", FILE *f=stdout); + dReal maxDifference (const dMatrix &); +}; + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_math.cpp b/src/external/open_dynamics_engine-ef/ode/ode_math.cpp new file mode 100644 index 00000000..7ef06d0c --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_math.cpp @@ -0,0 +1,165 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_common.h" +#include "ode/ode_math.h" + +// get some math functions under windows +#ifdef WIN32 +#include +#ifndef CYGWIN // added by andy for cygwin +//messes up codewarrior +#define copysign(a,b) ((dReal)_copysign(a,b)) +#endif // added by andy for cygwin +#endif + + +// this may be called for vectors `a' with extremely small magnitude, for +// example the result of a cross product on two nearly perpendicular vectors. +// we must be robust to these small vectors. to prevent numerical error, +// first find the component a[i] with the largest magnitude and then scale +// all the components by 1/a[i]. then we can compute the length of `a' and +// scale the components by 1/l. this has been verified to work with vectors +// containing the smallest representable numbers. + +void dNormalize3 (dVector3 a) +{ + dReal a0,a1,a2,aa0,aa1,aa2,l; + dAASSERT (a); + a0 = a[0]; + a1 = a[1]; + a2 = a[2]; + aa0 = dFabs(a0); + aa1 = dFabs(a1); + aa2 = dFabs(a2); + if (aa1 > aa0) { + if (aa2 > aa1) { + goto aa2_largest; + } + else { // aa1 is largest + a0 /= aa1; + a2 /= aa1; + l = dRecipSqrt (a0*a0 + a2*a2 + 1); + a[0] = a0*l; + a[1] = dCopySign(l,a1); + a[2] = a2*l; + } + } + else { + if (aa2 > aa0) { + aa2_largest: // aa2 is largest + a0 /= aa2; + a1 /= aa2; + l = dRecipSqrt (a0*a0 + a1*a1 + 1); + a[0] = a0*l; + a[1] = a1*l; + a[2] = dCopySign(l,a2); + } + else { // aa0 is largest + if (aa0 <= 0) { + // dDEBUGMSG ("vector has zero size"); ... this messace is annoying + a[0] = 1; // if all a's are zero, this is where we'll end up. + a[1] = 0; // return a default unit length vector. + a[2] = 0; + return; + } + a1 /= aa0; + a2 /= aa0; + l = dRecipSqrt (a1*a1 + a2*a2 + 1); + a[0] = dCopySign(l,a0); + a[1] = a1*l; + a[2] = a2*l; + } + } +} + + +/* OLD VERSION */ +/* +void dNormalize3 (dVector3 a) +{ + dASSERT (a); + dReal l = dDOT(a,a); + if (l > 0) { + l = dRecipSqrt(l); + a[0] *= l; + a[1] *= l; + a[2] *= l; + } + else { + a[0] = 1; + a[1] = 0; + a[2] = 0; + } +} +*/ + + +void dNormalize4 (dVector4 a) +{ + dAASSERT (a); + dReal l = dDOT(a,a)+a[3]*a[3]; + if (l > 0) { + l = dRecipSqrt(l); + a[0] *= l; + a[1] *= l; + a[2] *= l; + a[3] *= l; + } + else { + dDEBUGMSG ("vector has zero size"); + a[0] = 1; + a[1] = 0; + a[2] = 0; + a[3] = 0; + } +} + + +void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q) +{ + dAASSERT (n && p && q); + if (dFabs(n[2]) > M_SQRT1_2) { + // choose p in y-z plane + dReal a = n[1]*n[1] + n[2]*n[2]; + dReal k = dRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + dReal a = n[0]*n[0] + n[1]*n[1]; + dReal k = dRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_math.h b/src/external/open_dynamics_engine-ef/ode/ode_math.h new file mode 100644 index 00000000..624b3cfd --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_math.h @@ -0,0 +1,258 @@ +/************************************************************************* + * * + * 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_ODEMATH_H_ +#define _ODE_ODEMATH_H_ + +#include "ode/ode_common.h" + +#ifdef __GNUC__ +#define PURE_INLINE extern inline +#else +#define PURE_INLINE inline +#endif + +/* + * macro to access elements i,j in an NxM matrix A, independent of the + * matrix storage convention. + */ +#define dACCESS33(A,i,j) ((A)[(i)*4+(j)]) + + +/* + * 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced + * p and q indexes apart respectively. dDOT() means dDOT11. + * in C++ we could use function templates to get all the versions of these + * functions - but on some compilers this will result in sub-optimal code. + */ + +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) + +#ifdef __cplusplus + +PURE_INLINE dReal dDOT (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,1); } +PURE_INLINE dReal dDOT13 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,3); } +PURE_INLINE dReal dDOT31 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,1); } +PURE_INLINE dReal dDOT33 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,3); } +PURE_INLINE dReal dDOT14 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,4); } +PURE_INLINE dReal dDOT41 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,1); } +PURE_INLINE dReal dDOT44 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,4); } + +#else + +#define dDOT(a,b) dDOTpq(a,b,1,1) +#define dDOT13(a,b) dDOTpq(a,b,1,3) +#define dDOT31(a,b) dDOTpq(a,b,3,1) +#define dDOT33(a,b) dDOTpq(a,b,3,3) +#define dDOT14(a,b) dDOTpq(a,b,1,4) +#define dDOT41(a,b) dDOTpq(a,b,4,1) +#define dDOT44(a,b) dDOTpq(a,b,4,4) + +#endif /* __cplusplus */ + + +/* + * cross product, set a = b x c. dCROSSpqr means that elements of `a', `b' + * and `c' are spaced p, q and r indexes apart respectively. + * dCROSS() means dCROSS111. `op' is normally `=', but you can set it to + * +=, -= etc to get other effects. + */ + +#define dCROSS(a,op,b,c) \ +do { \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); \ +} while(0) +#define dCROSSpqr(a,op,b,c,p,q,r) \ +do { \ + (a)[ 0] op ((b)[ q]*(c)[2*r] - (b)[2*q]*(c)[ r]); \ + (a)[ p] op ((b)[2*q]*(c)[ 0] - (b)[ 0]*(c)[2*r]); \ + (a)[2*p] op ((b)[ 0]*(c)[ r] - (b)[ q]*(c)[ 0]); \ +} while(0) +#define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4) +#define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1) +#define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4) +#define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1) +#define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4) +#define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1) +#define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4) + + +/* + * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. + * A is stored by rows, and has `skip' elements per row. the matrix is + * assumed to be already zero, so this does not write zero elements! + * if (plus,minus) is (+,-) then a positive version will be written. + * if (plus,minus) is (-,+) then a negative version will be written. + */ + +#define dCROSSMAT(A,a,skip,plus,minus) \ +do { \ + (A)[1] = minus (a)[2]; \ + (A)[2] = plus (a)[1]; \ + (A)[(skip)+0] = plus (a)[2]; \ + (A)[(skip)+2] = minus (a)[0]; \ + (A)[2*(skip)+0] = minus (a)[1]; \ + (A)[2*(skip)+1] = plus (a)[0]; \ +} while(0) + + +/* + * compute the distance between two 3-vectors + */ + +#ifdef __cplusplus +PURE_INLINE float dDISTANCE (const float a[3], const float b[3]) + { return (float) dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); } +PURE_INLINE double dDISTANCE (const double a[3], const double b[3]) + { return dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); } +#else +#define dDISTANCE(a,b) \ + (dSqrt( ((a)[0]-(b)[0])*((a)[0]-(b)[0]) + ((a)[1]-(b)[1])*((a)[1]-(b)[1]) + ((a)[2]-(b)[2])*((a)[2]-(b)[2]) )) +#endif + + +/* + * special case matrix multipication, with operator selection + */ + +#define dMULTIPLYOP0_331(A,op,B,C) \ +do { \ + (A)[0] op dDOT((B),(C)); \ + (A)[1] op dDOT((B+4),(C)); \ + (A)[2] op dDOT((B+8),(C)); \ +} while(0) +#define dMULTIPLYOP1_331(A,op,B,C) \ +do { \ + (A)[0] op dDOT41((B),(C)); \ + (A)[1] op dDOT41((B+1),(C)); \ + (A)[2] op dDOT41((B+2),(C)); \ +} while(0) +#define dMULTIPLYOP0_133(A,op,B,C) \ +do { \ + (A)[0] op dDOT14((B),(C)); \ + (A)[1] op dDOT14((B),(C+1)); \ + (A)[2] op dDOT14((B),(C+2)); \ +} while(0) +#define dMULTIPLYOP0_333(A,op,B,C) \ +do { \ + (A)[0] op dDOT14((B),(C)); \ + (A)[1] op dDOT14((B),(C+1)); \ + (A)[2] op dDOT14((B),(C+2)); \ + (A)[4] op dDOT14((B+4),(C)); \ + (A)[5] op dDOT14((B+4),(C+1)); \ + (A)[6] op dDOT14((B+4),(C+2)); \ + (A)[8] op dDOT14((B+8),(C)); \ + (A)[9] op dDOT14((B+8),(C+1)); \ + (A)[10] op dDOT14((B+8),(C+2)); \ +} while(0) +#define dMULTIPLYOP1_333(A,op,B,C) \ +do { \ + (A)[0] op dDOT44((B),(C)); \ + (A)[1] op dDOT44((B),(C+1)); \ + (A)[2] op dDOT44((B),(C+2)); \ + (A)[4] op dDOT44((B+1),(C)); \ + (A)[5] op dDOT44((B+1),(C+1)); \ + (A)[6] op dDOT44((B+1),(C+2)); \ + (A)[8] op dDOT44((B+2),(C)); \ + (A)[9] op dDOT44((B+2),(C+1)); \ + (A)[10] op dDOT44((B+2),(C+2)); \ +} while(0) +#define dMULTIPLYOP2_333(A,op,B,C) \ +do { \ + (A)[0] op dDOT((B),(C)); \ + (A)[1] op dDOT((B),(C+4)); \ + (A)[2] op dDOT((B),(C+8)); \ + (A)[4] op dDOT((B+4),(C)); \ + (A)[5] op dDOT((B+4),(C+4)); \ + (A)[6] op dDOT((B+4),(C+8)); \ + (A)[8] op dDOT((B+8),(C)); \ + (A)[9] op dDOT((B+8),(C+4)); \ + (A)[10] op dDOT((B+8),(C+8)); \ +} while(0) + +#ifdef __cplusplus + +#define DECL template PURE_INLINE void + +DECL dMULTIPLY0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,=,B,C); } +DECL dMULTIPLY1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,=,B,C); } +DECL dMULTIPLY0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,=,B,C); } +DECL dMULTIPLY0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,=,B,C); } +DECL dMULTIPLY1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,=,B,C); } +DECL dMULTIPLY2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,=,B,C); } + +DECL dMULTIPLYADD0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,+=,B,C); } +DECL dMULTIPLYADD1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,+=,B,C); } +DECL dMULTIPLYADD0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,+=,B,C); } +DECL dMULTIPLYADD0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,+=,B,C); } +DECL dMULTIPLYADD1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,+=,B,C); } +DECL dMULTIPLYADD2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,+=,B,C); } + +#undef DECL + +#else + +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) +#define dMULTIPLY0_133(A,B,C) dMULTIPLYOP0_133(A,=,B,C) +#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) +#define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C) +#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) + +#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) +#define dMULTIPLYADD1_331(A,B,C) dMULTIPLYOP1_331(A,+=,B,C) +#define dMULTIPLYADD0_133(A,B,C) dMULTIPLYOP0_133(A,+=,B,C) +#define dMULTIPLYADD0_333(A,B,C) dMULTIPLYOP0_333(A,+=,B,C) +#define dMULTIPLYADD1_333(A,B,C) dMULTIPLYOP1_333(A,+=,B,C) +#define dMULTIPLYADD2_333(A,B,C) dMULTIPLYOP2_333(A,+=,B,C) + +#endif + + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length) + */ +void dNormalize3 (dVector3 a); +void dNormalize4 (dVector4 a); + + +/* + * given a unit length "normal" vector n, generate vectors p and q vectors + * that are an orthonormal basis for the plane space perpendicular to n. + * i.e. this makes p,q such that n,p,q are all perpendicular to each other. + * q will equal n x p. if n is not unit length then p will be unit length but + * q wont be. + */ + +void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_matrix.cpp b/src/external/open_dynamics_engine-ef/ode/ode_matrix.cpp new file mode 100644 index 00000000..10c01227 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_matrix.cpp @@ -0,0 +1,358 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_common.h" +#include "ode/ode_matrix.h" + +// misc defines +#define ALLOCA dALLOCA16 + + +void dSetZero (dReal *a, int n) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = 0; + n--; + } +} + + +void dSetValue (dReal *a, int n, dReal value) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = value; + n--; + } +} + + +void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) +{ + int i,j,k,qskip,rskip,rpad; + dAASSERT (A && B && C && p>0 && q>0 && r>0); + qskip = dPAD(q); + rskip = dPAD(r); + rpad = rskip - r; + dReal sum; + const dReal *b,*c,*bb; + bb = B; + for (i=p; i; i--) { + for (j=0 ; j0 && q>0 && r>0); + pskip = dPAD(p); + rskip = dPAD(r); + for (i=0; i0 && q>0 && r>0); + rpad = dPAD(r) - r; + qskip = dPAD(q); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + z = 0; + sum = 0; + for (k=q; k; k--,z++) sum += bb[z] * cc[z]; + *(A++) = sum; + cc += qskip; + } + A += rpad; + bb += qskip; + } +} + + +int dFactorCholesky (dReal *A, int n) +{ + int i,j,k,nskip; + dReal sum,*a,*b,*aa,*bb,*cc,*recip; + dAASSERT (n > 0 && A); + nskip = dPAD (n); + recip = (dReal*) ALLOCA (n * sizeof(dReal)); + aa = A; + for (i=0; i 0 && L && b); + nskip = dPAD (n); + y = (dReal*) ALLOCA (n*sizeof(dReal)); + for (i=0; i= 0; i--) { + sum = 0; + for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k]; + b[i] = (y[i]-sum)/L[i*nskip+i]; + } +} + + +int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) +{ + int i,j,nskip; + dReal *L,*x; + dAASSERT (n > 0 && A && Ainv); + nskip = dPAD (n); + L = (dReal*) ALLOCA (nskip*n*sizeof(dReal)); + memcpy (L,A,nskip*n*sizeof(dReal)); + x = (dReal*) ALLOCA (n*sizeof(dReal)); + if (dFactorCholesky (L,n)==0) return 0; + dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0 + for (i=0; i 0 && A); + int nskip = dPAD (n); + Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal)); + memcpy (Acopy,A,nskip*n * sizeof(dReal)); + return dFactorCholesky (Acopy,n); +} + + +/***** this has been replaced by a faster version +void dSolveL1T (const dReal *L, dReal *b, int n, int nskip) +{ + int i,j; + dAASSERT (L && b && n >= 0 && nskip >= n); + dReal sum; + for (i=n-2; i>=0; i--) { + sum = 0; + for (j=i+1; j= 0); + for (int i=0; i 0 && nskip >= n); + dSolveL1 (L,b,n,nskip); + dVectorScale (b,d,n); + dSolveL1T (L,b,n,nskip); +} + + +void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip) +{ + int j,p; + dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee; + dAASSERT (L && d && a && n > 0 && nskip >= n); + + if (n < 2) return; + W1 = (dReal*) ALLOCA (n*sizeof(dReal)); + W2 = (dReal*) ALLOCA (n*sizeof(dReal)); + + W1[0] = 0; + W2[0] = 0; + for (j=1; j j) ? _GETA(i,j) : _GETA(j,i)) + + +void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, + int n1, int n2, int r, int nskip) +{ + int i; + dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && + n1 >= n2 && nskip >= n1); + #ifndef dNODEBUG + for (i=0; i= 0 && p[i] < n1); + #endif + + if (r==n2-1) { + return; // deleting last row/col is easy + } + else if (r==0) { + dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal)); + for (i=0; i 0 && nskip >= n && r >= 0 && r < n); + if (r >= n-1) return; + if (r > 0) { + for (i=0; i 0 for any x. this performs a + * cholesky decomposition of A. if the decomposition fails then the matrix + * is not positive definite. A is stored by rows. A is not altered. + */ + +int dIsPositiveDefinite (const dReal *A, int n); + + +/* factorize a matrix A into L*D*L', where L is lower triangular with ones on + * the diagonal, and D is diagonal. + * A is an n*n matrix stored by rows, with a leading dimension of n rounded + * up to 4. L is written into the strict lower triangle of A (the ones are not + * written) and the reciprocal of the diagonal elements of D are written into + * d. + */ +void dFactorLDLT (dReal *A, dReal *d, int n, int nskip); + + +/* solve L*x=b, where L is n*n lower triangular with ones on the diagonal, + * and x,b are n*1. b is overwritten with x. + * the leading dimension of L is `nskip'. + */ +void dSolveL1 (const dReal *L, dReal *b, int n, int nskip); + + +/* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal, + * and x,b are n*1. b is overwritten with x. + * the leading dimension of L is `nskip'. + */ +void dSolveL1T (const dReal *L, dReal *b, int n, int nskip); + + +/* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */ + +void dVectorScale (dReal *a, const dReal *d, int n); + + +/* given `L', a n*n lower triangular matrix with ones on the diagonal, + * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix + * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b. + * the leading dimension of L is `nskip'. + */ + +void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip); + + +/* given an L*D*L' factorization of an n*n matrix A, return the updated + * factorization L2*D2*L2' of A plus the following "top left" matrix: + * + * [ b a' ] <-- b is a[0] + * [ a 0 ] <-- a is a[1..n-1] + * + * - L has size n*n, its leading dimension is nskip. L is lower triangular + * with ones on the diagonal. only the lower triangle of L is referenced. + * - d has size n. d contains the reciprocal diagonal elements of D. + * - a has size n. + * the result is written into L, except that the left column of L and d[0] + * are not actually modified. see ldltaddTL.m for further comments. + */ +void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip); + + +/* given an L*D*L' factorization of a permuted matrix A, produce a new + * factorization for row and column `r' removed. + * - A has size n1*n1, its leading dimension in nskip. A is symmetric and + * positive definite. only the lower triangle of A is referenced. + * A itself may actually be an array of row pointers. + * - L has size n2*n2, its leading dimension in nskip. L is lower triangular + * with ones on the diagonal. only the lower triangle of L is referenced. + * - d has size n2. d contains the reciprocal diagonal elements of D. + * - p is a permutation vector. it contains n2 indexes into A. each index + * must be in the range 0..n1-1. + * - r is the row/column of L to remove. + * the new L will be written within the old L, i.e. will have the same leading + * dimension. the last row and column of L, and the last element of d, are + * undefined on exit. + * + * a fast O(n^2) algorithm is used. see ldltremove.m for further comments. + */ +void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, + int n1, int n2, int r, int nskip); + + +/* given an n*n matrix A (with leading dimension nskip), remove the r'th row + * and column by moving elements. the new matrix will have the same leading + * dimension. the last row and column of A are untouched on exit. + */ +void dRemoveRowCol (dReal *A, int n, int nskip, int r); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_memory.cpp b/src/external/open_dynamics_engine-ef/ode/ode_memory.cpp new file mode 100644 index 00000000..d627b398 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_memory.cpp @@ -0,0 +1,87 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_config.h" +#include "ode/ode_memory.h" +#include "ode/ode_error.h" + + +static dAllocFunction *allocfn = 0; +static dReallocFunction *reallocfn = 0; +static dFreeFunction *freefn = 0; + + + +void dSetAllocHandler (dAllocFunction *fn) +{ + allocfn = fn; +} + + +void dSetReallocHandler (dReallocFunction *fn) +{ + reallocfn = fn; +} + + +void dSetFreeHandler (dFreeFunction *fn) +{ + freefn = fn; +} + + +dAllocFunction *dGetAllocHandler() +{ + return allocfn; +} + + +dReallocFunction *dGetReallocHandler() +{ + return reallocfn; +} + + +dFreeFunction *dGetFreeHandler() +{ + return freefn; +} + + +void * dAlloc (size_t size) +{ + if (allocfn) return allocfn (size); else return malloc (size); +} + + +void * dRealloc (void *ptr, size_t oldsize, size_t newsize) +{ + if (reallocfn) return reallocfn (ptr,oldsize,newsize); + else return realloc (ptr,newsize); +} + + +void dFree (void *ptr, size_t size) +{ + if (!ptr) return; + if (freefn) freefn (ptr,size); else free (ptr); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_memory.h b/src/external/open_dynamics_engine-ef/ode/ode_memory.h new file mode 100644 index 00000000..bc0679ad --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_memory.h @@ -0,0 +1,59 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* this comes from the `reuse' library. copy any changes back to the source */ + +#ifndef _ODE_MEMORY_H_ +#define _ODE_MEMORY_H_ + +#include "ode/ode_config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* function types to allocate and free memory */ +typedef void * dAllocFunction (size_t size); +typedef void * dReallocFunction (void *ptr, size_t oldsize, size_t newsize); +typedef void dFreeFunction (void *ptr, size_t size); + +/* set new memory management functions. if fn is 0, the default handlers are + * used. */ +void dSetAllocHandler (dAllocFunction *fn); +void dSetReallocHandler (dReallocFunction *fn); +void dSetFreeHandler (dFreeFunction *fn); + +/* get current memory management functions */ +dAllocFunction *dGetAllocHandler (void); +dReallocFunction *dGetReallocHandler (void); +dFreeFunction *dGetFreeHandler (void); + +/* allocate and free memory. */ +void * dAlloc (size_t size); +void * dRealloc (void *ptr, size_t oldsize, size_t newsize); +void dFree (void *ptr, size_t size); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_misc.cpp b/src/external/open_dynamics_engine-ef/ode/ode_misc.cpp new file mode 100644 index 00000000..18fabac4 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_misc.cpp @@ -0,0 +1,147 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_config.h" +#include "ode/ode_misc.h" +#include "ode/ode_matrix.h" + +//**************************************************************************** +// random numbers + +static unsigned long seed = 0; + +unsigned long dRand() +{ + seed = (1664525L*seed + 1013904223L) & 0xffffffff; + return seed; +} + + +unsigned long dRandGetSeed() +{ + return seed; +} + + +void dRandSetSeed (unsigned long s) +{ + seed = s; +} + + +int dTestRand() +{ + unsigned long oldseed = seed; + int ret = 1; + seed = 0; + if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 || + dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 || + dRand() != 0x6252e503) ret = 0; + seed = oldseed; + return ret; +} + + +int dRandInt (int n) +{ + double a = double(n) / 4294967296.0; + return (int) (double(dRand()) * a); +} + + +dReal dRandReal() +{ + return ((dReal) dRand()) / ((dReal) 0xffffffff); +} + +//**************************************************************************** +// matrix utility stuff + +void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f) +{ + int i,j; + int skip = dPAD(m); + for (i=0; i max) max = diff; + } + } + return max; +} + + +dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n) +{ + int i,j; + int skip = dPAD(n); + dReal diff,max; + max = 0; + for (i=0; i max) max = diff; + } + } + return max; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_misc.h b/src/external/open_dynamics_engine-ef/ode/ode_misc.h new file mode 100644 index 00000000..d7174e59 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_misc.h @@ -0,0 +1,85 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* miscellaneous math functions. these are mostly useful for testing */ + +#ifndef _ODE_MISC_H_ +#define _ODE_MISC_H_ + +#include "ode/ode_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +/* return 1 if the random number generator is working. */ +int dTestRand(void); + +/* return next 32 bit random number. this uses a not-very-random linear + * congruential method. + */ +unsigned long dRand(void); + +/* get and set the current random number seed. */ +unsigned long dRandGetSeed(void); +void dRandSetSeed (unsigned long s); + +/* return a random integer between 0..n-1. the distribution will get worse + * as n approaches 2^32. + */ +int dRandInt (int n); + +/* return a random real number between 0..1 */ +dReal dRandReal(void); + +/* print out a matrix */ +#ifdef __cplusplus +void dPrintMatrix (const dReal *A, int n, int m, const char *fmt = "%10.4f ", + FILE *f=stdout); +#else +void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f); +#endif + +/* make a random vector with entries between +/- range. A has n elements. */ +void dMakeRandomVector (dReal *A, int n, dReal range); + +/* make a random matrix with entries between +/- range. A has size n*m. */ +void dMakeRandomMatrix (dReal *A, int n, int m, dReal range); + +/* clear the upper triangle of a square matrix */ +void dClearUpperTriangle (dReal *A, int n); + +/* return the maximum element difference between the two n*m matrices */ +dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m); + +/* return the maximum element difference between the lower triangle of two + * n*n matrices */ +dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_objects.h b/src/external/open_dynamics_engine-ef/ode/ode_objects.h new file mode 100644 index 00000000..7e50683d --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_objects.h @@ -0,0 +1,284 @@ +/************************************************************************* + * * + * 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_OBJECTS_H_ +#define _ODE_OBJECTS_H_ + +#include "ode/ode_common.h" +#include "ode/ode_mass.h" +#include "ode/ode_contact.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* world */ + +dWorldID dWorldCreate(void); +void dWorldDestroy (dWorldID); + + int dWorldGetBodyCount(dWorldID); + +void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z); +void dWorldGetGravity (dWorldID, dVector3 gravity); +void dWorldSetERP (dWorldID, dReal erp); +dReal dWorldGetERP (dWorldID); +void dWorldSetCFM (dWorldID, dReal cfm); +dReal dWorldGetCFM (dWorldID); +void dWorldStep (dWorldID, dReal stepsize); +void dWorldImpulseToForce (dWorldID, dReal stepsize, + dReal ix, dReal iy, dReal iz, dVector3 force); + +/* World QuickStep functions */ + +void dWorldQuickStep (dWorldID w, dReal stepsize); +void dWorldSetQuickStepNumIterations (dWorldID, int num); +int dWorldGetQuickStepNumIterations (dWorldID); +void dWorldSetQuickStepW (dWorldID, dReal param); +dReal dWorldGetQuickStepW (dWorldID); + + int dWorldGetQuickStepWarmStartingDataSize(dWorldID w); + void dWorldGetQuickStepWarmStartingData(dWorldID w, dReal *array); + void dWorldSetQuickStepWarmStartingData(dWorldID w, dReal *array); + +/* World contact parameter functions */ + +void dWorldSetContactMaxCorrectingVel (dWorldID, dReal vel); +dReal dWorldGetContactMaxCorrectingVel (dWorldID); +void dWorldSetContactSurfaceLayer (dWorldID, dReal depth); +dReal dWorldGetContactSurfaceLayer (dWorldID); + +/* StepFast1 functions */ + +void dWorldStepFast1(dWorldID, dReal stepsize, int maxiterations); +void dWorldSetAutoEnableDepthSF1(dWorldID, int autoEnableDepth); +int dWorldGetAutoEnableDepthSF1(dWorldID); + +/* Auto-disable functions */ + +dReal dWorldGetAutoDisableLinearThreshold (dWorldID); +void dWorldSetAutoDisableLinearThreshold (dWorldID, dReal linear_threshold); +dReal dWorldGetAutoDisableAngularThreshold (dWorldID); +void dWorldSetAutoDisableAngularThreshold (dWorldID, dReal angular_threshold); +int dWorldGetAutoDisableSteps (dWorldID); +void dWorldSetAutoDisableSteps (dWorldID, int steps); +dReal dWorldGetAutoDisableTime (dWorldID); +void dWorldSetAutoDisableTime (dWorldID, dReal time); +int dWorldGetAutoDisableFlag (dWorldID); +void dWorldSetAutoDisableFlag (dWorldID, int do_auto_disable); + +dReal dBodyGetAutoDisableLinearThreshold (dBodyID); +void dBodySetAutoDisableLinearThreshold (dBodyID, dReal linear_threshold); +dReal dBodyGetAutoDisableAngularThreshold (dBodyID); +void dBodySetAutoDisableAngularThreshold (dBodyID, dReal angular_threshold); +int dBodyGetAutoDisableSteps (dBodyID); +void dBodySetAutoDisableSteps (dBodyID, int steps); +dReal dBodyGetAutoDisableTime (dBodyID); +void dBodySetAutoDisableTime (dBodyID, dReal time); +int dBodyGetAutoDisableFlag (dBodyID); +void dBodySetAutoDisableFlag (dBodyID, int do_auto_disable); +void dBodySetAutoDisableDefaults (dBodyID); + +/* bodies */ + +dBodyID dBodyCreate (dWorldID); +void dBodyDestroy (dBodyID); + +void dBodySetData (dBodyID, void *data); +void *dBodyGetData (dBodyID); + +void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z); +void dBodySetRotation (dBodyID, const dMatrix3 R); +void dBodySetQuaternion (dBodyID, const dQuaternion q); +void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z); +void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z); +const dReal * dBodyGetPosition (dBodyID); +const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */ +const dReal * dBodyGetQuaternion (dBodyID); +const dReal * dBodyGetLinearVel (dBodyID); +const dReal * dBodyGetAngularVel (dBodyID); + +void dBodySetMass (dBodyID, const dMass *mass); +void dBodyGetMass (dBodyID, dMass *mass); + +void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); + +const dReal * dBodyGetForce (dBodyID); +const dReal * dBodyGetTorque (dBodyID); +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z); +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z); + +void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); + +void dBodySetFiniteRotationMode (dBodyID, int mode); +void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z); + +int dBodyGetFiniteRotationMode (dBodyID); +void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result); + +int dBodyGetNumJoints (dBodyID b); +dJointID dBodyGetJoint (dBodyID, int index); + +void dBodyEnable (dBodyID); +void dBodyDisable (dBodyID); +int dBodyIsEnabled (dBodyID); + +void dBodySetGravityMode (dBodyID b, int mode); +int dBodyGetGravityMode (dBodyID b); + + +/* joints */ + +dJointID dJointCreateBall (dWorldID, dJointGroupID); +dJointID dJointCreateHinge (dWorldID, dJointGroupID); +dJointID dJointCreateSlider (dWorldID, dJointGroupID); +dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *); +dJointID dJointCreateHinge2 (dWorldID, dJointGroupID); +dJointID dJointCreateUniversal (dWorldID, dJointGroupID); +dJointID dJointCreateFixed (dWorldID, dJointGroupID); +dJointID dJointCreateNull (dWorldID, dJointGroupID); +dJointID dJointCreateAMotor (dWorldID, dJointGroupID); + +void dJointDestroy (dJointID); + +dJointGroupID dJointGroupCreate (int max_size); +void dJointGroupDestroy (dJointGroupID); +void dJointGroupEmpty (dJointGroupID); + +void dJointAttach (dJointID, dBodyID body1, dBodyID body2); +void dJointSetData (dJointID, void *data); +void *dJointGetData (dJointID); +int dJointGetType (dJointID); +dBodyID dJointGetBody (dJointID, int index); + +void dJointSetFeedback (dJointID, dJointFeedback *); +dJointFeedback *dJointGetFeedback (dJointID); + +void dJointSetBallParam (dJointID, int parameter, dReal value); + void dJointSetBallSpringMode(dJointID,int enable); +void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetBallAnchor2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeAnchorDelta (dJointID, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); +void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeParam (dJointID, int parameter, dReal value); +void dJointAddHingeTorque(dJointID joint, dReal torque); +void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z); +void dJointSetSliderAxisDelta (dJointID, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); +void dJointSetSliderParam (dJointID, int parameter, dReal value); +void dJointAddSliderForce(dJointID joint, dReal force); +void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Param (dJointID, int parameter, dReal value); +void dJointAddHinge2Torques(dJointID joint, dReal torque1, dReal torque2); +void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalParam (dJointID, int parameter, dReal value); +void dJointAddUniversalTorques(dJointID joint, dReal torque1, dReal torque2); +void dJointSetFixed (dJointID); +void dJointSetFixedAnchor (dJointID, dReal x, dReal y, dReal z, int abs); + void dJointSetFixedParam (dJointID,int parameter, dReal value); + void dJointSetFixedSpringMode(dJointID,int linearEnable, int angularEnable, + int customAnchorPointEnable); + + //added by Eric Froemling - for joints with only one body connection + void dJointSetFixedAnchorRotation(dJointID, const dQuaternion q); +void dJointSetAMotorNumAxes (dJointID, int num); +void dJointSetAMotorAxis (dJointID, int anum, int rel, + dReal x, dReal y, dReal z); +void dJointSetAMotorAngle (dJointID, int anum, dReal angle); +void dJointSetAMotorParam (dJointID, int parameter, dReal value); +void dJointSetAMotorMode (dJointID, int mode); +void dJointAddAMotorTorques (dJointID, dReal torque1, dReal torque2, dReal torque3); + +void dJointGetBallAnchor (dJointID, dVector3 result); +void dJointGetBallAnchor2 (dJointID, dVector3 result); +void dJointGetHingeAnchor (dJointID, dVector3 result); +void dJointGetHingeAnchor2 (dJointID, dVector3 result); +void dJointGetHingeAxis (dJointID, dVector3 result); +dReal dJointGetHingeParam (dJointID, int parameter); +dReal dJointGetHingeAngle (dJointID); +dReal dJointGetHingeAngleRate (dJointID); +dReal dJointGetSliderPosition (dJointID); +dReal dJointGetSliderPositionRate (dJointID); +void dJointGetSliderAxis (dJointID, dVector3 result); +dReal dJointGetSliderParam (dJointID, int parameter); +void dJointGetHinge2Anchor (dJointID, dVector3 result); +void dJointGetHinge2Anchor2 (dJointID, dVector3 result); +void dJointGetHinge2Axis1 (dJointID, dVector3 result); +void dJointGetHinge2Axis2 (dJointID, dVector3 result); +dReal dJointGetHinge2Param (dJointID, int parameter); +dReal dJointGetHinge2Angle1 (dJointID); +dReal dJointGetHinge2Angle1Rate (dJointID); +dReal dJointGetHinge2Angle2Rate (dJointID); +void dJointGetUniversalAnchor (dJointID, dVector3 result); +void dJointGetUniversalAnchor2 (dJointID, dVector3 result); +void dJointGetUniversalAxis1 (dJointID, dVector3 result); +void dJointGetUniversalAxis2 (dJointID, dVector3 result); +dReal dJointGetUniversalParam (dJointID, int parameter); +dReal dJointGetUniversalAngle1 (dJointID); +dReal dJointGetUniversalAngle2 (dJointID); +dReal dJointGetUniversalAngle1Rate (dJointID); +dReal dJointGetUniversalAngle2Rate (dJointID); +int dJointGetAMotorNumAxes (dJointID); +void dJointGetAMotorAxis (dJointID, int anum, dVector3 result); +int dJointGetAMotorAxisRel (dJointID, int anum); +dReal dJointGetAMotorAngle (dJointID, int anum); +dReal dJointGetAMotorAngleRate (dJointID, int anum); +dReal dJointGetAMotorParam (dJointID, int parameter); +int dJointGetAMotorMode (dJointID); + +dJointID dConnectingJoint (dBodyID, dBodyID); +int dConnectingJointList (dBodyID, dBodyID, dJointID*); +int dAreConnected (dBodyID, dBodyID); +int dAreConnectedExcluding (dBodyID, dBodyID, int joint_type); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_objects_private.h b/src/external/open_dynamics_engine-ef/ode/ode_objects_private.h new file mode 100644 index 00000000..a118a4d6 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_objects_private.h @@ -0,0 +1,125 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// object, body, and world structs. + + +#ifndef _ODE_OBJECT_H_ +#define _ODE_OBJECT_H_ + +#include "ode/ode_common.h" +#include "ode/ode_memory.h" +#include "ode/ode_mass.h" +#include "ode/ode_array.h" + + +// some body flags + +enum { + dxBodyFlagFiniteRotation = 1, // use finite rotations + dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis + dxBodyDisabled = 4, // body is disabled + dxBodyNoGravity = 8, // body is not influenced by gravity + dxBodyAutoDisable = 16 // enable auto-disable on body +}; + + +// base class that does correct object allocation / deallocation + +struct dBase { + void *operator new (size_t size) { return dAlloc (size); } + void operator delete (void *ptr, size_t size) { dFree (ptr,size); } + void *operator new[] (size_t size) { return dAlloc (size); } + void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); } +}; + + +// base class for bodies and joints + +struct dObject : public dBase { + dxWorld *world; // world this object is in + dObject *next; // next object of this type in list + dObject **tome; // pointer to previous object's next ptr + void *userdata; // user settable data + int tag; // used by dynamics algorithms +}; + + +// auto disable parameters +struct dxAutoDisable { + dReal linear_threshold; // linear (squared) velocity treshold + dReal angular_threshold; // angular (squared) velocity treshold + dReal idle_time; // time the body needs to be idle to auto-disable it + int idle_steps; // steps the body needs to be idle to auto-disable it +}; + + +// quick-step parameters +struct dxQuickStepParameters { + int num_iterations; // number of SOR iterations to perform + dReal w; // the SOR over-relaxation parameter +}; + + +// contact generation parameters +struct dxContactParameters { + dReal max_vel; // maximum correcting velocity + dReal min_depth; // thickness of 'surface layer' +}; + + +struct dxBody : public dObject { + dxJointNode *firstjoint; // list of attached joints + int flags; // some dxBodyFlagXXX flags + dGeomID geom; // first collision geom associated with body + dMass mass; // mass parameters about POR + dMatrix3 invI; // inverse of mass.I + dReal invMass; // 1 / mass.mass + dVector3 pos; // position of POR (point of reference) + dQuaternion q; // orientation quaternion + dMatrix3 R; // rotation matrix, always corresponds to q + dVector3 lvel,avel; // linear and angular velocity of POR + dVector3 facc,tacc; // force and torque accumulators + dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none + + // auto-disable information + dxAutoDisable adis; // auto-disable parameters + dReal adis_timeleft; // time left to be idle + int adis_stepsleft; // steps left to be idle +}; + + +struct dxWorld : public dBase { + dxBody *firstbody; // body linked list + dxJoint *firstjoint; // joint linked list + int nb,nj; // number of bodies and joints in lists + dVector3 gravity; // gravity vector (m/s/s) + dReal global_erp; // global error reduction parameter + dReal global_cfm; // global costraint force mixing parameter + dxAutoDisable adis; // auto-disable parameters + int adis_flag; // auto-disable flag for new bodies + dxQuickStepParameters qs; + dxContactParameters contactp; +}; + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_obstack.cpp b/src/external/open_dynamics_engine-ef/ode/ode_obstack.cpp new file mode 100644 index 00000000..fb13c674 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_obstack.cpp @@ -0,0 +1,138 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +// 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 + +#include "ode/ode_common.h" +#include "ode/ode_error.h" +#include "ode/ode_memory.h" +#include "ode/ode_obstack.h" + +//**************************************************************************** +// macros and constants + +#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \ + ofs = (size_t) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) ); + +#define MAX_ALLOC_SIZE \ + ((size_t)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1)) + +//**************************************************************************** +// dObStack + +dObStack::dObStack() +{ + first = 0; + last = 0; + current_arena = 0; + current_ofs = 0; +} + + +dObStack::~dObStack() +{ + // free all arenas + Arena *a,*nexta; + a = first; + while (a) { + nexta = a->next; + dFree (a,dOBSTACK_ARENA_SIZE); + a = nexta; + } +} + + +void *dObStack::alloc (int num_bytes) +{ + if ((size_t)num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large"); + + // allocate or move to a new arena if necessary + if (!first) { + // allocate the first arena if necessary + first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); + first->next = 0; + first->used = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used); + } + else { + // we already have one or more arenas, see if a new arena must be used + if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) { + if (!last->next) { + last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); + last->next->next = 0; + } + last = last->next; + last->used = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used); + } + } + + // allocate an area in the arena + char *c = ((char*) last) + last->used; + last->used += num_bytes; + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used); + return c; +} + + +void dObStack::freeAll() +{ + last = first; + if (first) { + first->used = sizeof(Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used); + } +} + + +void *dObStack::rewind() +{ + current_arena = first; + current_ofs = sizeof (Arena); + if (current_arena) { + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs) + return ((char*) current_arena) + current_ofs; + } + else return 0; +} + + +void *dObStack::next (int num_bytes) +{ + // this functions like alloc, except that no new storage is ever allocated + if (!current_arena) return 0; + current_ofs += num_bytes; + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs); + if (current_ofs >= current_arena->used) { + current_arena = current_arena->next; + if (!current_arena) return 0; + current_ofs = sizeof (Arena); + ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs); + } + return ((char*) current_arena) + current_ofs; +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_obstack.h b/src/external/open_dynamics_engine-ef/ode/ode_obstack.h new file mode 100644 index 00000000..8fb4ba42 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_obstack.h @@ -0,0 +1,68 @@ +/************************************************************************* + * * + * 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_OBSTACK_H_ +#define _ODE_OBSTACK_H_ + +#include "ode/ode_objects_private.h" + +// each obstack Arena pointer points to a block of this many bytes +#define dOBSTACK_ARENA_SIZE 16384 + + +struct dObStack : public dBase { + struct Arena { + Arena *next; // next arena in linked list + int used; // total number of bytes used in this arena, counting + }; // this header + + Arena *first; // head of the arena linked list. 0 if no arenas yet + Arena *last; // arena where blocks are currently being allocated + + // used for iterator + Arena *current_arena; + int current_ofs; + + dObStack(); + ~dObStack(); + + void *alloc (int num_bytes); + // allocate a block in the last arena, allocating a new arena if necessary. + // it is a runtime error if num_bytes is larger than the arena size. + + void freeAll(); + // free all blocks in all arenas. this does not deallocate the arenas + // themselves, so future alloc()s will reuse them. + + void *rewind(); + // rewind the obstack iterator, and return the address of the first + // allocated block. return 0 if there are no allocated blocks. + + void *next (int num_bytes); + // return the address of the next allocated block. 'num_bytes' is the size + // of the previous block. this returns null if there are no more arenas. + // the sequence of 'num_bytes' parameters passed to next() during a + // traversal of the list must exactly match the parameters passed to alloc(). +}; + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_quickstep.cpp b/src/external/open_dynamics_engine-ef/ode/ode_quickstep.cpp new file mode 100644 index 00000000..dabd486e --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_quickstep.cpp @@ -0,0 +1,1040 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 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. * + * * + *************************************************************************/ + +#include "ode/ode_objects_private.h" +#include "ode/ode_joint.h" +#include "ode/ode_config.h" +#include "ode/ode_math.h" +#include "ode/ode_rotation.h" +#include "ode/ode_timer.h" +#include "ode/ode_error.h" +#include "ode/ode_matrix.h" +#include "ode/ode_misc.h" +#include "ode/ode_lcp.h" +#include "ode/ode_util.h" +#include "ode/ode_misc.h" + +#define ALLOCA dALLOCA16 + +typedef const dReal *dRealPtr; +typedef dReal *dRealMutablePtr; +#define dRealArray(name,n) dReal name[n]; + +// ericf addition - a simple memory buffer +class _Buffer{ +public: + _Buffer():_ptr(NULL),_allocated(false){ + } + void allocate(unsigned long size){ + if (_allocated && _ptr) free(_ptr); + //dIASSERT(size > 0); + _ptr = malloc(size); + dIASSERT(_ptr); + _allocated = true; + } + ~_Buffer(){ + if (_allocated && _ptr) free(_ptr); + _ptr = NULL; + } + void* getPtr() const {return _ptr;} +private: + void* _ptr; + bool _allocated; +}; + + +// ...ericf update - we can shave a bit of time off by using our own bare-bones buffer class... +#define TRIXY_ALLOCA(name,type,n) _Buffer name ## BUF; if (n > 6000){name ## BUF.allocate((n)); name = (type*)name ## BUF.getPtr();} else {name = (type*)ALLOCA((n)); dIASSERT(name);} + +#define dRealAllocaArray(name,n) dReal *name; TRIXY_ALLOCA(name,dReal,n*sizeof(dReal)) +//#define dRealAllocaArray(name,n) dReal *name = (dReal*) ALLOCA ((n)*sizeof(dReal)); + + +// whether to use quickstep feedback patch submitted to the ode mailing +// list by Jaroslav Sinecky in June '05 +#define JUNE_05_PATCH 1 + +//*************************************************************************** +// configuration + +// for the SOR and CG methods: +// uncomment the following line to use warm starting. this definitely +// help for motor-driven joints. unfortunately it appears to hurt +// with high-friction contacts using the SOR method. use with care + +#define WARM_STARTING 0 + + +// for the SOR method: +// uncomment the following line to determine a new constraint-solving +// order for each iteration. however, the qsort per iteration is expensive, +// and the optimal order is somewhat problem dependent. +// @@@ try the leaf->root ordering. + +//#define REORDER_CONSTRAINTS 1 + + +// for the SOR method: +// uncomment the following line to randomly reorder constraint rows +// during the solution. depending on the situation, this can help a lot +// or hardly at all, but it doesn't seem to hurt. + +// ericf note: this was on for original release; trying it off to save processing.. +#define RANDOMLY_REORDER_CONSTRAINTS 0 + +#if JUNE_05_PATCH + +//**************************************************************************** +// special matrix multipliers + +// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q) +static void Multiply1_12q1 (dReal *A, dReal *B, dReal *C, int q) +{ + int k; + dReal sum; + dIASSERT (q>0 && A && B && C); + sum = 0; + for (k=0; kinvMass; + for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; + dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); + if (b2 >= 0) { + k = body[b2]->invMass; + for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; + dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); + } + J_ptr += 12; + iMJ_ptr += 12; + } +} + + +// compute out = inv(M)*J'*in. + +static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dSetZero (out,6*nb); + dRealPtr iMJ_ptr = iMJ; + for (i=0; i= 0) { + out_ptr = out + b2*6; + for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; + } + iMJ_ptr += 6; + } +} + + +// compute out = J*in. + +static void multiply_J (int m, dRealMutablePtr J, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dRealPtr J_ptr = J; + for (i=0; i= 0) { + in_ptr = in + b2*6; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + } + J_ptr += 6; + out[i] = sum; + } +} + + +// compute out = (J*inv(M)*J' + cfm)*in. +// use z as an nb*6 temporary. + +#if 0 +static void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, + dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out) +{ + multiply_invM_JT (m,nb,iMJ,jb,in,z); + multiply_J (m,J,jb,z,out); + + // add cfm + for (int i=0; inum_iterations; + + // precompute iMJ = inv(M)*J' + dRealAllocaArray (iMJ,m*12); + compute_invM_JT (m,J,iMJ,jb,body,invI); + + dReal last_rho = 0; + dRealAllocaArray (r,m); + dRealAllocaArray (z,m); + dRealAllocaArray (p,m); + dRealAllocaArray (q,m); + + // precompute 1 / diagonals of A + dRealAllocaArray (Ad,m); + dRealPtr iMJ_ptr = iMJ; + dRealPtr J_ptr = J; + for (i=0; i= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = REAL(1.0) / (sum + cfm[i]); + } + +#ifdef WARM_STARTING + // compute residual r = b - A*lambda + multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r); + for (i=0; ifindex < 0 && i2->findex >= 0) return -1; + if (i1->findex >= 0 && i2->findex < 0) return 1; + if (i1->error < i2->error) return -1; + if (i1->error > i2->error) return 1; + return 0; +} + +#endif + + +static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body, + dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b, + dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, + dxQuickStepParameters *qs) +{ + const int num_iterations = qs->num_iterations; + const dReal sor_w = qs->w; // SOR over-relaxation parameter + + int i,j; + +#ifdef WARM_STARTING + // for warm starting, this seems to be necessary to prevent + // jerkiness in motor-driven joints. i have no idea why this works. + for (i=0; i= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = sor_w / (sum + cfm[i]); + } + + // scale J and b by Ad + J_ptr = J; + for (i=0; i= 0) order[j++].index = i; + dIASSERT (j==m); +#endif + + for (int iteration=0; iteration < num_iterations; iteration++) { + +#ifdef REORDER_CONSTRAINTS + // constraints with findex < 0 always come first. + if (iteration < 2) { + // for the first two iterations, solve the constraints in + // the given order + for (i=0; i v2) ? v1 : v2; + if (max > 0) { + //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; + order[i].error = dFabs(lambda[i]-last_lambda[i]); + } + else { + order[i].error = dInfinity; + } + order[i].findex = findex[i]; + order[i].index = i; + } + } + qsort (order,m,sizeof(IndexError),&compare_index_error); +#endif + +//ericf: we save and restore the random seed here so each island is not affected by the +//existance of other islands +#ifdef RANDOMLY_REORDER_CONSTRAINTS + unsigned long oldSeed = dRandGetSeed(); + if ((iteration & 7) == 0) { + for (i=1; i= 0) { + hi[index] = dFabs (hicopy[index] * lambda[findex[index]]); + lo[index] = -hi[index]; + } + + int b1 = jb[index*2]; + int b2 = jb[index*2+1]; + dReal delta = b[index] - lambda[index]*Ad[index]; + dRealMutablePtr fc_ptr = fc + 6*b1; + + //DIFF HERE + + // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case + delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + + fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = fc + 6*b2; + delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + + fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + + fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; + } + + // compute lambda and clamp it to [lo,hi]. + // @@@ potential optimization: does SSE have clamping instructions + // to save test+jump penalties here? + dReal new_lambda = lambda[index] + delta; + if (new_lambda < lo[index]) { + delta = lo[index]-lambda[index]; + lambda[index] = lo[index]; + } + else if (new_lambda > hi[index]) { + delta = hi[index]-lambda[index]; + lambda[index] = hi[index]; + } + else { + lambda[index] = new_lambda; + } + + //@@@ a trick that may or may not help + //dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations)); + //delta *= ramp; + + + // update fc. + // @@@ potential optimization: SIMD for this and the b2 >= 0 case + fc_ptr = fc + 6*b1; + fc_ptr[0] += delta * iMJ_ptr[0]; + fc_ptr[1] += delta * iMJ_ptr[1]; + fc_ptr[2] += delta * iMJ_ptr[2]; + fc_ptr[3] += delta * iMJ_ptr[3]; + fc_ptr[4] += delta * iMJ_ptr[4]; + fc_ptr[5] += delta * iMJ_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = fc + 6*b2; + fc_ptr[0] += delta * iMJ_ptr[6]; + fc_ptr[1] += delta * iMJ_ptr[7]; + fc_ptr[2] += delta * iMJ_ptr[8]; + fc_ptr[3] += delta * iMJ_ptr[9]; + fc_ptr[4] += delta * iMJ_ptr[10]; + fc_ptr[5] += delta * iMJ_ptr[11]; + } + } + } +} + + +void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j; + IFTIMING(dTimerStart("preprocessing");) + + dReal stepsize1 = dRecip(stepsize); + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints + dxJoint **joint = (dxJoint**) alloca (nj * sizeof(dxJoint*)); + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. +#if !JUNE_05_PATCH + dRealAllocaArray (I,3*4*nb); // need to remember all I's for feedback purposes only +#endif + dRealAllocaArray (invI,3*4*nb); + for (i=0; imass.I,body[i]->R); +#if !JUNE_05_PATCH + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); +#else + dMULTIPLY0_333 (I,body[i]->R,tmp); +#endif + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + + // ERICF TEST +// #ifndef dNODEBUG +// { +// dReal testVal = (nb > 0)?dFabs(body[0]->lvel[0]):0.0; +// if (isnan(testVal) || isinf(testVal) || testVal > 9999.0f){ +// printf("ARGGGGG3!\n"); +// } +// } +// #endif + + +#define SIMPLE_ROTATION 1 + + +#if !SIMPLE_ROTATION + +#if !JUNE_05_PATCH + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); +#else + dMULTIPLY0_331 (tmp,I,body[i]->avel); +#endif + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); +#endif // SIMPLE_ROTATION + + } + + // { + // dReal testVal = (nb > 0)?dFabs(body[0]->facc[0]):0.0; + // if (isnan(testVal) || isinf(testVal) || testVal > 9999.0f){ + // printf("WHOOOOOAAAAAA!\n"); + // } + // } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + // else{ + // body[i]->facc[0] = 0; + // body[i]->facc[1] = 0; + // body[i]->facc[2] = 0; + // body[i]->lvel[0] = 0; + // body[i]->lvel[1] = 0; + // body[i]->lvel[2] = 0; + // body[i]->avel[0] = 0; + // body[i]->avel[1] = 0; + // body[i]->avel[2] = 0; + // } + } + + // get joint information (m = total constraint dimension, nub = number of unbounded variables). + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + //@@@ do we really need to save all the info1's + //printf("SIZE IS %d coutn is %d\n",sizeof(dxJoint::Info1),nj); + dxJoint::Info1 *info = (dxJoint::Info1*) alloca (nj*sizeof(dxJoint::Info1)); + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // create the row offset array + int m = 0; + int *ofs = (int*) alloca (nj*sizeof(int)); + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dRealAllocaArray (c,m); + dRealAllocaArray (cfm,m); + dRealAllocaArray (lo,m); + dRealAllocaArray (hi,m); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + + + for (i=0; ivtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + + } + +#if JUNE_05_PATCH + // we need a copy of Jacobian for joint feedbacks + // because it gets destroyed by SOR solver + // instead of saving all Jacobian, we can save just rows + // for joints, that requested feedback (which is normaly much less) + int mfb = 0; // number of rows we will have to save + for (i=0; ifeedback) + mfb += info[i].m; + dRealAllocaArray (Jcopy,mfb*12); + if (mfb > 0) { + mfb = 0; + for (i=0; ifeedback) { + memcpy(Jcopy+mfb*12, J+ofs[i]*12, info[i].m*12*sizeof(dReal)); + mfb += info[i].m; + } + } + + +#endif + + // create an array of body numbers for each joint row + int *jb_ptr = jb; + for (i=0; inode[0].body) ? (joint[i]->node[0].body->tag) : -1; + int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->tag) : -1; + for (j=0; jinvMass; + for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc); + for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1; + + } + + // put J*tmp1 into rhs + dRealAllocaArray (rhs,m); + multiply_J (m,J,jb,tmp1,rhs); + + // complete rhs + for (i=0; ilambda,info[i].m * sizeof(dReal)); + } +#endif + + + //check the stuff going into SOR_LCP + + // solve the LCP problem and get lambda and invM*constraint_force + IFTIMING (dTimerNow ("solving LCP problem");) + dRealAllocaArray (cforce,nb*6); + SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,&world->qs); + + // ERICF TEST + { +// dReal testVal = (nb > 0)?dFabs(cforce[0]):0.0; +// if (isnan(testVal) || isinf(testVal) || testVal > 9999.0f){ +// printf("ARGGGGG1!\n"); +// for (i=0; ilambda[0]); +// } +// printf("dn\n"); + +// } +// else{ + +// # ifdef WARM_STARTING +// // save lambda for the next iteration +// //@@@ note that this doesn't work for contact joints yet, as they are +// // recreated every iteration +// for (i=0; ilambda,lambda+ofs[i],info[i].m * sizeof(dReal)); +// } +// # endif + + // note that the SOR method overwrites rhs and J at this point, so + // they should not be used again. + + // add stepsize * cforce to the body velocity + for (i=0; ilvel[j] += stepsize * cforce[i*6+j]; + } + for (j=0; j<3; j++){ + body[i]->avel[j] += stepsize * cforce[i*6+3+j]; + } + } + // } + + // ERICF TEST +// #ifndef dNODEBUG +// { +// dReal testVal = (nb > 0)?dFabs(body[0]->lvel[0]):0.0; +// if (isnan(testVal) || isinf(testVal) || testVal > 9999.0f){ +// printf("ARGGGGGffff1! %f\n",cforce[0]); +// } +// } +// #endif + + + +#if !JUNE_05_PATCH + // if joint feedback is requested, compute the constraint force. + // BUT: cforce is inv(M)*J'*lambda, whereas we want just J'*lambda, + // so we must compute M*cforce. + // @@@ if any joint has a feedback request we compute the entire + // adjusted cforce, which is not the most efficient way to do it. + for (j=0; jfeedback) { + // compute adjusted cforce + for (i=0; imass.mass; + cforce [i*6+0] *= k; + cforce [i*6+1] *= k; + cforce [i*6+2] *= k; + dVector3 tmp; + dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3); + cforce [i*6+3] = tmp[0]; + cforce [i*6+4] = tmp[1]; + cforce [i*6+5] = tmp[2]; + } + // compute feedback for this and all remaining joints + for (; jfeedback; + if (fb) { + int b1 = joint[j]->node[0].body->tag; + memcpy (fb->f1,cforce+b1*6,3*sizeof(dReal)); + memcpy (fb->t1,cforce+b1*6+3,3*sizeof(dReal)); + if (joint[j]->node[1].body) { + int b2 = joint[j]->node[1].body->tag; + memcpy (fb->f2,cforce+b2*6,3*sizeof(dReal)); + memcpy (fb->t2,cforce+b2*6+3,3*sizeof(dReal)); + } + } + } + } + } +#else + if (mfb > 0) { + // straightforward computation of joint contraint forces: + // multiply related lambdas with respective J' block for joints + // where feedback was requested + mfb = 0; + for (i=0; ifeedback) { + dJointFeedback *fb = joint[i]->feedback; + dReal data[6]; + Multiply1_12q1 (data, Jcopy+mfb*12, lambda+ofs[i], info[i].m); + fb->f1[0] = data[0]; + fb->f1[1] = data[1]; + fb->f1[2] = data[2]; + fb->t1[0] = data[3]; + fb->t1[1] = data[4]; + fb->t1[2] = data[5]; + if (joint[i]->node[1].body) + { + Multiply1_12q1 (data, Jcopy+mfb*12+6, lambda+ofs[i], info[i].m); + fb->f2[0] = data[0]; + fb->f2[1] = data[1]; + fb->f2[2] = data[2]; + fb->t2[0] = data[3]; + fb->t2[1] = data[4]; + fb->t2[2] = data[5]; + } + mfb += info[i].m; + } + } + } +#endif + } + + } + + // compute the velocity update: + // add stepsize * invM * fe to the body velocity + + IFTIMING (dTimerNow ("compute velocity update");) + + // ERICF CHANGE - take a look at the velocity on our first body - if it looks like it exploded, + // ignore this entire update. + { + // dReal testVal = (nb > 0)?dFabs(body[0]->lvel[0] + (stepsize * body[0]->invMass * body[0]->facc[0])):0.0; + // if (isnan(testVal) || isinf(testVal) || testVal > 9999.0f){ + // printf("SKIPPPPPPPING!\n"); + // } + // else{ + for (i=0; iinvMass; + for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j]; + for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize; + dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc); +// #ifndef dNODEBUG +// for (j=0; j<3; j++){ +// if ((dFabs(body[i]->lvel[j]) > 9999.0f) || (dFabs(body[i]->avel[j]) > 9999.0f)){ +// printf("WTFFFFFFF!!!!\n"); +// } +// } +// #endif + } + // } + } + +#if 0 + // check that the updated velocity obeys the constraint (this check needs unmodified J) + dRealAllocaArray (vel,nb*6); + for (i=0; ilvel[j]; + for (j=0; j<3; j++) vel[i*6+3+j] = body[i]->avel[j]; + } + dRealAllocaArray (tmp,m); + multiply_J (m,J,jb,vel,tmp); + dReal error = 0; + for (i=0; ifacc,3); + dSetZero (body[i]->tacc,3); + } + + IFTIMING (dTimerEnd();) + IFTIMING (if (m > 0) dTimerReport (stdout,1);) +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_quickstep.h b/src/external/open_dynamics_engine-ef/ode/ode_quickstep.h new file mode 100644 index 00000000..c46cef53 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_quickstep.h @@ -0,0 +1,33 @@ +/************************************************************************* + * * + * 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_QUICK_STEP_H_ +#define _ODE_QUICK_STEP_H_ + +#include "ode/ode_common.h" + + +void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize); + + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_rotation.cpp b/src/external/open_dynamics_engine-ef/ode/ode_rotation.cpp new file mode 100644 index 00000000..ed9eae9f --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_rotation.cpp @@ -0,0 +1,304 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the +"rotation axis" and s is the "rotation angle". + +*/ + +#include "ode/ode_rotation.h" +#include "ode/ode_math.h" + + +#define _R(i,j) R[(i)*4+(j)] + +#define SET_3x3_IDENTITY \ + _R(0,0) = REAL(1.0); \ + _R(0,1) = REAL(0.0); \ + _R(0,2) = REAL(0.0); \ + _R(0,3) = REAL(0.0); \ + _R(1,0) = REAL(0.0); \ + _R(1,1) = REAL(1.0); \ + _R(1,2) = REAL(0.0); \ + _R(1,3) = REAL(0.0); \ + _R(2,0) = REAL(0.0); \ + _R(2,1) = REAL(0.0); \ + _R(2,2) = REAL(1.0); \ + _R(2,3) = REAL(0.0); + + +void dRSetIdentity (dMatrix3 R) +{ + dAASSERT (R); + SET_3x3_IDENTITY; +} + + +void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal angle) +{ + dAASSERT (R); + dQuaternion q; + dQFromAxisAndAngle (q,ax,ay,az,angle); + dQtoR (q,R); +} + + +void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi) +{ + dReal sphi,cphi,stheta,ctheta,spsi,cpsi; + dAASSERT (R); + sphi = dSin(phi); + cphi = dCos(phi); + stheta = dSin(theta); + ctheta = dCos(theta); + spsi = dSin(psi); + cpsi = dCos(psi); + _R(0,0) = cpsi*ctheta; + _R(0,1) = spsi*ctheta; + _R(0,2) =-stheta; + _R(1,0) = cpsi*stheta*sphi - spsi*cphi; + _R(1,1) = spsi*stheta*sphi + cpsi*cphi; + _R(1,2) = ctheta*sphi; + _R(2,0) = cpsi*stheta*cphi + spsi*sphi; + _R(2,1) = spsi*stheta*cphi - cpsi*sphi; + _R(2,2) = ctheta*cphi; +} + + +void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal bx, dReal by, dReal bz) +{ + dReal l,k; + dAASSERT (R); + l = dSqrt (ax*ax + ay*ay + az*az); + if (l <= REAL(0.0)) { + dDEBUGMSG ("zero length vector"); + return; + } + l = dRecip(l); + ax *= l; + ay *= l; + az *= l; + k = ax*bx + ay*by + az*bz; + bx -= k*ax; + by -= k*ay; + bz -= k*az; + l = dSqrt (bx*bx + by*by + bz*bz); + if (l <= REAL(0.0)) { + dDEBUGMSG ("zero length vector"); + return; + } + l = dRecip(l); + bx *= l; + by *= l; + bz *= l; + _R(0,0) = ax; + _R(1,0) = ay; + _R(2,0) = az; + _R(0,1) = bx; + _R(1,1) = by; + _R(2,1) = bz; + _R(0,2) = - by*az + ay*bz; + _R(1,2) = - bz*ax + az*bx; + _R(2,2) = - bx*ay + ax*by; +} + + +void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az) +{ + dVector3 n,p,q; + n[0] = ax; + n[1] = ay; + n[2] = az; + dNormalize3 (n); + dPlaneSpace (n,p,q); + _R(0,0) = p[0]; + _R(1,0) = p[1]; + _R(2,0) = p[2]; + _R(0,1) = q[0]; + _R(1,1) = q[1]; + _R(2,1) = q[2]; + _R(0,2) = n[0]; + _R(1,2) = n[1]; + _R(2,2) = n[2]; +} + + +void dQSetIdentity (dQuaternion q) +{ + dAASSERT (q); + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; +} + + +void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, + dReal angle) +{ + dAASSERT (q); + dReal l = ax*ax + ay*ay + az*az; + if (l > REAL(0.0)) { + angle *= REAL(0.5); + q[0] = dCos (angle); + l = dSin(angle) * dRecipSqrt(l); + q[1] = ax*l; + q[2] = ay*l; + q[3] = az*l; + } + else { + q[0] = 1; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } +} + + +void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; + qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; + qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; + qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; +} + + +void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; + qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; + qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; + qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; +} + + +void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; + qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; + qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; + qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; +} + + +void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) +{ + dAASSERT (qa && qb && qc); + qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; + qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; + qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; + qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; +} + + +// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction +// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained +// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon +// University, 1997. + +void dRfromQ (dMatrix3 R, const dQuaternion q) +{ + dAASSERT (q && R); + // q = (s,vx,vy,vz) + dReal qq1 = 2*q[1]*q[1]; + dReal qq2 = 2*q[2]*q[2]; + dReal qq3 = 2*q[3]*q[3]; + _R(0,0) = 1 - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1 - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1 - qq1 - qq2; +} + + +void dQfromR (dQuaternion q, const dMatrix3 R) +{ + dAASSERT (q && R); + dReal tr,s; + tr = _R(0,0) + _R(1,1) + _R(2,2); + if (tr >= 0) { + s = dSqrt (tr + 1); + q[0] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[1] = (_R(2,1) - _R(1,2)) * s; + q[2] = (_R(0,2) - _R(2,0)) * s; + q[3] = (_R(1,0) - _R(0,1)) * s; + } + else { + // find the largest diagonal element and jump to the appropriate case + if (_R(1,1) > _R(0,0)) { + if (_R(2,2) > _R(1,1)) goto case_2; + goto case_1; + } + if (_R(2,2) > _R(0,0)) goto case_2; + goto case_0; + + case_0: + s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1); + q[1] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[2] = (_R(0,1) + _R(1,0)) * s; + q[3] = (_R(2,0) + _R(0,2)) * s; + q[0] = (_R(2,1) - _R(1,2)) * s; + return; + + case_1: + s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1); + q[2] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[3] = (_R(1,2) + _R(2,1)) * s; + q[1] = (_R(0,1) + _R(1,0)) * s; + q[0] = (_R(0,2) - _R(2,0)) * s; + return; + + case_2: + s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1); + q[3] = REAL(0.5) * s; + s = REAL(0.5) * dRecip(s); + q[1] = (_R(2,0) + _R(0,2)) * s; + q[2] = (_R(1,2) + _R(2,1)) * s; + q[0] = (_R(1,0) - _R(0,1)) * s; + return; + } +} + + +void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q) +{ + dAASSERT (w && q && dq); + dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]); + dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]); + dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]); + dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_rotation.h b/src/external/open_dynamics_engine-ef/ode/ode_rotation.h new file mode 100644 index 00000000..b8879dfe --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_rotation.h @@ -0,0 +1,70 @@ +/************************************************************************* + * * + * 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_ROTATION_H_ +#define _ODE_ROTATION_H_ + +#include "ode/ode_common.h" +#include "ode/ode_compatibility.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +void dRSetIdentity (dMatrix3 R); + +void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal angle); + +void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi); + +void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, + dReal bx, dReal by, dReal bz); + +void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az); + +void dQSetIdentity (dQuaternion q); + +void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, + dReal angle); + +/* Quaternion multiplication, analogous to the matrix multiplication routines. */ +/* qa = rotate by qc, then qb */ +void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); +/* qa = rotate by qc, then by inverse of qb */ +void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); +/* qa = rotate by inverse of qc, then by qb */ +void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); +/* qa = rotate by inverse of qc, then by inverse of qb */ +void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); + +void dRfromQ (dMatrix3 R, const dQuaternion q); +void dQfromR (dQuaternion q, const dMatrix3 R); +void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/ode_step.cpp b/src/external/open_dynamics_engine-ef/ode/ode_step.cpp new file mode 100644 index 00000000..61803d2c --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_step.cpp @@ -0,0 +1,1786 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +#include "ode/ode_objects_private.h" +#include "ode/ode_joint.h" +#include "ode/ode_config.h" +#include "ode/ode_math.h" +#include "ode/ode_rotation.h" +#include "ode/ode_timer.h" +#include "ode/ode_error.h" +#include "ode/ode_matrix.h" +#include "ode/ode_lcp.h" +#include "ode/ode_util.h" + +//**************************************************************************** +// misc defines + +#define FAST_FACTOR +//#define TIMING + +// memory allocation system +#ifdef dUSE_MALLOC_FOR_ALLOCA +unsigned int dMemoryFlag; +#define REPORT_OUT_OF_MEMORY fprintf(stderr, "Insufficient memory to complete rigid body simulation. Results will not be accurate.\n") + +#define ALLOCA(t,v,s) t* v=(t*)malloc(s) +#define UNALLOCA(t) free(t) + +#else +#define ALLOCA(t,v,s) t* v=(t*)dALLOCA16(s) +#define UNALLOCA(t) /* nothing */ +#endif + + +//**************************************************************************** +// debugging - comparison of various vectors and matrices produced by the +// slow and fast versions of the stepper. + +//#define COMPARE_METHODS + +#ifdef COMPARE_METHODS +#include "ode/ode_testing.h" +dMatrixComparison comparator; +#endif + +// undef to use the fast decomposition +#define DIRECT_CHOLESKY +#undef REPORT_ERROR + +//**************************************************************************** +// special matrix multipliers + +// this assumes the 4th and 8th rows of B and C are zero. + +static void Multiply2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) = sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B and C are zero. + +static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) += sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q) +{ + int k; + dReal sum; + dIASSERT (q>0 && A && B && C); + sum = 0; + for (k=0; k0 && A && B && C); + sum = 0; + for (k=0; ktag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (joint == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. + // @@@ check computation of rotational force. + ALLOCA(dReal,I,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (I == NULL) { + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,invI,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (invI == NULL) { + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->R); + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + int m = 0; + ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (info == NULL) { + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + ALLOCA(int,ofs,nj*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ofs == NULL) { + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; iinvMass; + MM[nskip+1] = body[i]->invMass; + MM[2*nskip+2] = body[i]->invMass; + MM += 3*nskip+3; + for (j=0; j<3; j++) for (k=0; k<3; k++) { + MM[j*nskip+k] = invI[i*12+j*4+k]; + } + } + + // assemble some body vectors: fe = external forces, v = velocities + ALLOCA(dReal,fe,n6*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (fe == NULL) { + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + ALLOCA(dReal,v,n6*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (v == NULL) { + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + //dSetZero (fe,n6); + //dSetZero (v,n6); + for (i=0; ifacc[j]; + for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j]; + for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j]; + for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j]; + } + + // this will be set to the velocity update + ALLOCA(dReal,vnew,n6*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (vnew == NULL) { + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (vnew,n6); + + // if there are constraints, compute cforce + if (m > 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + ALLOCA(dReal,c,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (c == NULL) { + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,cfm,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (cfm == NULL) { + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,lo,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo == NULL) { + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,hi,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi == NULL) { + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(int,findex,m*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (findex == NULL) { + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; inode[0].body->tag; + Jinfo.J1a = Jinfo.J1l + 3; + if (joint[i]->node[1].body) { + Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag; + Jinfo.J2a = Jinfo.J2l + 3; + } + else { + Jinfo.J2l = 0; + Jinfo.J2a = 0; + } + Jinfo.c = c + ofs[i]; + Jinfo.cfm = cfm + ofs[i]; + Jinfo.lo = lo + ofs[i]; + Jinfo.hi = hi + ofs[i]; + Jinfo.findex = findex + ofs[i]; + joint[i]->vtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J' +# ifdef TIMING + dTimerNow ("compute A"); +# endif + ALLOCA(dReal,JinvM,m*nskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (JinvM == NULL) { + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + //dSetZero (JinvM,m*nskip); + dMultiply0 (JinvM,J,invM,m,n6,n6); + int mskip = dPAD(m); + ALLOCA(dReal,A,m*mskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A == NULL) { + UNALLOCA(JinvM); + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(vnew); + UNALLOCA(v); + UNALLOCA(fe); + UNALLOCA(invM); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + //dSetZero (A,m*mskip); + dMultiply2 (A,JinvM,J,m,n6,m); + + // add cfm to the diagonal of A + for (i=0; ilvel[j] = vnew[i*6+j]; + for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j]; + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +#ifdef TIMING + dTimerNow ("update position"); +#endif + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +#ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +#endif + + UNALLOCA(joint); + UNALLOCA(I); + UNALLOCA(invI); + UNALLOCA(info); + UNALLOCA(ofs); + UNALLOCA(invM); + UNALLOCA(fe); + UNALLOCA(v); + UNALLOCA(vnew); +} + +//**************************************************************************** +// an optimized version of dInternalStepIsland1() + +void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j,k; +#ifdef TIMING + dTimerStart("preprocessing"); +#endif + + dReal stepsize1 = dRecip(stepsize); + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (joint == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + ALLOCA(dReal,I,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (I == NULL) { + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,invI,3*nb*4*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (invI == NULL) { + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->R); + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (info == NULL) { + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(int,ofs,nj*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (ofs == NULL) { + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + joint[i]->tag = i; + i++; + } + else { + joint[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + ALLOCA(dReal,c,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (c == NULL) { + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,cfm,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (cfm == NULL) { + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,lo,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (lo == NULL) { + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(dReal,hi,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (hi == NULL) { + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + ALLOCA(int,findex,m*sizeof(int)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (findex == NULL) { + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; ivtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + ALLOCA(dReal,JinvM,2*m*8*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (JinvM == NULL) { + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (JinvM,2*m*8); + for (i=0; inode[0].body->tag; + dReal body_invMass = body[b]->invMass; + dReal *body_invI = invI + b*12; + dReal *Jsrc = J + 2*8*ofs[i]; + dReal *Jdst = JinvM + 2*8*ofs[i]; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + if (joint[i]->node[1].body) { + b = joint[i]->node[1].body->tag; + body_invMass = body[b]->invMass; + body_invI = invI + b*12; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + } + } + + // now compute A = JinvM * J'. A's rows and columns are grouped by joint, + // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero + // if joints i and j have at least one body in common. this fact suggests + // the algorithm used to fill A: + // + // for b = all bodies + // n = number of joints attached to body b + // for i = 1..n + // for j = i+1..n + // ii = actual joint number for i + // jj = actual joint number for j + // // (ii,jj) will be set to all pairs of joints around body b + // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)' + // + // this algorithm catches all pairs of joints that have at least one body + // in common. it does not compute the diagonal blocks of A however - + // another similar algorithm does that. + + int mskip = dPAD(m); + ALLOCA(dReal,A,m*mskip*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (A == NULL) { + UNALLOCA(JinvM); + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + dSetZero (A,m*mskip); + for (i=0; ifirstjoint; n1; n1=n1->next) { + for (dxJointNode *n2=n1->next; n2; n2=n2->next) { + // get joint numbers and ensure ofs[j1] >= ofs[j2] + int j1 = n1->joint->tag; + int j2 = n2->joint->tag; + if (ofs[j1] < ofs[j2]) { + int tmp = j1; + j1 = j2; + j2 = tmp; + } + + // if either joint was tagged as -1 then it is an inactive (m=0) + // joint that should not be considered + if (j1==-1 || j2==-1) continue; + + // determine if body i is the 1st or 2nd body of joints j1 and j2 + int jb1 = (joint[j1]->node[1].body == body[i]); + int jb2 = (joint[j2]->node[1].body == body[i]); + // jb1/jb2 must be 0 for joints with only one body + dIASSERT(joint[j1]->node[1].body || jb1==0); + dIASSERT(joint[j2]->node[1].body || jb2==0); + + // set block of A + MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2], + JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m, + J + 2*8*ofs[j2] + jb2*8*info[j2].m, + info[j1].m,info[j2].m, mskip); + } + } + } + // compute diagonal blocks of A + for (i=0; inode[1].body) { + MultiplyAdd2_p8r (A + ofs[i]*(mskip+1), + JinvM + 2*8*ofs[i] + 8*info[i].m, + J + 2*8*ofs[i] + 8*info[i].m, + info[i].m,info[i].m, mskip); + } + } + + // add cfm to the diagonal of A + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass + + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc); + for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + ALLOCA(dReal,rhs,m*sizeof(dReal)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (rhs == NULL) { + UNALLOCA(tmp1); + UNALLOCA(A); + UNALLOCA(JinvM); + UNALLOCA(J); + UNALLOCA(findex); + UNALLOCA(hi); + UNALLOCA(lo); + UNALLOCA(cfm); + UNALLOCA(c); + UNALLOCA(cforce); + UNALLOCA(ofs); + UNALLOCA(info); + UNALLOCA(invI); + UNALLOCA(I); + UNALLOCA(joint); + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + return; + } +#endif + //dSetZero (rhs,m); + for (i=0; inode[0].body->tag, info[i].m); + if (joint[i]->node[1].body) { + MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m, + tmp1 + 8*joint[i]->node[1].body->tag, info[i].m); + } + } + // complete rhs + for (i=0; inode[0].body; + dxBody* b2 = joint[i]->node[1].body; + dJointFeedback *fb = joint[i]->feedback; + + if (fb) { + // the user has requested feedback on the amount of force that this + // joint is applying to the bodies. we use a slightly slower + // computation that splits out the force components and puts them + // in the feedback structure. + dReal data1[8],data2[8]; + Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); + dReal *cf1 = cforce + 8*b1->tag; + cf1[0] += (fb->f1[0] = data1[0]); + cf1[1] += (fb->f1[1] = data1[1]); + cf1[2] += (fb->f1[2] = data1[2]); + cf1[4] += (fb->t1[0] = data1[4]); + cf1[5] += (fb->t1[1] = data1[5]); + cf1[6] += (fb->t1[2] = data1[6]); + if (b2){ + Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + dReal *cf2 = cforce + 8*b2->tag; + cf2[0] += (fb->f2[0] = data2[0]); + cf2[1] += (fb->f2[1] = data2[1]); + cf2[2] += (fb->f2[2] = data2[2]); + cf2[4] += (fb->t2[0] = data2[4]); + cf2[5] += (fb->t2[1] = data2[5]); + cf2[6] += (fb->t2[2] = data2[6]); + } + } + else { + // no feedback is required, let's compute cforce the faster way + MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m); + if (b2) { + MultiplyAdd1_8q1 (cforce + 8*b2->tag, + JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + } + } + } + UNALLOCA(c); + UNALLOCA(cfm); + UNALLOCA(lo); + UNALLOCA(hi); + UNALLOCA(findex); + UNALLOCA(J); + UNALLOCA(JinvM); + UNALLOCA(A); + UNALLOCA(tmp1); + UNALLOCA(rhs); + UNALLOCA(lambda); + UNALLOCA(residual); + } + + // compute the velocity update +#ifdef TIMING + dTimerNow ("compute velocity update"); +#endif + + // add fe to cforce + for (i=0; ifacc[j]; + for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j]; + } + // multiply cforce by stepsize + for (i=0; i < nb*8; i++) cforce[i] *= stepsize; + // add invM * cforce to the body velocity + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j]; + dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4); + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +# ifdef TIMING + dTimerNow ("update position"); +# endif + for (i=0; ilvel[j]; + for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j]; + } + comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew"); + UNALLOCA(tmp); +#endif + +#ifdef TIMING + dTimerNow ("tidy up"); +#endif + + // zero all force accumulators + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +#ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +#endif + + UNALLOCA(joint); + UNALLOCA(I); + UNALLOCA(invI); + UNALLOCA(info); + UNALLOCA(ofs); + UNALLOCA(cforce); +} + +//**************************************************************************** + +void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *joint, int nj, dReal stepsize) +{ + +#ifdef dUSE_MALLOC_FOR_ALLOCA + dMemoryFlag = d_MEMORY_OK; +#endif + +#ifndef COMPARE_METHODS + dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); + +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { + REPORT_OUT_OF_MEMORY; + return; + } +#endif + +#endif + +#ifdef COMPARE_METHODS + int i; + + // save body state + ALLOCA(dxBody,state,nb*sizeof(dxBody)); +#ifdef dUSE_MALLOC_FOR_ALLOCA + if (state == NULL) { + dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; + REPORT_OUT_OF_MEMORY; + return; + } +#endif + for (i=0; i 0) + autoEnableDepth = autodepth; + else + autoEnableDepth = 0; +} + +int dWorldGetAutoEnableDepthSF1 (dxWorld *world) +{ + return autoEnableDepth; +} + +//little bit of math.... the _sym_ functions assume the return matrix will be symmetric +static void +Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) = *ad = sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + +static void +MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) += sum; + *ad += sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q) +{ + int k; + dReal sum; + dIASSERT (q > 0 && A && B && C); + sum = 0; + for (k = 0; k < q; k++) + sum += B[k * 8] * C[k]; + A[0] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[1 + k * 8] * C[k]; + A[1] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[2 + k * 8] * C[k]; + A[2] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[4 + k * 8] * C[k]; + A[4] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[5 + k * 8] * C[k]; + A[5] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[6 + k * 8] * C[k]; + A[6] = sum; +} + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal +sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs (x) < 1.0e-4) + return REAL (1.0) - x * x * REAL (0.166666666666666666667); + else + return dSin (x) / x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +static inline void +moveAndRotateBody (dxBody * b, dReal h) +{ + int j; + + // handle linear velocity + for (j = 0; j < 3; j++) + b->pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) + { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv, irv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis, b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL (0.5); + dReal theta = k * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else + { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); + h *= REAL (0.5); + dReal theta = wlen * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2, q, b->q); + for (j = 0; j < 4; j++) + b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + dReal dq[4]; + dWtoDQ (irv, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + } + else + { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q, b->R); + + // notify all attached geoms that this body has moved + for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +//This is an implementation of the iterated/relaxation algorithm. +//Here is a quick overview of the algorithm per Sergi Valverde's posts to the +//mailing list: +// +// for i=0..N-1 do +// for c = 0..C-1 do +// Solve constraint c-th +// Apply forces to constraint bodies +// next +// next +// Integrate bodies + +void +dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) +{ + int i, j, k; +# ifdef TIMING + dTimerNow ("constraint preprocessing"); +# endif + + dReal stepsize1 = dRecip (stepsize); + + int m = info.m; + // nothing to do if no constraints. + if (m <= 0) + return; + + int nub = 0; + if (info.nub == info.m) + nub = m; + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal JinvM[2 * 6 * 8]; + //dSetZero (JinvM, 2 * m * 8); + + dReal *Jsrc = Jinfo.J1l; + dReal *Jdst = JinvM; + if (body[0]) + { + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[0]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); + Jsrc += 8; + Jdst += 8; + } + } + if (body[1]) + { + Jsrc = Jinfo.J2l; + Jdst = JinvM + 8 * m; + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[1]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); + Jsrc += 8; + Jdst += 8; + } + } + + + // now compute A = JinvM * J'. + int mskip = dPAD (m); + dReal A[6 * 8]; + //dSetZero (A, 6 * 8); + + if (body[0]) { + Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); + if (body[1]) + MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, + m, mskip); + } else { + if (body[1]) + Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, + m, mskip); + } + + // add cfm to the diagonal of A + for (i = 0; i < m; i++) + A[i * mskip + i] += Jinfo.cfm[i] * stepsize1; + + // compute the right hand side `rhs' +# ifdef TIMING + dTimerNow ("compute rhs"); +# endif + dReal tmp1[16]; + //dSetZero (tmp1, 16); + // put v/h + invM*fe into tmp1 + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); + for (j = 0; j < 3; j++) + tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + dReal rhs[6]; + //dSetZero (rhs, 6); + + if (body[0]) { + Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); + if (body[1]) + MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); + } else { + if (body[1]) + Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); + } + + // complete rhs + for (i = 0; i < m; i++) + rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i]; + +#ifdef SLOW_LCP + // solve the LCP problem and get lambda. + // this will destroy A but that's okay +# ifdef TIMING + dTimerNow ("solving LCP problem"); +# endif + dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal lo[6], hi[6]; + memcpy (lo, Jinfo.lo, m * sizeof (dReal)); + memcpy (hi, Jinfo.hi, m * sizeof (dReal)); + dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); +#endif + + // LCP Solver replacement: + // This algorithm goes like this: + // Do a straightforward LDLT factorization of the matrix A, solving for + // A*x = rhs + // For each x[i] that is outside of the bounds of lo[i] and hi[i], + // clamp x[i] into that range. + // Substitute into A the now known x's + // subtract the residual away from the rhs. + // Remove row and column i from L, updating the factorization + // place the known x's at the end of the array, keeping up with location in p + // Repeat until all constraints have been clamped or all are within bounds + // + // This is probably only faster in the single joint case where only one repeat is + // the norm. + +#ifdef FAST_FACTOR + // factorize A (L*D*L'=A) +# ifdef TIMING + dTimerNow ("factorize A"); +# endif + dReal d[6]; + dReal L[6 * 8]; + memcpy (L, A, m * mskip * sizeof (dReal)); + dFactorLDLT (L, d, m, mskip); + + // compute lambda +# ifdef TIMING + dTimerNow ("compute lambda"); +# endif + + int left = m; //constraints left to solve. + int remove[6]; + dReal lambda[6]; + dReal x[6]; + int p[6]; + for (i = 0; i < 6; i++) + p[i] = i; + while (true) + { + memcpy (x, rhs, left * sizeof (dReal)); + dSolveLDLT (L, d, x, left, mskip); + + int fixed = 0; + for (i = 0; i < left; i++) + { + j = p[i]; + remove[i] = false; + // This isn't the exact same use of findex as dSolveLCP.... since x[findex] + // may change after I've already clamped x[i], but it should be close + if (Jinfo.findex[j] > -1) + { + dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]); + if (x[i] > f) + x[i] = f; + else if (x[i] < -f) + x[i] = -f; + else + continue; + } + else + { + if (x[i] > Jinfo.hi[j]) + x[i] = Jinfo.hi[j]; + else if (x[i] < Jinfo.lo[j]) + x[i] = Jinfo.lo[j]; + else + continue; + } + remove[i] = true; + fixed++; + } + if (fixed == 0 || fixed == left) //no change or all constraints solved + break; + + for (i = 0; i < left; i++) //sub in to right hand side. + if (remove[i]) + for (j = 0; j < left; j++) + if (!remove[j]) + rhs[j] -= A[j * mskip + i] * x[i]; + + for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables + { + if (remove[r]) + { + //dRemoveLDLT adapted for use without row pointers. + if (r == left - 1) + { + left--; + continue; // deleting last row/col is easy + } + else if (r == 0) + { + dReal a[6]; + for (i = 0; i < left; i++) + a[i] = -A[i * mskip]; + a[0] += REAL (1.0); + dLDLTAddTL (L, d, a, left, mskip); + } + else + { + dReal t[6]; + dReal a[6]; + for (i = 0; i < r; i++) + t[i] = L[r * mskip + i] / d[i]; + for (i = 0; i < left - r; i++) + a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r]; + a[0] += REAL (1.0); + dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip); + } + + dRemoveRowCol (L, left, mskip, r); + //end dRemoveLDLT + + left--; + if (r < (left - 1)) + { + dReal tx = x[r]; + memmove (d + r, d + r + 1, (left - r) * sizeof (dReal)); + memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal)); + //x will get written over by rhs anyway, no need to move it around + //just store the fixed value we just discovered in it. + x[left] = tx; + for (i = 0; i < m; i++) + if (p[i] > r && p[i] <= left) + p[i]--; + p[r] = left; + } + } + } + } + + for (i = 0; i < m; i++) + lambda[i] = x[p[i]]; +# endif + // compute the constraint force `cforce' +# ifdef TIMING + dTimerNow ("compute constraint force"); +#endif + + // compute cforce = J'*lambda + dJointFeedback *fb = joint->feedback; + dReal cforce[16]; + //dSetZero (cforce, 16); + + if (fb) + { + // the user has requested feedback on the amount of force that this + // joint is applying to the bodies. we use a slightly slower + // computation that splits out the force components and puts them + // in the feedback structure. + dReal data1[8], data2[8]; + if (body[0]) + { + Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); + dReal *cf1 = cforce; + cf1[0] = (fb->f1[0] = data1[0]); + cf1[1] = (fb->f1[1] = data1[1]); + cf1[2] = (fb->f1[2] = data1[2]); + cf1[4] = (fb->t1[0] = data1[4]); + cf1[5] = (fb->t1[1] = data1[5]); + cf1[6] = (fb->t1[2] = data1[6]); + } + if (body[1]) + { + Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); + dReal *cf2 = cforce + 8; + cf2[0] = (fb->f2[0] = data2[0]); + cf2[1] = (fb->f2[1] = data2[1]); + cf2[2] = (fb->f2[2] = data2[2]); + cf2[4] = (fb->t2[0] = data2[4]); + cf2[5] = (fb->t2[1] = data2[5]); + cf2[6] = (fb->t2[2] = data2[6]); + } + } + else + { + // no feedback is required, let's compute cforce the faster way + if (body[0]) + Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); + if (body[1]) + Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); + } + + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + { + body[i]->facc[j] += cforce[i * 8 + j]; + body[i]->tacc[j] += cforce[i * 8 + 4 + j]; + } + } +} + +void +dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) +{ +# ifdef TIMING + dTimerNow ("preprocessing"); +# endif + dxBody *bodyPair[2], *body; + dReal *GIPair[2], *GinvIPair[2]; + dxJoint *joint; + int iter, b, j, i; + dReal ministep = stepsize / maxiterations; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); + memcpy (joints, _joints, nj * sizeof (dxJoint *)); + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); + int *ofs = (int *) ALLOCA (nj * sizeof (int)); + for (i = 0, j = 0; j < nj; j++) + { // i=dest, j=src + joints[j]->vtable->getInfo1 (joints[j], info + i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) + { + joints[i] = joints[j]; + joints[i]->tag = i; + i++; + } + else + { + joints[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i = 0; i < nj; i++) + { + ofs[i] = m; + m += info[i].m; + } + dReal *c = NULL; + dReal *cfm = NULL; + dReal *lo = NULL; + dReal *hi = NULL; + int *findex = NULL; + + dReal *J = NULL; + dxJoint::Info2 * Jinfo = NULL; + + if (m) + { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + c = (dReal *) ALLOCA (m * sizeof (dReal)); + cfm = (dReal *) ALLOCA (m * sizeof (dReal)); + lo = (dReal *) ALLOCA (m * sizeof (dReal)); + hi = (dReal *) ALLOCA (m * sizeof (dReal)); + findex = (int *) ALLOCA (m * sizeof (int)); + dSetZero (c, m); + dSetValue (cfm, m, world->global_cfm); + dSetValue (lo, m, -dInfinity); + dSetValue (hi, m, dInfinity); + for (i = 0; i < m; i++) + findex[i] = -1; + + // get jacobian data from constraints. a (2*m)x8 matrix will be created + // to store the two jacobian blocks from each constraint. it has this + // format: + // + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) + // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) + // etc... + // + // (lll) = linear jacobian data + // (aaa) = angular jacobian data + // +# ifdef TIMING + dTimerNow ("create J"); +# endif + J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); + dSetZero (J, 2 * m * 8); + Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); + for (i = 0; i < nj; i++) + { + Jinfo[i].rowskip = 8; + Jinfo[i].fps = dRecip (stepsize); + Jinfo[i].erp = world->global_erp; + Jinfo[i].J1l = J + 2 * 8 * ofs[i]; + Jinfo[i].J1a = Jinfo[i].J1l + 4; + Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; + Jinfo[i].J2a = Jinfo[i].J2l + 4; + Jinfo[i].c = c + ofs[i]; + Jinfo[i].cfm = cfm + ofs[i]; + Jinfo[i].lo = lo + ofs[i]; + Jinfo[i].hi = hi + ofs[i]; + Jinfo[i].findex = findex + ofs[i]; + //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); + } + + } + + dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + for (b = 0; b < nb; b++) + { + for (i = 0; i < 4; i++) + { + saveFacc[b * 4 + i] = bodies[b]->facc[i]; + saveTacc[b * 4 + i] = bodies[b]->tacc[i]; + } + bodies[b]->tag = b; + } + + for (iter = 0; iter < maxiterations; iter++) + { +# ifdef TIMING + dTimerNow ("applying inertia and gravity"); +# endif + dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->mass.I, body->R); + dMULTIPLY0_333 (globalI + b * 12, body->R, tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->invI, body->R); + dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp); + + for (i = 0; i < 4; i++) + body->tacc[i] = saveTacc[b * 4 + i]; + // compute rotational force + dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel); + dCROSS (body->tacc, -=, body->avel, tmp); + + // add the gravity force to all bodies + if ((body->flags & dxBodyNoGravity) == 0) + { + body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0]; + body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1]; + body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2]; + body->facc[3] = 0; + } else { + body->facc[0] = saveFacc[b * 4 + 0]; + body->facc[1] = saveFacc[b * 4 + 1]; + body->facc[2] = saveFacc[b * 4 + 2]; + body->facc[3] = 0; + } + + } + +#ifdef RANDOM_JOINT_ORDER +#ifdef TIMING + dTimerNow ("randomizing joint order"); +#endif + //randomize the order of the joints by looping through the array + //and swapping the current joint pointer with a random one before it. + for (j = 0; j < nj; j++) + { + joint = joints[j]; + dxJoint::Info1 i1 = info[j]; + dxJoint::Info2 i2 = Jinfo[j]; + const int r = dRandInt(j+1); + joints[j] = joints[r]; + info[j] = info[r]; + Jinfo[j] = Jinfo[r]; + joints[r] = joint; + info[r] = i1; + Jinfo[r] = i2; + } +#endif + + //now iterate through the random ordered joint array we created. + for (j = 0; j < nj; j++) + { +#ifdef TIMING + dTimerNow ("setting up joint"); +#endif + joint = joints[j]; + bodyPair[0] = joint->node[0].body; + bodyPair[1] = joint->node[1].body; + + if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) + bodyPair[0] = 0; + if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) + bodyPair[1] = 0; + + //if this joint is not connected to any enabled bodies, skip it. + if (!bodyPair[0] && !bodyPair[1]) + continue; + + if (bodyPair[0]) + { + GIPair[0] = globalI + bodyPair[0]->tag * 12; + GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; + } + if (bodyPair[1]) + { + GIPair[1] = globalI + bodyPair[1]->tag * 12; + GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; + } + + joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); + + //dInternalStepIslandFast is an exact copy of the old routine with one + //modification: the calculated forces are added back to the facc and tacc + //vectors instead of applying them to the bodies and moving them. + if (info[j].m > 0) + { + dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); + } + } + // } +# ifdef TIMING + dTimerNow ("moving bodies"); +# endif + //Now we can simulate all the free floating bodies, and move them. + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + for (i = 0; i < 4; i++) + { + body->facc[i] *= ministep; + body->tacc[i] *= ministep; + } + + //apply torque + dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); + + //apply force + for (i = 0; i < 3; i++) + body->lvel[i] += body->invMass * body->facc[i]; + + //move It! + moveAndRotateBody (body, ministep); + } + } + for (b = 0; b < nb; b++) + for (j = 0; j < 4; j++) + bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; +} + + +#ifdef NO_ISLANDS + +// Since the iterative algorithm doesn't care about islands of bodies, this is a +// faster algorithm that just sends it all the joints and bodies in one array. +// It's downfall is it's inability to handle disabled bodies as well as the old one. +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ + // nothing to do if no bodies + if (world->nb <= 0) + return; + + dInternalHandleAutoDisabling (world,stepsize); + +# ifdef TIMING + dTimerStart ("creating joint and body arrays"); +# endif + dxBody **bodies, *body; + dxJoint **joints, *joint; + joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + + int nj = 0; + for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) + joints[nj++] = joint; + + int nb = 0; + for (body = world->firstbody; body; body = (dxBody *) body->next) + bodies[nb++] = body; + + dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations); +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#else + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ +#ifdef TIMING + dTimerStart ("Island Setup"); +#endif + dxBody *b, *bb, **body; + dxJoint *j, **joint; + + // nothing to do if no bodies + if (world->nb <= 0) + return; + + dInternalHandleAutoDisabling (world,stepsize); + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + int tbcount = 0; + int tjcount = 0; + + // set all body/joint tags to 0 + for (b = world->firstbody; b; b = (dxBody *) b->next) + b->tag = 0; + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *)); + int *autostack = (int *) ALLOCA (stackalloc * sizeof (int)); + + for (bb = world->firstbody; bb; bb = (dxBody *) bb->next) + { +#ifdef TIMING + dTimerNow ("Island Processing"); +#endif + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) + continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + int autoDepth = autoEnableDepth; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) + { + b = stack[--stacksize]; // pop body off stack + autoDepth = autostack[stacksize]; + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode * n = b->firstjoint; n; n = n->next) + { + if (!n->joint->tag) + { + int thisDepth = autoEnableDepth; + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) + { + if (n->body->flags & dxBodyDisabled) + thisDepth = autoDepth - 1; + if (thisDepth < 0) + continue; + n->body->flags &= ~dxBodyDisabled; + n->body->tag = 1; + autostack[stacksize] = thisDepth; + stack[stacksize++] = n->body; + } + } + } + dIASSERT (stacksize <= world->nb); + dIASSERT (stacksize <= world->nj); + } + + // now do something with body and joint lists + dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i = 0; i < bcount; i++) + { + body[i]->tag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i = 0; i < jcount; i++) + joint[i]->tag = 1; + + tbcount += bcount; + tjcount += jcount; + } + +#ifdef TIMING + dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount); +#endif + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b = world->firstbody; b; b = (dxBody *) b->next) + { + if (b->flags & dxBodyDisabled) + { + if (b->tag) + dDebug (0, "disabled body tagged"); + } + else + { + if (!b->tag) + dDebug (0, "enabled body not tagged"); + } + } + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0)) + { + if (!j->tag) + dDebug (0, "attached enabled joint not tagged"); + } + else + { + if (j->tag) + dDebug (0, "unattached or disabled joint tagged"); + } + } +# endif + +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#endif + + +void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations) +{ + dUASSERT (w, "bad world argument"); + dUASSERT (stepsize > 0, "stepsize must be > 0"); + processIslandsFast (w, stepsize, maxiterations); +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_timer.cpp b/src/external/open_dynamics_engine-ef/ode/ode_timer.cpp new file mode 100644 index 00000000..38997392 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_timer.cpp @@ -0,0 +1,427 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* + +TODO +---- + +* gettimeofday() and the pentium time stamp counter return the real time, + not the process time. fix this somehow! + +*/ + +#include "ode/ode_common.h" +#include "ode/ode_timer.h" + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// implementation for windows based on the multimedia performance counter. + +// #ifdef WIN32 +#if _WIN32 || _WIN64 + + +#include "windows.h" + +static inline void getClockCount (unsigned long cc[2]) +{ + LARGE_INTEGER a; + QueryPerformanceCounter (&a); + cc[0] = a.LowPart; + cc[1] = a.HighPart; +} + + +static inline void serialize() +{ +} + + +static inline double loadClockCount (unsigned long cc[2]) +{ + LARGE_INTEGER a; + a.LowPart = cc[0]; + a.HighPart = cc[1]; + return double(a.QuadPart); +} + + +double dTimerResolution() +{ + return 1.0/dTimerTicksPerSecond(); +} + + +double dTimerTicksPerSecond() +{ + static int query=0; + static double hz=0.0; + if (!query) { + LARGE_INTEGER a; + QueryPerformanceFrequency (&a); + hz = double(a.QuadPart); + query = 1; + } + return hz; +} + + +// Gonna go ahead and deactivate this funky assembly business since we get by +// fine without it on arm and elsewhere elsewhere. + +// //**************************************************************************** +// // implementation based on the pentium time stamp counter. the timer functions +// // can be serializing or non-serializing. serializing will ensure that all +// // instructions have executed and data has been written back before the cpu +// // time stamp counter is read. the CPUID instruction is used to serialize. + +// #if defined(PENTIUM) && !defined(WIN32) + +// // we need to know the clock rate so that the timing function can report +// // accurate times. this number only needs to be set accurately if we're +// // doing performance tests and care about real-world time numbers - otherwise, +// // just ignore this. i have not worked out how to determine this number +// // automatically yet. + +// #define PENTIUM_HZ (500e6) + +// static inline void getClockCount (unsigned long cc[2]) +// { +// #ifndef SYS64bits +// asm volatile ( +// "rdtsc\n" +// "movl %%eax,(%%esi)\n" +// "movl %%edx,4(%%esi)\n" +// : : "S" (cc) : "%eax","%edx","cc","memory"); +// #else +// asm volatile ( +// "rdtsc\n" +// "movl %%eax,(%%rsi)\n" +// "movl %%edx,4(%%rsi)\n" +// : : "S" (cc) : "%eax","%edx","cc","memory"); +// #endif +// } + + +// static inline void serialize() +// { +// #ifndef SYS64bits +// asm volatile ( + +// // PATCH TO GET WORKING ON INTEL MAC +// //"mov $0,%%eax\n" +// "mov %%ebx,%%edi\n" +// "cpuid\n" +// //: : : "%eax","%ebx","%ecx","%edx","cc","memory"); +// : : : "%eax","%edi","%ecx","%edx","cc","memory"); +// #else +// asm volatile ( +// "mov $0,%%rax\n" +// "cpuid\n" +// : : : "%rax","%rbx","%rcx","%rdx","cc","memory"); +// #endif +// } + + +// static inline double loadClockCount (unsigned long a[2]) +// { +// double ret; +// #ifndef SYS64bits +// asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : +// "cc","memory"); +// #else +// asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : +// "cc","memory"); +// #endif +// return ret; +// } + + +// double dTimerResolution() +// { +// return 1.0/PENTIUM_HZ; +// } + + +// double dTimerTicksPerSecond() +// { +// return PENTIUM_HZ; +// } + +// #endif + +// //**************************************************************************** +// // otherwise, do the implementation based on gettimeofday(). + +// #if !defined(PENTIUM) && !defined(WIN32) + +#else + +#ifndef macintosh + +#include +#include + + +static inline void getClockCount (unsigned long cc[2]) +{ + struct timeval tv; + gettimeofday (&tv,0); + cc[0] = tv.tv_usec; + cc[1] = tv.tv_sec; +} + +#else // macintosh + +#include +#include + +static inline void getClockCount (unsigned long cc[2]) +{ + UnsignedWide ms; + Microseconds (&ms); + cc[1] = ms.lo / 1000000; + cc[0] = ms.lo - ( cc[1] * 1000000 ); +} + +#endif + + +static inline void serialize() +{ +} + + +static inline double loadClockCount (unsigned long a[2]) +{ + return a[1]*1.0e6 + a[0]; +} + + +double dTimerResolution() +{ + unsigned long cc1[2],cc2[2]; + getClockCount (cc1); + do { + getClockCount (cc2); + } + while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); + do { + getClockCount (cc1); + } + while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); + double t1 = loadClockCount (cc1); + double t2 = loadClockCount (cc2); + return (t1-t2) / dTimerTicksPerSecond(); +} + + +double dTimerTicksPerSecond() +{ + return 1000000; +} + +#endif + +//**************************************************************************** +// stop watches + +void dStopwatchReset (dStopwatch *s) +{ + s->time = 0; + s->cc[0] = 0; + s->cc[1] = 0; +} + + +void dStopwatchStart (dStopwatch *s) +{ + serialize(); + getClockCount (s->cc); +} + + +void dStopwatchStop (dStopwatch *s) +{ + unsigned long cc[2]; + serialize(); + getClockCount (cc); + double t1 = loadClockCount (s->cc); + double t2 = loadClockCount (cc); + s->time += t2-t1; +} + + +double dStopwatchTime (dStopwatch *s) +{ + return s->time / dTimerTicksPerSecond(); +} + +//**************************************************************************** +// code timers + +// maximum number of events to record +#define MAXNUM 100 + +static int num = 0; // number of entries used in event array +static struct { + unsigned long cc[2]; // clock counts + double total_t; // total clocks used in this slot. + double total_p; // total percentage points used in this slot. + int count; // number of times this slot has been updated. + const char *description; // pointer to static string +} event[MAXNUM]; + + +// make sure all slot totals and counts reset to 0 at start + +static void initSlots() +{ + static int initialized=0; + if (!initialized) { + for (int i=0; i (description); + num = 1; + serialize(); + getClockCount (event[0].cc); +} + + +void dTimerNow (const char *description) +{ + if (num < MAXNUM) { + // do not serialize + getClockCount (event[num].cc); + event[num].description = const_cast (description); + num++; + } +} + + +void dTimerEnd() +{ + if (num < MAXNUM) { + serialize(); + getClockCount (event[num].cc); + event[num].description = "TOTAL"; + num++; + } +} + +//**************************************************************************** +// print report + +static void fprintDoubleWithPrefix (FILE *f, double a, const char *fmt) +{ + if (a >= 0.999999) { + fprintf (f,fmt,a); + return; + } + a *= 1000.0; + if (a >= 0.999999) { + fprintf (f,fmt,a); + fprintf (f,"m"); + return; + } + a *= 1000.0; + if (a >= 0.999999) { + fprintf (f,fmt,a); + fprintf (f,"u"); + return; + } + a *= 1000.0; + fprintf (f,fmt,a); + fprintf (f,"n"); +} + + +void dTimerReport (FILE *fout, int average) +{ + int i; + size_t maxl; + double ccunit = 1.0/dTimerTicksPerSecond(); + fprintf (fout,"\nTimer Report ("); + fprintDoubleWithPrefix (fout,ccunit,"%.2f "); + fprintf (fout,"s resolution)\n------------\n"); + if (num < 1) return; + + // get maximum description length + maxl = 0; + for (i=0; i maxl) maxl = l; + } + + // calculate total time + double t1 = loadClockCount (event[0].cc); + double t2 = loadClockCount (event[num-1].cc); + double total = t2 - t1; + if (total <= 0) total = 1; + + // compute time difference for all slots except the last one. update totals + double *times = (double*) ALLOCA (num * sizeof(double)); + for (i=0; i < (num-1); i++) { + double t1 = loadClockCount (event[i].cc); + double t2 = loadClockCount (event[i+1].cc); + times[i] = t2 - t1; + event[i].count++; + event[i].total_t += times[i]; + event[i].total_p += times[i]/total * 100.0; + } + + // print report (with optional averages) + for (i=0; i + +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// Auto disabling + +void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize) +{ + dxBody *bb; + for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { + // nothing to do unless this body is currently enabled and has + // the auto-disable flag set + if ((bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable) continue; + + // see if the body is idle + int idle = 1; // initial assumption + dReal lspeed2 = dDOT(bb->lvel,bb->lvel); + if (lspeed2 > bb->adis.linear_threshold) { + idle = 0; // moving fast - not idle + } + else { + dReal aspeed = dDOT(bb->avel,bb->avel); + if (aspeed > bb->adis.angular_threshold) { + idle = 0; // turning fast - not idle + } + } + + // if it's idle, accumulate steps and time. + // these counters won't overflow because this code doesn't run for disabled bodies. + if (idle) { + bb->adis_stepsleft--; + bb->adis_timeleft -= stepsize; + } + else { + bb->adis_stepsleft = bb->adis.idle_steps; + bb->adis_timeleft = bb->adis.idle_time; + } + + // disable the body if it's idle for a long enough time + if (bb->adis_stepsleft < 0 && bb->adis_timeleft < 0) { + bb->flags |= dxBodyDisabled; + + // ericf tweak - kill force and motion + // bb->lvel[0] = bb->lvel[1] = bb->lvel[2] = bb->avel[0] = bb->avel[1] = bb->avel[2] = 0.0; + // bb->facc[0] = bb->facc[1] = bb->facc[2] = 0.0; + } + } +} + + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667); + else return dSin(x)/x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +void dxStepBody (dxBody *b, dReal h) +{ + int j; + + // handle linear velocity + for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis,b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL(0.5); + dReal theta = k * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else { + + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + + b->avel[2]*b->avel[2]); + h *= REAL(0.5); + dReal theta = wlen * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2,q,b->q); + for (j=0; j<4; j++) b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) { + dReal dq[4]; + dWtoDQ (irv,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + } + else { + + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper) +{ + dxBody *b,*bb,**body; + dxJoint *j,**joint; + + // nothing to do if no bodies + if (world->nb <= 0) return; + + // handle auto-disabling of bodies + dInternalHandleAutoDisabling (world,stepsize); + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*)); + joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + + // set all body/joint tags to 0 + for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; + for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); + + for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) { + b = stack[--stacksize]; // pop body off stack + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (!n->joint->tag) { + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) { + n->body->tag = 1; + stack[stacksize++] = n->body; + } + } + } + dIASSERT(stacksize <= world->nb); + dIASSERT(stacksize <= world->nj); + } + + // now do something with body and joint lists + stepper (world,body,bcount,joint,jcount,stepsize); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i=0; itag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i=0; itag = 1; + } + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b=world->firstbody; b; b=(dxBody*)b->next) { + if (b->flags & dxBodyDisabled) { + if (b->tag) dDebug (0,"disabled body tagged"); + } + else { + if (!b->tag) dDebug (0,"enabled body not tagged"); + } + } + for (j=world->firstjoint; j; j=(dxJoint*)j->next) { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || + (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { + if (!j->tag) dDebug (0,"attached enabled joint not tagged"); + } + else { + if (j->tag) dDebug (0,"unattached or disabled joint tagged"); + } + } +# endif +} diff --git a/src/external/open_dynamics_engine-ef/ode/ode_util.h b/src/external/open_dynamics_engine-ef/ode/ode_util.h new file mode 100644 index 00000000..a950bb9d --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/ode_util.h @@ -0,0 +1,56 @@ +/************************************************************************* + * * + * 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_UTIL_H_ +#define _ODE_UTIL_H_ + +#include "ode/ode_objects_private.h" + + +void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize); +void dxStepBody (dxBody *b, dReal h); + +typedef void (*dstepper_fn_t) (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize); + +void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper); + + + +/////////ERIC ADDED STUFF////////////////////// +#define VALUE_TESTING 0 + +#if VALUE_TESTING +extern FILE *f; +extern bool testLogging; +void odeTestEnableLogging(); +void odeTestDisableLogging(); + +#define PRINT_DBL_HEX(x) {int *poo = (int*)&(x); fprintf(f,"0x%X%X\n",poo[0],poo[1]);} +#define PRINT_DBL_HEX_STDOUT(x) {int *poo = (int*)&(x); printf("0x%X%X\n",poo[0],poo[1]);} +void printBody(dxBody* b, int index); +void printJoint(dxJoint* j, int index); +#endif + +////////////////////////////////////////////// + +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/odecpp.h b/src/external/open_dynamics_engine-ef/ode/odecpp.h new file mode 100644 index 00000000..a694caf7 --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/odecpp.h @@ -0,0 +1,621 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* C++ interface for non-collision stuff */ + + +#ifndef _ODE_ODECPP_H_ +#define _ODE_ODECPP_H_ +#ifdef __cplusplus + +#include "ode/ode_error.h" + + +class dWorld { + dWorldID _id; + + // intentionally undefined, don't use these + dWorld (const dWorld &); + void operator= (const dWorld &); + +public: + dWorld() + { _id = dWorldCreate(); } + ~dWorld() + { dWorldDestroy (_id); } + + dWorldID id() const + { return _id; } + operator dWorldID() const + { return _id; } + + void setGravity (dReal x, dReal y, dReal z) + { dWorldSetGravity (_id,x,y,z); } + void getGravity (dVector3 g) const + { dWorldGetGravity (_id,g); } + + void setERP (dReal erp) + { dWorldSetERP(_id, erp); } + dReal getERP() const + { return dWorldGetERP(_id); } + + void setCFM (dReal cfm) + { dWorldSetCFM(_id, cfm); } + dReal getCFM() const + { return dWorldGetCFM(_id); } + + void step (dReal stepsize) + { dWorldStep (_id,stepsize); } + + void stepFast1 (dReal stepsize, int maxiterations) + { dWorldStepFast1 (_id,stepsize,maxiterations); } + void setAutoEnableDepthSF1(dWorldID, int depth) + { dWorldSetAutoEnableDepthSF1 (_id, depth); } + int getAutoEnableDepthSF1(dWorldID) + { return dWorldGetAutoEnableDepthSF1 (_id); } + + void setAutoDisableLinearThreshold (dReal threshold) + { dWorldSetAutoDisableLinearThreshold (_id,threshold); } + dReal getAutoDisableLinearThreshold() + { return dWorldGetAutoDisableLinearThreshold (_id); } + void setAutoDisableAngularThreshold (dReal threshold) + { dWorldSetAutoDisableAngularThreshold (_id,threshold); } + dReal getAutoDisableAngularThreshold() + { return dWorldGetAutoDisableAngularThreshold (_id); } + void setAutoDisableSteps (int steps) + { dWorldSetAutoDisableSteps (_id,steps); } + int getAutoDisableSteps() + { return dWorldGetAutoDisableSteps (_id); } + void setAutoDisableTime (dReal time) + { dWorldSetAutoDisableTime (_id,time); } + dReal getAutoDisableTime() + { return dWorldGetAutoDisableTime (_id); } + void setAutoDisableFlag (int do_auto_disable) + { dWorldSetAutoDisableFlag (_id,do_auto_disable); } + int getAutoDisableFlag() + { return dWorldGetAutoDisableFlag (_id); } + + void impulseToForce (dReal stepsize, dReal ix, dReal iy, dReal iz, + dVector3 force) + { dWorldImpulseToForce (_id,stepsize,ix,iy,iz,force); } +}; + + +class dBody { + dBodyID _id; + + // intentionally undefined, don't use these + dBody (const dBody &); + void operator= (const dBody &); + +public: + dBody() + { _id = 0; } + dBody (dWorldID world) + { _id = dBodyCreate (world); } + ~dBody() + { if (_id) dBodyDestroy (_id); } + + void create (dWorldID world) { + if (_id) dBodyDestroy (_id); + _id = dBodyCreate (world); + } + + dBodyID id() const + { return _id; } + operator dBodyID() const + { return _id; } + + void setData (void *data) + { dBodySetData (_id,data); } + void *getData() const + { return dBodyGetData (_id); } + + void setPosition (dReal x, dReal y, dReal z) + { dBodySetPosition (_id,x,y,z); } + void setRotation (const dMatrix3 R) + { dBodySetRotation (_id,R); } + void setQuaternion (const dQuaternion q) + { dBodySetQuaternion (_id,q); } + void setLinearVel (dReal x, dReal y, dReal z) + { dBodySetLinearVel (_id,x,y,z); } + void setAngularVel (dReal x, dReal y, dReal z) + { dBodySetAngularVel (_id,x,y,z); } + + const dReal * getPosition() const + { return dBodyGetPosition (_id); } + const dReal * getRotation() const + { return dBodyGetRotation (_id); } + const dReal * getQuaternion() const + { return dBodyGetQuaternion (_id); } + const dReal * getLinearVel() const + { return dBodyGetLinearVel (_id); } + const dReal * getAngularVel() const + { return dBodyGetAngularVel (_id); } + + void setMass (const dMass *mass) + { dBodySetMass (_id,mass); } + void getMass (dMass *mass) const + { dBodyGetMass (_id,mass); } + + void addForce (dReal fx, dReal fy, dReal fz) + { dBodyAddForce (_id, fx, fy, fz); } + void addTorque (dReal fx, dReal fy, dReal fz) + { dBodyAddTorque (_id, fx, fy, fz); } + void addRelForce (dReal fx, dReal fy, dReal fz) + { dBodyAddRelForce (_id, fx, fy, fz); } + void addRelTorque (dReal fx, dReal fy, dReal fz) + { dBodyAddRelTorque (_id, fx, fy, fz); } + void addForceAtPos (dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) + { dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); } + void addForceAtRelPos (dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) + { dBodyAddForceAtRelPos (_id, fx, fy, fz, px, py, pz); } + void addRelForceAtPos (dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) + { dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); } + void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) + { dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); } + + const dReal * getForce() const + { return dBodyGetForce(_id); } + const dReal * getTorque() const + { return dBodyGetTorque(_id); } + void setForce (dReal x, dReal y, dReal z) + { dBodySetForce (_id,x,y,z); } + void setTorque (dReal x, dReal y, dReal z) + { dBodySetTorque (_id,x,y,z); } + + void enable() + { dBodyEnable (_id); } + void disable() + { dBodyDisable (_id); } + int isEnabled() const + { return dBodyIsEnabled (_id); } + + void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result) const + { dBodyGetRelPointPos (_id, px, py, pz, result); } + void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result) const + { dBodyGetRelPointVel (_id, px, py, pz, result); } + void getPointVel (dReal px, dReal py, dReal pz, dVector3 result) const + { dBodyGetPointVel (_id,px,py,pz,result); } + void getPosRelPoint (dReal px, dReal py, dReal pz, dVector3 result) const + { dBodyGetPosRelPoint (_id,px,py,pz,result); } + void vectorToWorld (dReal px, dReal py, dReal pz, dVector3 result) const + { dBodyVectorToWorld (_id,px,py,pz,result); } + void vectorFromWorld (dReal px, dReal py, dReal pz, dVector3 result) const + { dBodyVectorFromWorld (_id,px,py,pz,result); } + + void setFiniteRotationMode (int mode) + { dBodySetFiniteRotationMode (_id, mode); } + void setFiniteRotationAxis (dReal x, dReal y, dReal z) + { dBodySetFiniteRotationAxis (_id, x, y, z); } + + int getFiniteRotationMode() const + { return dBodyGetFiniteRotationMode (_id); } + void getFiniteRotationAxis (dVector3 result) const + { dBodyGetFiniteRotationAxis (_id, result); } + + int getNumJoints() const + { return dBodyGetNumJoints (_id); } + dJointID getJoint (int index) const + { return dBodyGetJoint (_id, index); } + + void setGravityMode (int mode) + { dBodySetGravityMode (_id,mode); } + int getGravityMode() const + { return dBodyGetGravityMode (_id); } + + int isConnectedTo (dBodyID body) const + { return dAreConnected (_id, body); } + + void setAutoDisableLinearThreshold (dReal threshold) + { dBodySetAutoDisableLinearThreshold (_id,threshold); } + dReal getAutoDisableLinearThreshold() + { return dBodyGetAutoDisableLinearThreshold (_id); } + void setAutoDisableAngularThreshold (dReal threshold) + { dBodySetAutoDisableAngularThreshold (_id,threshold); } + dReal getAutoDisableAngularThreshold() + { return dBodyGetAutoDisableAngularThreshold (_id); } + void setAutoDisableSteps (int steps) + { dBodySetAutoDisableSteps (_id,steps); } + int getAutoDisableSteps() + { return dBodyGetAutoDisableSteps (_id); } + void setAutoDisableTime (dReal time) + { dBodySetAutoDisableTime (_id,time); } + dReal getAutoDisableTime() + { return dBodyGetAutoDisableTime (_id); } + void setAutoDisableFlag (int do_auto_disable) + { dBodySetAutoDisableFlag (_id,do_auto_disable); } + int getAutoDisableFlag() + { return dBodyGetAutoDisableFlag (_id); } +}; + + +class dJointGroup { + dJointGroupID _id; + + // intentionally undefined, don't use these + dJointGroup (const dJointGroup &); + void operator= (const dJointGroup &); + +public: + dJointGroup (int dummy_arg=0) + { _id = dJointGroupCreate (0); } + ~dJointGroup() + { dJointGroupDestroy (_id); } + void create (int dummy_arg=0) { + if (_id) dJointGroupDestroy (_id); + _id = dJointGroupCreate (0); + } + + dJointGroupID id() const + { return _id; } + operator dJointGroupID() const + { return _id; } + + void empty() + { dJointGroupEmpty (_id); } +}; + + +class dJoint { +private: + // intentionally undefined, don't use these + dJoint (const dJoint &) ; + void operator= (const dJoint &); + +protected: + dJointID _id; + +public: + dJoint() + { _id = 0; } + ~dJoint() + { if (_id) dJointDestroy (_id); } + + dJointID id() const + { return _id; } + operator dJointID() const + { return _id; } + + void attach (dBodyID body1, dBodyID body2) + { dJointAttach (_id, body1, body2); } + + void setData (void *data) + { dJointSetData (_id, data); } + void *getData() const + { return dJointGetData (_id); } + + int getType() const + { return dJointGetType (_id); } + + dBodyID getBody (int index) const + { return dJointGetBody (_id, index); } +}; + + +class dBallJoint : public dJoint { +private: + // intentionally undefined, don't use these + dBallJoint (const dBallJoint &); + void operator= (const dBallJoint &); + +public: + dBallJoint() { } + dBallJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateBall (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateBall (world, group); + } + + void setAnchor (dReal x, dReal y, dReal z) + { dJointSetBallAnchor (_id, x, y, z); } + void getAnchor (dVector3 result) const + { dJointGetBallAnchor (_id, result); } + void getAnchor2 (dVector3 result) const + { dJointGetBallAnchor2 (_id, result); } +} ; + + +class dHingeJoint : public dJoint { + // intentionally undefined, don't use these + dHingeJoint (const dHingeJoint &); + void operator = (const dHingeJoint &); + +public: + dHingeJoint() { } + dHingeJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateHinge (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateHinge (world, group); + } + + void setAnchor (dReal x, dReal y, dReal z) + { dJointSetHingeAnchor (_id, x, y, z); } + void getAnchor (dVector3 result) const + { dJointGetHingeAnchor (_id, result); } + void getAnchor2 (dVector3 result) const + { dJointGetHingeAnchor2 (_id, result); } + + void setAxis (dReal x, dReal y, dReal z) + { dJointSetHingeAxis (_id, x, y, z); } + void getAxis (dVector3 result) const + { dJointGetHingeAxis (_id, result); } + + dReal getAngle() const + { return dJointGetHingeAngle (_id); } + dReal getAngleRate() const + { return dJointGetHingeAngleRate (_id); } + + void setParam (int parameter, dReal value) + { dJointSetHingeParam (_id, parameter, value); } + dReal getParam (int parameter) const + { return dJointGetHingeParam (_id, parameter); } + + void addTorque (dReal torque) + { dJointAddHingeTorque(_id, torque); } +}; + + +class dSliderJoint : public dJoint { + // intentionally undefined, don't use these + dSliderJoint (const dSliderJoint &); + void operator = (const dSliderJoint &); + +public: + dSliderJoint() { } + dSliderJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateSlider (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateSlider (world, group); + } + + void setAxis (dReal x, dReal y, dReal z) + { dJointSetSliderAxis (_id, x, y, z); } + void getAxis (dVector3 result) const + { dJointGetSliderAxis (_id, result); } + + dReal getPosition() const + { return dJointGetSliderPosition (_id); } + dReal getPositionRate() const + { return dJointGetSliderPositionRate (_id); } + + void setParam (int parameter, dReal value) + { dJointSetSliderParam (_id, parameter, value); } + dReal getParam (int parameter) const + { return dJointGetSliderParam (_id, parameter); } + + void addForce (dReal force) + { dJointAddSliderForce(_id, force); } +}; + + +class dUniversalJoint : public dJoint { + // intentionally undefined, don't use these + dUniversalJoint (const dUniversalJoint &); + void operator = (const dUniversalJoint &); + +public: + dUniversalJoint() { } + dUniversalJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateUniversal (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateUniversal (world, group); + } + + void setAnchor (dReal x, dReal y, dReal z) + { dJointSetUniversalAnchor (_id, x, y, z); } + void setAxis1 (dReal x, dReal y, dReal z) + { dJointSetUniversalAxis1 (_id, x, y, z); } + void setAxis2 (dReal x, dReal y, dReal z) + { dJointSetUniversalAxis2 (_id, x, y, z); } + void setParam (int parameter, dReal value) + { dJointSetUniversalParam (_id, parameter, value); } + + void getAnchor (dVector3 result) const + { dJointGetUniversalAnchor (_id, result); } + void getAnchor2 (dVector3 result) const + { dJointGetUniversalAnchor2 (_id, result); } + void getAxis1 (dVector3 result) const + { dJointGetUniversalAxis1 (_id, result); } + void getAxis2 (dVector3 result) const + { dJointGetUniversalAxis2 (_id, result); } + dReal getParam (int parameter) const + { return dJointGetUniversalParam (_id, parameter); } + dReal getAngle1() const + { return dJointGetUniversalAngle1 (_id); } + dReal getAngle1Rate() const + { return dJointGetUniversalAngle1Rate (_id); } + dReal getAngle2() const + { return dJointGetUniversalAngle2 (_id); } + dReal getAngle2Rate() const + { return dJointGetUniversalAngle2Rate (_id); } + + void addTorques (dReal torque1, dReal torque2) + { dJointAddUniversalTorques(_id, torque1, torque2); } +}; + + +class dHinge2Joint : public dJoint { + // intentionally undefined, don't use these + dHinge2Joint (const dHinge2Joint &); + void operator = (const dHinge2Joint &); + +public: + dHinge2Joint() { } + dHinge2Joint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateHinge2 (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateHinge2 (world, group); + } + + void setAnchor (dReal x, dReal y, dReal z) + { dJointSetHinge2Anchor (_id, x, y, z); } + void setAxis1 (dReal x, dReal y, dReal z) + { dJointSetHinge2Axis1 (_id, x, y, z); } + void setAxis2 (dReal x, dReal y, dReal z) + { dJointSetHinge2Axis2 (_id, x, y, z); } + + void getAnchor (dVector3 result) const + { dJointGetHinge2Anchor (_id, result); } + void getAnchor2 (dVector3 result) const + { dJointGetHinge2Anchor2 (_id, result); } + void getAxis1 (dVector3 result) const + { dJointGetHinge2Axis1 (_id, result); } + void getAxis2 (dVector3 result) const + { dJointGetHinge2Axis2 (_id, result); } + + dReal getAngle1() const + { return dJointGetHinge2Angle1 (_id); } + dReal getAngle1Rate() const + { return dJointGetHinge2Angle1Rate (_id); } + dReal getAngle2Rate() const + { return dJointGetHinge2Angle2Rate (_id); } + + void setParam (int parameter, dReal value) + { dJointSetHinge2Param (_id, parameter, value); } + dReal getParam (int parameter) const + { return dJointGetHinge2Param (_id, parameter); } + + void addTorques(dReal torque1, dReal torque2) + { dJointAddHinge2Torques(_id, torque1, torque2); } +}; + + +class dFixedJoint : public dJoint { + // intentionally undefined, don't use these + dFixedJoint (const dFixedJoint &); + void operator = (const dFixedJoint &); + +public: + dFixedJoint() { } + dFixedJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateFixed (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateFixed (world, group); + } + + void set() + { dJointSetFixed (_id); } +}; + + +class dContactJoint : public dJoint { + // intentionally undefined, don't use these + dContactJoint (const dContactJoint &); + void operator = (const dContactJoint &); + +public: + dContactJoint() { } + dContactJoint (dWorldID world, dJointGroupID group, dContact *contact) + { _id = dJointCreateContact (world, group, contact); } + + void create (dWorldID world, dJointGroupID group, dContact *contact) { + if (_id) dJointDestroy (_id); + _id = dJointCreateContact (world, group, contact); + } +}; + + +class dNullJoint : public dJoint { + // intentionally undefined, don't use these + dNullJoint (const dNullJoint &); + void operator = (const dNullJoint &); + +public: + dNullJoint() { } + dNullJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateNull (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateNull (world, group); + } +}; + + +class dAMotorJoint : public dJoint { + // intentionally undefined, don't use these + dAMotorJoint (const dAMotorJoint &); + void operator = (const dAMotorJoint &); + +public: + dAMotorJoint() { } + dAMotorJoint (dWorldID world, dJointGroupID group=0) + { _id = dJointCreateAMotor (world, group); } + + void create (dWorldID world, dJointGroupID group=0) { + if (_id) dJointDestroy (_id); + _id = dJointCreateAMotor (world, group); + } + + void setMode (int mode) + { dJointSetAMotorMode (_id, mode); } + int getMode() const + { return dJointGetAMotorMode (_id); } + + void setNumAxes (int num) + { dJointSetAMotorNumAxes (_id, num); } + int getNumAxes() const + { return dJointGetAMotorNumAxes (_id); } + + void setAxis (int anum, int rel, dReal x, dReal y, dReal z) + { dJointSetAMotorAxis (_id, anum, rel, x, y, z); } + void getAxis (int anum, dVector3 result) const + { dJointGetAMotorAxis (_id, anum, result); } + int getAxisRel (int anum) const + { return dJointGetAMotorAxisRel (_id, anum); } + + void setAngle (int anum, dReal angle) + { dJointSetAMotorAngle (_id, anum, angle); } + dReal getAngle (int anum) const + { return dJointGetAMotorAngle (_id, anum); } + dReal getAngleRate (int anum) + { return dJointGetAMotorAngleRate (_id,anum); } + + void setParam (int parameter, dReal value) + { dJointSetAMotorParam (_id, parameter, value); } + dReal getParam (int parameter) const + { return dJointGetAMotorParam (_id, parameter); } + + void addTorques(dReal torque1, dReal torque2, dReal torque3) + { dJointAddAMotorTorques(_id, torque1, torque2, torque3); } +}; + + +#endif +#endif diff --git a/src/external/open_dynamics_engine-ef/ode/odecpp_collision.h b/src/external/open_dynamics_engine-ef/ode/odecpp_collision.h new file mode 100644 index 00000000..36a6db3f --- /dev/null +++ b/src/external/open_dynamics_engine-ef/ode/odecpp_collision.h @@ -0,0 +1,346 @@ +/************************************************************************* + * * + * 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. * + * * + *************************************************************************/ + +/* C++ interface for new collision API */ + + +#ifndef _ODE_ODECPP_COLLISION_H_ +#define _ODE_ODECPP_COLLISION_H_ +#ifdef __cplusplus + +#include "ode/ode_error.h" + + +class dGeom { + // intentionally undefined, don't use these + dGeom (dGeom &); + void operator= (dGeom &); + +protected: + dGeomID _id; + +public: + dGeom() + { _id = 0; } + ~dGeom() + { if (_id) dGeomDestroy (_id); } + + dGeomID id() const + { return _id; } + operator dGeomID() const + { return _id; } + + void destroy() { + if (_id) dGeomDestroy (_id); + _id = 0; + } + + int getClass() const + { return dGeomGetClass (_id); } + + dSpaceID getSpace() const + { return dGeomGetSpace (_id); } + + void setData (void *data) + { dGeomSetData (_id,data); } + void *getData() const + { return dGeomGetData (_id); } + + void setBody (dBodyID b) + { dGeomSetBody (_id,b); } + dBodyID body() const + { return dGeomGetBody (_id); } + + void setPosition (dReal x, dReal y, dReal z) + { dGeomSetPosition (_id,x,y,z); } + const dReal * getPosition() const + { return dGeomGetPosition (_id); } + + void setRotation (const dMatrix3 R) + { dGeomSetRotation (_id,R); } + const dReal * getRotation() const + { return dGeomGetRotation (_id); } + + void setQuaternion (const dQuaternion quat) + { dGeomSetQuaternion (_id,quat); } + void getQuaternion (dQuaternion quat) const + { dGeomGetQuaternion (_id,quat); } + + void getAABB (dReal aabb[6]) const + { dGeomGetAABB (_id, aabb); } + + int isSpace() + { return dGeomIsSpace (_id); } + + void setCategoryBits (unsigned long bits) + { dGeomSetCategoryBits (_id, bits); } + void setCollideBits (unsigned long bits) + { dGeomSetCollideBits (_id, bits); } + unsigned long getCategoryBits() + { return dGeomGetCategoryBits (_id); } + unsigned long getCollideBits() + { return dGeomGetCollideBits (_id); } + + void enable() + { dGeomEnable (_id); } + void disable() + { dGeomDisable (_id); } + int isEnabled() + { return dGeomIsEnabled (_id); } + + void collide2 (dGeomID g, void *data, dNearCallback *callback) + { dSpaceCollide2 (_id,g,data,callback); } +}; + + +class dSpace : public dGeom { + // intentionally undefined, don't use these + dSpace (dSpace &); + void operator= (dSpace &); + +protected: + // the default constructor is protected so that you + // can't instance this class. you must instance one + // of its subclasses instead. + dSpace () { _id = 0; } + +public: + dSpaceID id() const + { return (dSpaceID) _id; } + operator dSpaceID() const + { return (dSpaceID) _id; } + + void setCleanup (int mode) + { dSpaceSetCleanup (id(), mode); } + int getCleanup() + { return dSpaceGetCleanup (id()); } + + void add (dGeomID x) + { dSpaceAdd (id(), x); } + void remove (dGeomID x) + { dSpaceRemove (id(), x); } + int query (dGeomID x) + { return dSpaceQuery (id(),x); } + + int getNumGeoms() + { return dSpaceGetNumGeoms (id()); } + dGeomID getGeom (int i) + { return dSpaceGetGeom (id(),i); } + + void collide (void *data, dNearCallback *callback) + { dSpaceCollide (id(),data,callback); } +}; + + +class dSimpleSpace : public dSpace { + // intentionally undefined, don't use these + dSimpleSpace (dSimpleSpace &); + void operator= (dSimpleSpace &); + +public: + dSimpleSpace (dSpaceID space) + { _id = (dGeomID) dSimpleSpaceCreate (space); } +}; + + +class dHashSpace : public dSpace { + // intentionally undefined, don't use these + dHashSpace (dHashSpace &); + void operator= (dHashSpace &); + +public: + dHashSpace (dSpaceID space) + { _id = (dGeomID) dHashSpaceCreate (space); } + void setLevels (int minlevel, int maxlevel) + { dHashSpaceSetLevels (id(),minlevel,maxlevel); } +}; + + +class dQuadTreeSpace : public dSpace { + // intentionally undefined, don't use these + dQuadTreeSpace (dQuadTreeSpace &); + void operator= (dQuadTreeSpace &); + +public: + dQuadTreeSpace (dSpaceID space, dVector3 center, dVector3 extents, int depth) + { _id = (dGeomID) dQuadTreeSpaceCreate (space,center,extents,depth); } +}; + + +class dSphere : public dGeom { + // intentionally undefined, don't use these + dSphere (dSphere &); + void operator= (dSphere &); + +public: + dSphere () { } + dSphere (dSpaceID space, dReal radius) + { _id = dCreateSphere (space, radius); } + + void create (dSpaceID space, dReal radius) { + if (_id) dGeomDestroy (_id); + _id = dCreateSphere (space, radius); + } + + void setRadius (dReal radius) + { dGeomSphereSetRadius (_id, radius); } + dReal getRadius() const + { return dGeomSphereGetRadius (_id); } +}; + + +class dBox : public dGeom { + // intentionally undefined, don't use these + dBox (dBox &); + void operator= (dBox &); + +public: + dBox () { } + dBox (dSpaceID space, dReal lx, dReal ly, dReal lz) + { _id = dCreateBox (space,lx,ly,lz); } + + void create (dSpaceID space, dReal lx, dReal ly, dReal lz) { + if (_id) dGeomDestroy (_id); + _id = dCreateBox (space,lx,ly,lz); + } + + void setLengths (dReal lx, dReal ly, dReal lz) + { dGeomBoxSetLengths (_id, lx, ly, lz); } + void getLengths (dVector3 result) const + { dGeomBoxGetLengths (_id,result); } +}; + + +class dPlane : public dGeom { + // intentionally undefined, don't use these + dPlane (dPlane &); + void operator= (dPlane &); + +public: + dPlane() { } + dPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) + { _id = dCreatePlane (space,a,b,c,d); } + + void create (dSpaceID space, dReal a, dReal b, dReal c, dReal d) { + if (_id) dGeomDestroy (_id); + _id = dCreatePlane (space,a,b,c,d); + } + + void setParams (dReal a, dReal b, dReal c, dReal d) + { dGeomPlaneSetParams (_id, a, b, c, d); } + void getParams (dVector4 result) const + { dGeomPlaneGetParams (_id,result); } +}; + + +class dCCylinder : public dGeom { + // intentionally undefined, don't use these + dCCylinder (dCCylinder &); + void operator= (dCCylinder &); + +public: + dCCylinder() { } + dCCylinder (dSpaceID space, dReal radius, dReal length) + { _id = dCreateCCylinder (space,radius,length); } + + void create (dSpaceID space, dReal radius, dReal length) { + if (_id) dGeomDestroy (_id); + _id = dCreateCCylinder (space,radius,length); + } + + void setParams (dReal radius, dReal length) + { dGeomCCylinderSetParams (_id, radius, length); } + void getParams (dReal *radius, dReal *length) const + { dGeomCCylinderGetParams (_id,radius,length); } +}; + + +class dRay : public dGeom { + // intentionally undefined, don't use these + dRay (dRay &); + void operator= (dRay &); + +public: + dRay() { } + dRay (dSpaceID space, dReal length) + { _id = dCreateRay (space,length); } + + void create (dSpaceID space, dReal length) { + if (_id) dGeomDestroy (_id); + _id = dCreateRay (space,length); + } + + void setLength (dReal length) + { dGeomRaySetLength (_id, length); } + dReal getLength() + { return dGeomRayGetLength (_id); } + + void set (dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz) + { dGeomRaySet (_id, px, py, pz, dx, dy, dz); } + void get (dVector3 start, dVector3 dir) + { dGeomRayGet (_id, start, dir); } + + void setParams (int firstContact, int backfaceCull) + { dGeomRaySetParams (_id, firstContact, backfaceCull); } + void getParams (int *firstContact, int *backfaceCull) + { dGeomRayGetParams (_id, firstContact, backfaceCull); } + void setClosestHit (int closestHit) + { dGeomRaySetClosestHit (_id, closestHit); } + int getClosestHit() + { return dGeomRayGetClosestHit (_id); } +}; + + +class dGeomTransform : public dGeom { + // intentionally undefined, don't use these + dGeomTransform (dGeomTransform &); + void operator= (dGeomTransform &); + +public: + dGeomTransform() { } + dGeomTransform (dSpaceID space) + { _id = dCreateGeomTransform (space); } + + void create (dSpaceID space=0) { + if (_id) dGeomDestroy (_id); + _id = dCreateGeomTransform (space); + } + + void setGeom (dGeomID geom) + { dGeomTransformSetGeom (_id, geom); } + dGeomID geom() const + { return dGeomTransformGetGeom (_id); } + + void setCleanup (int mode) + { dGeomTransformSetCleanup (_id,mode); } + int getCleanup () + { return dGeomTransformGetCleanup (_id); } + + void setInfo (int mode) + { dGeomTransformSetInfo (_id,mode); } + int getInfo() + { return dGeomTransformGetInfo (_id); } +}; + + +#endif +#endif diff --git a/src/external/qr_code_generator/QrCode.cpp b/src/external/qr_code_generator/QrCode.cpp new file mode 100644 index 00000000..b9de8621 --- /dev/null +++ b/src/external/qr_code_generator/QrCode.cpp @@ -0,0 +1,862 @@ +/* + * QR Code generator library (C++) + * + * Copyright (c) Project Nayuki. (MIT License) + * https://www.nayuki.io/page/qr-code-generator-library + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * - The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * - The Software is provided "as is", without warranty of any kind, express or + * implied, including but not limited to the warranties of merchantability, + * fitness for a particular purpose and noninfringement. In no event shall the + * authors or copyright holders be liable for any claim, damages or other + * liability, whether in an action of contract, tort or otherwise, arising from, + * out of or in connection with the Software or the use or other dealings in the + * Software. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "QrCode.hpp" + +using std::int8_t; +using std::uint8_t; +using std::size_t; +using std::vector; + + +namespace qrcodegen { + +QrSegment::Mode::Mode(int mode, int cc0, int cc1, int cc2) : + modeBits(mode) { + numBitsCharCount[0] = cc0; + numBitsCharCount[1] = cc1; + numBitsCharCount[2] = cc2; +} + + +int QrSegment::Mode::getModeBits() const { + return modeBits; +} + + +int QrSegment::Mode::numCharCountBits(int ver) const { + return numBitsCharCount[(ver + 7) / 17]; +} + + +const QrSegment::Mode QrSegment::Mode::NUMERIC (0x1, 10, 12, 14); +const QrSegment::Mode QrSegment::Mode::ALPHANUMERIC(0x2, 9, 11, 13); +const QrSegment::Mode QrSegment::Mode::BYTE (0x4, 8, 16, 16); +const QrSegment::Mode QrSegment::Mode::KANJI (0x8, 8, 10, 12); +const QrSegment::Mode QrSegment::Mode::ECI (0x7, 0, 0, 0); + + +QrSegment QrSegment::makeBytes(const vector &data) { + if (data.size() > static_cast(INT_MAX)) + throw std::length_error("Data too long"); + BitBuffer bb; + for (uint8_t b : data) + bb.appendBits(b, 8); + return QrSegment(Mode::BYTE, static_cast(data.size()), std::move(bb)); +} + + +QrSegment QrSegment::makeNumeric(const char *digits) { + BitBuffer bb; + int accumData = 0; + int accumCount = 0; + int charCount = 0; + for (; *digits != '\0'; digits++, charCount++) { + char c = *digits; + if (c < '0' || c > '9') + throw std::domain_error("String contains non-numeric characters"); + accumData = accumData * 10 + (c - '0'); + accumCount++; + if (accumCount == 3) { + bb.appendBits(static_cast(accumData), 10); + accumData = 0; + accumCount = 0; + } + } + if (accumCount > 0) // 1 or 2 digits remaining + bb.appendBits(static_cast(accumData), accumCount * 3 + 1); + return QrSegment(Mode::NUMERIC, charCount, std::move(bb)); +} + + +QrSegment QrSegment::makeAlphanumeric(const char *text) { + BitBuffer bb; + int accumData = 0; + int accumCount = 0; + int charCount = 0; + for (; *text != '\0'; text++, charCount++) { + const char *temp = std::strchr(ALPHANUMERIC_CHARSET, *text); + if (temp == nullptr) + throw std::domain_error("String contains unencodable characters in alphanumeric mode"); + accumData = accumData * 45 + static_cast(temp - ALPHANUMERIC_CHARSET); + accumCount++; + if (accumCount == 2) { + bb.appendBits(static_cast(accumData), 11); + accumData = 0; + accumCount = 0; + } + } + if (accumCount > 0) // 1 character remaining + bb.appendBits(static_cast(accumData), 6); + return QrSegment(Mode::ALPHANUMERIC, charCount, std::move(bb)); +} + + +vector QrSegment::makeSegments(const char *text) { + // Select the most efficient segment encoding automatically + vector result; + if (*text == '\0'); // Leave result empty + else if (isNumeric(text)) + result.push_back(makeNumeric(text)); + else if (isAlphanumeric(text)) + result.push_back(makeAlphanumeric(text)); + else { + vector bytes; + for (; *text != '\0'; text++) + bytes.push_back(static_cast(*text)); + result.push_back(makeBytes(bytes)); + } + return result; +} + + +QrSegment QrSegment::makeEci(long assignVal) { + BitBuffer bb; + if (assignVal < 0) + throw std::domain_error("ECI assignment value out of range"); + else if (assignVal < (1 << 7)) + bb.appendBits(static_cast(assignVal), 8); + else if (assignVal < (1 << 14)) { + bb.appendBits(2, 2); + bb.appendBits(static_cast(assignVal), 14); + } else if (assignVal < 1000000L) { + bb.appendBits(6, 3); + bb.appendBits(static_cast(assignVal), 21); + } else + throw std::domain_error("ECI assignment value out of range"); + return QrSegment(Mode::ECI, 0, std::move(bb)); +} + + +QrSegment::QrSegment(Mode md, int numCh, const std::vector &dt) : + mode(md), + numChars(numCh), + data(dt) { + if (numCh < 0) + throw std::domain_error("Invalid value"); +} + + +QrSegment::QrSegment(Mode md, int numCh, std::vector &&dt) : + mode(md), + numChars(numCh), + data(std::move(dt)) { + if (numCh < 0) + throw std::domain_error("Invalid value"); +} + + +int QrSegment::getTotalBits(const vector &segs, int version) { + int result = 0; + for (const QrSegment &seg : segs) { + int ccbits = seg.mode.numCharCountBits(version); + if (seg.numChars >= (1L << ccbits)) + return -1; // The segment's length doesn't fit the field's bit width + if (4 + ccbits > INT_MAX - result) + return -1; // The sum will overflow an int type + result += 4 + ccbits; + if (seg.data.size() > static_cast(INT_MAX - result)) + return -1; // The sum will overflow an int type + result += static_cast(seg.data.size()); + } + return result; +} + + +bool QrSegment::isAlphanumeric(const char *text) { + for (; *text != '\0'; text++) { + if (std::strchr(ALPHANUMERIC_CHARSET, *text) == nullptr) + return false; + } + return true; +} + + +bool QrSegment::isNumeric(const char *text) { + for (; *text != '\0'; text++) { + char c = *text; + if (c < '0' || c > '9') + return false; + } + return true; +} + + +QrSegment::Mode QrSegment::getMode() const { + return mode; +} + + +int QrSegment::getNumChars() const { + return numChars; +} + + +const std::vector &QrSegment::getData() const { + return data; +} + + +const char *QrSegment::ALPHANUMERIC_CHARSET = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ $%*+-./:"; + + + +int QrCode::getFormatBits(Ecc ecl) { + switch (ecl) { + case Ecc::LOW : return 1; + case Ecc::MEDIUM : return 0; + case Ecc::QUARTILE: return 3; + case Ecc::HIGH : return 2; + default: throw std::logic_error("Assertion error"); + } +} + + +QrCode QrCode::encodeText(const char *text, Ecc ecl) { + vector segs = QrSegment::makeSegments(text); + return encodeSegments(segs, ecl); +} + + +QrCode QrCode::encodeBinary(const vector &data, Ecc ecl) { + vector segs{QrSegment::makeBytes(data)}; + return encodeSegments(segs, ecl); +} + + +QrCode QrCode::encodeSegments(const vector &segs, Ecc ecl, + int minVersion, int maxVersion, int mask, bool boostEcl) { + if (!(MIN_VERSION <= minVersion && minVersion <= maxVersion && maxVersion <= MAX_VERSION) || mask < -1 || mask > 7) + throw std::invalid_argument("Invalid value"); + + // Find the minimal version number to use + int version, dataUsedBits; + for (version = minVersion; ; version++) { + int dataCapacityBits = getNumDataCodewords(version, ecl) * 8; // Number of data bits available + dataUsedBits = QrSegment::getTotalBits(segs, version); + if (dataUsedBits != -1 && dataUsedBits <= dataCapacityBits) + break; // This version number is found to be suitable + if (version >= maxVersion) { // All versions in the range could not fit the given data + std::ostringstream sb; + if (dataUsedBits == -1) + sb << "Segment too long"; + else { + sb << "Data length = " << dataUsedBits << " bits, "; + sb << "Max capacity = " << dataCapacityBits << " bits"; + } + throw data_too_long(sb.str()); + } + } + if (dataUsedBits == -1) + throw std::logic_error("Assertion error"); + + // Increase the error correction level while the data still fits in the current version number + for (Ecc newEcl : vector{Ecc::MEDIUM, Ecc::QUARTILE, Ecc::HIGH}) { // From low to high + if (boostEcl && dataUsedBits <= getNumDataCodewords(version, newEcl) * 8) + ecl = newEcl; + } + + // Concatenate all segments to create the data bit string + BitBuffer bb; + for (const QrSegment &seg : segs) { + bb.appendBits(static_cast(seg.getMode().getModeBits()), 4); + bb.appendBits(static_cast(seg.getNumChars()), seg.getMode().numCharCountBits(version)); + bb.insert(bb.end(), seg.getData().begin(), seg.getData().end()); + } + if (bb.size() != static_cast(dataUsedBits)) + throw std::logic_error("Assertion error"); + + // Add terminator and pad up to a byte if applicable + size_t dataCapacityBits = static_cast(getNumDataCodewords(version, ecl)) * 8; + if (bb.size() > dataCapacityBits) + throw std::logic_error("Assertion error"); + bb.appendBits(0, std::min(4, static_cast(dataCapacityBits - bb.size()))); + bb.appendBits(0, (8 - static_cast(bb.size() % 8)) % 8); + if (bb.size() % 8 != 0) + throw std::logic_error("Assertion error"); + + // Pad with alternating bytes until data capacity is reached + for (uint8_t padByte = 0xEC; bb.size() < dataCapacityBits; padByte ^= 0xEC ^ 0x11) + bb.appendBits(padByte, 8); + + // Pack bits into bytes in big endian + vector dataCodewords(bb.size() / 8); + for (size_t i = 0; i < bb.size(); i++) + dataCodewords[i >> 3] |= (bb.at(i) ? 1 : 0) << (7 - (i & 7)); + + // Create the QR Code object + return QrCode(version, ecl, dataCodewords, mask); +} + + +QrCode::QrCode(int ver, Ecc ecl, const vector &dataCodewords, int msk) : + // Initialize fields and check arguments + version(ver), + errorCorrectionLevel(ecl) { + if (ver < MIN_VERSION || ver > MAX_VERSION) + throw std::domain_error("Version value out of range"); + if (msk < -1 || msk > 7) + throw std::domain_error("Mask value out of range"); + size = ver * 4 + 17; + size_t sz = static_cast(size); + modules = vector >(sz, vector(sz)); // Initially all white + isFunction = vector >(sz, vector(sz)); + + // Compute ECC, draw modules + drawFunctionPatterns(); + const vector allCodewords = addEccAndInterleave(dataCodewords); + drawCodewords(allCodewords); + + // Do masking + if (msk == -1) { // Automatically choose best mask + long minPenalty = LONG_MAX; + for (int i = 0; i < 8; i++) { + applyMask(i); + drawFormatBits(i); + long penalty = getPenaltyScore(); + if (penalty < minPenalty) { + msk = i; + minPenalty = penalty; + } + applyMask(i); // Undoes the mask due to XOR + } + } + if (msk < 0 || msk > 7) + throw std::logic_error("Assertion error"); + this->mask = msk; + applyMask(msk); // Apply the final choice of mask + drawFormatBits(msk); // Overwrite old format bits + + isFunction.clear(); + isFunction.shrink_to_fit(); +} + + +int QrCode::getVersion() const { + return version; +} + + +int QrCode::getSize() const { + return size; +} + + +QrCode::Ecc QrCode::getErrorCorrectionLevel() const { + return errorCorrectionLevel; +} + + +int QrCode::getMask() const { + return mask; +} + + +bool QrCode::getModule(int x, int y) const { + return 0 <= x && x < size && 0 <= y && y < size && module(x, y); +} + + +std::string QrCode::toSvgString(int border) const { + if (border < 0) + throw std::domain_error("Border must be non-negative"); + if (border > INT_MAX / 2 || border * 2 > INT_MAX - size) + throw std::overflow_error("Border too large"); + + std::ostringstream sb; + sb << "\n"; + sb << "\n"; + sb << "\n"; + sb << "\t\n"; + sb << "\t\n"; + sb << "\n"; + return sb.str(); +} + + +void QrCode::drawFunctionPatterns() { + // Draw horizontal and vertical timing patterns + for (int i = 0; i < size; i++) { + setFunctionModule(6, i, i % 2 == 0); + setFunctionModule(i, 6, i % 2 == 0); + } + + // Draw 3 finder patterns (all corners except bottom right; overwrites some timing modules) + drawFinderPattern(3, 3); + drawFinderPattern(size - 4, 3); + drawFinderPattern(3, size - 4); + + // Draw numerous alignment patterns + const vector alignPatPos = getAlignmentPatternPositions(); + size_t numAlign = alignPatPos.size(); + for (size_t i = 0; i < numAlign; i++) { + for (size_t j = 0; j < numAlign; j++) { + // Don't draw on the three finder corners + if (!((i == 0 && j == 0) || (i == 0 && j == numAlign - 1) || (i == numAlign - 1 && j == 0))) + drawAlignmentPattern(alignPatPos.at(i), alignPatPos.at(j)); + } + } + + // Draw configuration data + drawFormatBits(0); // Dummy mask value; overwritten later in the constructor + drawVersion(); +} + + +void QrCode::drawFormatBits(int msk) { + // Calculate error correction code and pack bits + int data = getFormatBits(errorCorrectionLevel) << 3 | msk; // errCorrLvl is uint2, msk is uint3 + int rem = data; + for (int i = 0; i < 10; i++) + rem = (rem << 1) ^ ((rem >> 9) * 0x537); + int bits = (data << 10 | rem) ^ 0x5412; // uint15 + if (bits >> 15 != 0) + throw std::logic_error("Assertion error"); + + // Draw first copy + for (int i = 0; i <= 5; i++) + setFunctionModule(8, i, getBit(bits, i)); + setFunctionModule(8, 7, getBit(bits, 6)); + setFunctionModule(8, 8, getBit(bits, 7)); + setFunctionModule(7, 8, getBit(bits, 8)); + for (int i = 9; i < 15; i++) + setFunctionModule(14 - i, 8, getBit(bits, i)); + + // Draw second copy + for (int i = 0; i < 8; i++) + setFunctionModule(size - 1 - i, 8, getBit(bits, i)); + for (int i = 8; i < 15; i++) + setFunctionModule(8, size - 15 + i, getBit(bits, i)); + setFunctionModule(8, size - 8, true); // Always black +} + + +void QrCode::drawVersion() { + if (version < 7) + return; + + // Calculate error correction code and pack bits + int rem = version; // version is uint6, in the range [7, 40] + for (int i = 0; i < 12; i++) + rem = (rem << 1) ^ ((rem >> 11) * 0x1F25); + long bits = static_cast(version) << 12 | rem; // uint18 + if (bits >> 18 != 0) + throw std::logic_error("Assertion error"); + + // Draw two copies + for (int i = 0; i < 18; i++) { + bool bit = getBit(bits, i); + int a = size - 11 + i % 3; + int b = i / 3; + setFunctionModule(a, b, bit); + setFunctionModule(b, a, bit); + } +} + + +void QrCode::drawFinderPattern(int x, int y) { + for (int dy = -4; dy <= 4; dy++) { + for (int dx = -4; dx <= 4; dx++) { + int dist = std::max(std::abs(dx), std::abs(dy)); // Chebyshev/infinity norm + int xx = x + dx, yy = y + dy; + if (0 <= xx && xx < size && 0 <= yy && yy < size) + setFunctionModule(xx, yy, dist != 2 && dist != 4); + } + } +} + + +void QrCode::drawAlignmentPattern(int x, int y) { + for (int dy = -2; dy <= 2; dy++) { + for (int dx = -2; dx <= 2; dx++) + setFunctionModule(x + dx, y + dy, std::max(std::abs(dx), std::abs(dy)) != 1); + } +} + + +void QrCode::setFunctionModule(int x, int y, bool isBlack) { + size_t ux = static_cast(x); + size_t uy = static_cast(y); + modules .at(uy).at(ux) = isBlack; + isFunction.at(uy).at(ux) = true; +} + + +bool QrCode::module(int x, int y) const { + return modules.at(static_cast(y)).at(static_cast(x)); +} + + +vector QrCode::addEccAndInterleave(const vector &data) const { + if (data.size() != static_cast(getNumDataCodewords(version, errorCorrectionLevel))) + throw std::invalid_argument("Invalid argument"); + + // Calculate parameter numbers + int numBlocks = NUM_ERROR_CORRECTION_BLOCKS[static_cast(errorCorrectionLevel)][version]; + int blockEccLen = ECC_CODEWORDS_PER_BLOCK [static_cast(errorCorrectionLevel)][version]; + int rawCodewords = getNumRawDataModules(version) / 8; + int numShortBlocks = numBlocks - rawCodewords % numBlocks; + int shortBlockLen = rawCodewords / numBlocks; + + // Split data into blocks and append ECC to each block + vector > blocks; + const vector rsDiv = reedSolomonComputeDivisor(blockEccLen); + for (int i = 0, k = 0; i < numBlocks; i++) { + vector dat(data.cbegin() + k, data.cbegin() + (k + shortBlockLen - blockEccLen + (i < numShortBlocks ? 0 : 1))); + k += static_cast(dat.size()); + const vector ecc = reedSolomonComputeRemainder(dat, rsDiv); + if (i < numShortBlocks) + dat.push_back(0); + dat.insert(dat.end(), ecc.cbegin(), ecc.cend()); + blocks.push_back(std::move(dat)); + } + + // Interleave (not concatenate) the bytes from every block into a single sequence + vector result; + for (size_t i = 0; i < blocks.at(0).size(); i++) { + for (size_t j = 0; j < blocks.size(); j++) { + // Skip the padding byte in short blocks + if (i != static_cast(shortBlockLen - blockEccLen) || j >= static_cast(numShortBlocks)) + result.push_back(blocks.at(j).at(i)); + } + } + if (result.size() != static_cast(rawCodewords)) + throw std::logic_error("Assertion error"); + return result; +} + + +void QrCode::drawCodewords(const vector &data) { + if (data.size() != static_cast(getNumRawDataModules(version) / 8)) + throw std::invalid_argument("Invalid argument"); + + size_t i = 0; // Bit index into the data + // Do the funny zigzag scan + for (int right = size - 1; right >= 1; right -= 2) { // Index of right column in each column pair + if (right == 6) + right = 5; + for (int vert = 0; vert < size; vert++) { // Vertical counter + for (int j = 0; j < 2; j++) { + size_t x = static_cast(right - j); // Actual x coordinate + bool upward = ((right + 1) & 2) == 0; + size_t y = static_cast(upward ? size - 1 - vert : vert); // Actual y coordinate + if (!isFunction.at(y).at(x) && i < data.size() * 8) { + modules.at(y).at(x) = getBit(data.at(i >> 3), 7 - static_cast(i & 7)); + i++; + } + // If this QR Code has any remainder bits (0 to 7), they were assigned as + // 0/false/white by the constructor and are left unchanged by this method + } + } + } + if (i != data.size() * 8) + throw std::logic_error("Assertion error"); +} + + +void QrCode::applyMask(int msk) { + if (msk < 0 || msk > 7) + throw std::domain_error("Mask value out of range"); + size_t sz = static_cast(size); + for (size_t y = 0; y < sz; y++) { + for (size_t x = 0; x < sz; x++) { + bool invert; + switch (msk) { + case 0: invert = (x + y) % 2 == 0; break; + case 1: invert = y % 2 == 0; break; + case 2: invert = x % 3 == 0; break; + case 3: invert = (x + y) % 3 == 0; break; + case 4: invert = (x / 3 + y / 2) % 2 == 0; break; + case 5: invert = x * y % 2 + x * y % 3 == 0; break; + case 6: invert = (x * y % 2 + x * y % 3) % 2 == 0; break; + case 7: invert = ((x + y) % 2 + x * y % 3) % 2 == 0; break; + default: throw std::logic_error("Assertion error"); + } + modules.at(y).at(x) = modules.at(y).at(x) ^ (invert & !isFunction.at(y).at(x)); + } + } +} + + +long QrCode::getPenaltyScore() const { + long result = 0; + + // Adjacent modules in row having same color, and finder-like patterns + for (int y = 0; y < size; y++) { + bool runColor = false; + int runX = 0; + std::array runHistory = {}; + for (int x = 0; x < size; x++) { + if (module(x, y) == runColor) { + runX++; + if (runX == 5) + result += PENALTY_N1; + else if (runX > 5) + result++; + } else { + finderPenaltyAddHistory(runX, runHistory); + if (!runColor) + result += finderPenaltyCountPatterns(runHistory) * PENALTY_N3; + runColor = module(x, y); + runX = 1; + } + } + result += finderPenaltyTerminateAndCount(runColor, runX, runHistory) * PENALTY_N3; + } + // Adjacent modules in column having same color, and finder-like patterns + for (int x = 0; x < size; x++) { + bool runColor = false; + int runY = 0; + std::array runHistory = {}; + for (int y = 0; y < size; y++) { + if (module(x, y) == runColor) { + runY++; + if (runY == 5) + result += PENALTY_N1; + else if (runY > 5) + result++; + } else { + finderPenaltyAddHistory(runY, runHistory); + if (!runColor) + result += finderPenaltyCountPatterns(runHistory) * PENALTY_N3; + runColor = module(x, y); + runY = 1; + } + } + result += finderPenaltyTerminateAndCount(runColor, runY, runHistory) * PENALTY_N3; + } + + // 2*2 blocks of modules having same color + for (int y = 0; y < size - 1; y++) { + for (int x = 0; x < size - 1; x++) { + bool color = module(x, y); + if ( color == module(x + 1, y) && + color == module(x, y + 1) && + color == module(x + 1, y + 1)) + result += PENALTY_N2; + } + } + + // Balance of black and white modules + int black = 0; + for (const vector &row : modules) { + for (bool color : row) { + if (color) + black++; + } + } + int total = size * size; // Note that size is odd, so black/total != 1/2 + // Compute the smallest integer k >= 0 such that (45-5k)% <= black/total <= (55+5k)% + int k = static_cast((std::abs(black * 20L - total * 10L) + total - 1) / total) - 1; + result += k * PENALTY_N4; + return result; +} + + +vector QrCode::getAlignmentPatternPositions() const { + if (version == 1) + return vector(); + else { + int numAlign = version / 7 + 2; + int step = (version == 32) ? 26 : + (version*4 + numAlign*2 + 1) / (numAlign*2 - 2) * 2; + vector result; + for (int i = 0, pos = size - 7; i < numAlign - 1; i++, pos -= step) + result.insert(result.begin(), pos); + result.insert(result.begin(), 6); + return result; + } +} + + +int QrCode::getNumRawDataModules(int ver) { + if (ver < MIN_VERSION || ver > MAX_VERSION) + throw std::domain_error("Version number out of range"); + int result = (16 * ver + 128) * ver + 64; + if (ver >= 2) { + int numAlign = ver / 7 + 2; + result -= (25 * numAlign - 10) * numAlign - 55; + if (ver >= 7) + result -= 36; + } + if (!(208 <= result && result <= 29648)) + throw std::logic_error("Assertion error"); + return result; +} + + +int QrCode::getNumDataCodewords(int ver, Ecc ecl) { + return getNumRawDataModules(ver) / 8 + - ECC_CODEWORDS_PER_BLOCK [static_cast(ecl)][ver] + * NUM_ERROR_CORRECTION_BLOCKS[static_cast(ecl)][ver]; +} + + +vector QrCode::reedSolomonComputeDivisor(int degree) { + if (degree < 1 || degree > 255) + throw std::domain_error("Degree out of range"); + // Polynomial coefficients are stored from highest to lowest power, excluding the leading term which is always 1. + // For example the polynomial x^3 + 255x^2 + 8x + 93 is stored as the uint8 array {255, 8, 93}. + vector result(static_cast(degree)); + result.at(result.size() - 1) = 1; // Start off with the monomial x^0 + + // Compute the product polynomial (x - r^0) * (x - r^1) * (x - r^2) * ... * (x - r^{degree-1}), + // and drop the highest monomial term which is always 1x^degree. + // Note that r = 0x02, which is a generator element of this field GF(2^8/0x11D). + uint8_t root = 1; + for (int i = 0; i < degree; i++) { + // Multiply the current product by (x - r^i) + for (size_t j = 0; j < result.size(); j++) { + result.at(j) = reedSolomonMultiply(result.at(j), root); + if (j + 1 < result.size()) + result.at(j) ^= result.at(j + 1); + } + root = reedSolomonMultiply(root, 0x02); + } + return result; +} + + +vector QrCode::reedSolomonComputeRemainder(const vector &data, const vector &divisor) { + vector result(divisor.size()); + for (uint8_t b : data) { // Polynomial division + uint8_t factor = b ^ result.at(0); + result.erase(result.begin()); + result.push_back(0); + for (size_t i = 0; i < result.size(); i++) + result.at(i) ^= reedSolomonMultiply(divisor.at(i), factor); + } + return result; +} + + +uint8_t QrCode::reedSolomonMultiply(uint8_t x, uint8_t y) { + // Russian peasant multiplication + int z = 0; + for (int i = 7; i >= 0; i--) { + z = (z << 1) ^ ((z >> 7) * 0x11D); + z ^= ((y >> i) & 1) * x; + } + if (z >> 8 != 0) + throw std::logic_error("Assertion error"); + return static_cast(z); +} + + +int QrCode::finderPenaltyCountPatterns(const std::array &runHistory) const { + int n = runHistory.at(1); + if (n > size * 3) + throw std::logic_error("Assertion error"); + bool core = n > 0 && runHistory.at(2) == n && runHistory.at(3) == n * 3 && runHistory.at(4) == n && runHistory.at(5) == n; + return (core && runHistory.at(0) >= n * 4 && runHistory.at(6) >= n ? 1 : 0) + + (core && runHistory.at(6) >= n * 4 && runHistory.at(0) >= n ? 1 : 0); +} + + +int QrCode::finderPenaltyTerminateAndCount(bool currentRunColor, int currentRunLength, std::array &runHistory) const { + if (currentRunColor) { // Terminate black run + finderPenaltyAddHistory(currentRunLength, runHistory); + currentRunLength = 0; + } + currentRunLength += size; // Add white border to final run + finderPenaltyAddHistory(currentRunLength, runHistory); + return finderPenaltyCountPatterns(runHistory); +} + + +void QrCode::finderPenaltyAddHistory(int currentRunLength, std::array &runHistory) const { + if (runHistory.at(0) == 0) + currentRunLength += size; // Add white border to initial run + std::copy_backward(runHistory.cbegin(), runHistory.cend() - 1, runHistory.end()); + runHistory.at(0) = currentRunLength; +} + + +bool QrCode::getBit(long x, int i) { + return ((x >> i) & 1) != 0; +} + + +/*---- Tables of constants ----*/ + +const int QrCode::PENALTY_N1 = 3; +const int QrCode::PENALTY_N2 = 3; +const int QrCode::PENALTY_N3 = 40; +const int QrCode::PENALTY_N4 = 10; + + +const int8_t QrCode::ECC_CODEWORDS_PER_BLOCK[4][41] = { + // Version: (note that index 0 is for padding, and is set to an illegal value) + //0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40 Error correction level + {-1, 7, 10, 15, 20, 26, 18, 20, 24, 30, 18, 20, 24, 26, 30, 22, 24, 28, 30, 28, 28, 28, 28, 30, 30, 26, 28, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30}, // Low + {-1, 10, 16, 26, 18, 24, 16, 18, 22, 22, 26, 30, 22, 22, 24, 24, 28, 28, 26, 26, 26, 26, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28}, // Medium + {-1, 13, 22, 18, 26, 18, 24, 18, 22, 20, 24, 28, 26, 24, 20, 30, 24, 28, 28, 26, 30, 28, 30, 30, 30, 30, 28, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30}, // Quartile + {-1, 17, 28, 22, 16, 22, 28, 26, 26, 24, 28, 24, 28, 22, 24, 24, 30, 28, 28, 26, 28, 30, 24, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30}, // High +}; + +const int8_t QrCode::NUM_ERROR_CORRECTION_BLOCKS[4][41] = { + // Version: (note that index 0 is for padding, and is set to an illegal value) + //0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40 Error correction level + {-1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 4, 4, 4, 4, 4, 6, 6, 6, 6, 7, 8, 8, 9, 9, 10, 12, 12, 12, 13, 14, 15, 16, 17, 18, 19, 19, 20, 21, 22, 24, 25}, // Low + {-1, 1, 1, 1, 2, 2, 4, 4, 4, 5, 5, 5, 8, 9, 9, 10, 10, 11, 13, 14, 16, 17, 17, 18, 20, 21, 23, 25, 26, 28, 29, 31, 33, 35, 37, 38, 40, 43, 45, 47, 49}, // Medium + {-1, 1, 1, 2, 2, 4, 4, 6, 6, 8, 8, 8, 10, 12, 16, 12, 17, 16, 18, 21, 20, 23, 23, 25, 27, 29, 34, 34, 35, 38, 40, 43, 45, 48, 51, 53, 56, 59, 62, 65, 68}, // Quartile + {-1, 1, 1, 2, 4, 4, 4, 5, 6, 8, 8, 11, 11, 16, 16, 18, 16, 19, 21, 25, 25, 25, 34, 30, 32, 35, 37, 40, 42, 45, 48, 51, 54, 57, 60, 63, 66, 70, 74, 77, 81}, // High +}; + + +data_too_long::data_too_long(const std::string &msg) : + std::length_error(msg) {} + + + +BitBuffer::BitBuffer() + : std::vector() {} + + +void BitBuffer::appendBits(std::uint32_t val, int len) { + if (len < 0 || len > 31 || val >> len != 0) + throw std::domain_error("Value out of range"); + for (int i = len - 1; i >= 0; i--) // Append bit by bit + this->push_back(((val >> i) & 1) != 0); +} + +} diff --git a/src/external/qr_code_generator/QrCode.hpp b/src/external/qr_code_generator/QrCode.hpp new file mode 100644 index 00000000..7341e410 --- /dev/null +++ b/src/external/qr_code_generator/QrCode.hpp @@ -0,0 +1,556 @@ +/* + * QR Code generator library (C++) + * + * Copyright (c) Project Nayuki. (MIT License) + * https://www.nayuki.io/page/qr-code-generator-library + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * - The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * - The Software is provided "as is", without warranty of any kind, express or + * implied, including but not limited to the warranties of merchantability, + * fitness for a particular purpose and noninfringement. In no event shall the + * authors or copyright holders be liable for any claim, damages or other + * liability, whether in an action of contract, tort or otherwise, arising from, + * out of or in connection with the Software or the use or other dealings in the + * Software. + */ + +#pragma once + +#include +#include +#include +#include +#include + + +namespace qrcodegen { + +/* + * A segment of character/binary/control data in a QR Code symbol. + * Instances of this class are immutable. + * The mid-level way to create a segment is to take the payload data + * and call a static factory function such as QrSegment::makeNumeric(). + * The low-level way to create a segment is to custom-make the bit buffer + * and call the QrSegment() constructor with appropriate values. + * This segment class imposes no length restrictions, but QR Codes have restrictions. + * Even in the most favorable conditions, a QR Code can only hold 7089 characters of data. + * Any segment longer than this is meaningless for the purpose of generating QR Codes. + */ +class QrSegment final { + + /*---- Public helper enumeration ----*/ + + /* + * Describes how a segment's data bits are interpreted. Immutable. + */ + public: class Mode final { + + /*-- Constants --*/ + + public: static const Mode NUMERIC; + public: static const Mode ALPHANUMERIC; + public: static const Mode BYTE; + public: static const Mode KANJI; + public: static const Mode ECI; + + + /*-- Fields --*/ + + // The mode indicator bits, which is a uint4 value (range 0 to 15). + private: int modeBits; + + // Number of character count bits for three different version ranges. + private: int numBitsCharCount[3]; + + + /*-- Constructor --*/ + + private: Mode(int mode, int cc0, int cc1, int cc2); + + + /*-- Methods --*/ + + /* + * (Package-private) Returns the mode indicator bits, which is an unsigned 4-bit value (range 0 to 15). + */ + public: int getModeBits() const; + + /* + * (Package-private) Returns the bit width of the character count field for a segment in + * this mode in a QR Code at the given version number. The result is in the range [0, 16]. + */ + public: int numCharCountBits(int ver) const; + + }; + + + + /*---- Static factory functions (mid level) ----*/ + + /* + * Returns a segment representing the given binary data encoded in + * byte mode. All input byte vectors are acceptable. Any text string + * can be converted to UTF-8 bytes and encoded as a byte mode segment. + */ + public: static QrSegment makeBytes(const std::vector &data); + + + /* + * Returns a segment representing the given string of decimal digits encoded in numeric mode. + */ + public: static QrSegment makeNumeric(const char *digits); + + + /* + * Returns a segment representing the given text string encoded in alphanumeric mode. + * The characters allowed are: 0 to 9, A to Z (uppercase only), space, + * dollar, percent, asterisk, plus, hyphen, period, slash, colon. + */ + public: static QrSegment makeAlphanumeric(const char *text); + + + /* + * Returns a list of zero or more segments to represent the given text string. The result + * may use various segment modes and switch modes to optimize the length of the bit stream. + */ + public: static std::vector makeSegments(const char *text); + + + /* + * Returns a segment representing an Extended Channel Interpretation + * (ECI) designator with the given assignment value. + */ + public: static QrSegment makeEci(long assignVal); + + + /*---- Public static helper functions ----*/ + + /* + * Tests whether the given string can be encoded as a segment in alphanumeric mode. + * A string is encodable iff each character is in the following set: 0 to 9, A to Z + * (uppercase only), space, dollar, percent, asterisk, plus, hyphen, period, slash, colon. + */ + public: static bool isAlphanumeric(const char *text); + + + /* + * Tests whether the given string can be encoded as a segment in numeric mode. + * A string is encodable iff each character is in the range 0 to 9. + */ + public: static bool isNumeric(const char *text); + + + + /*---- Instance fields ----*/ + + /* The mode indicator of this segment. Accessed through getMode(). */ + private: Mode mode; + + /* The length of this segment's unencoded data. Measured in characters for + * numeric/alphanumeric/kanji mode, bytes for byte mode, and 0 for ECI mode. + * Always zero or positive. Not the same as the data's bit length. + * Accessed through getNumChars(). */ + private: int numChars; + + /* The data bits of this segment. Accessed through getData(). */ + private: std::vector data; + + + /*---- Constructors (low level) ----*/ + + /* + * Creates a new QR Code segment with the given attributes and data. + * The character count (numCh) must agree with the mode and the bit buffer length, + * but the constraint isn't checked. The given bit buffer is copied and stored. + */ + public: QrSegment(Mode md, int numCh, const std::vector &dt); + + + /* + * Creates a new QR Code segment with the given parameters and data. + * The character count (numCh) must agree with the mode and the bit buffer length, + * but the constraint isn't checked. The given bit buffer is moved and stored. + */ + public: QrSegment(Mode md, int numCh, std::vector &&dt); + + + /*---- Methods ----*/ + + /* + * Returns the mode field of this segment. + */ + public: Mode getMode() const; + + + /* + * Returns the character count field of this segment. + */ + public: int getNumChars() const; + + + /* + * Returns the data bits of this segment. + */ + public: const std::vector &getData() const; + + + // (Package-private) Calculates the number of bits needed to encode the given segments at + // the given version. Returns a non-negative number if successful. Otherwise returns -1 if a + // segment has too many characters to fit its length field, or the total bits exceeds INT_MAX. + public: static int getTotalBits(const std::vector &segs, int version); + + + /*---- Private constant ----*/ + + /* The set of all legal characters in alphanumeric mode, where + * each character value maps to the index in the string. */ + private: static const char *ALPHANUMERIC_CHARSET; + +}; + + + +/* + * A QR Code symbol, which is a type of two-dimension barcode. + * Invented by Denso Wave and described in the ISO/IEC 18004 standard. + * Instances of this class represent an immutable square grid of black and white cells. + * The class provides static factory functions to create a QR Code from text or binary data. + * The class covers the QR Code Model 2 specification, supporting all versions (sizes) + * from 1 to 40, all 4 error correction levels, and 4 character encoding modes. + * + * Ways to create a QR Code object: + * - High level: Take the payload data and call QrCode::encodeText() or QrCode::encodeBinary(). + * - Mid level: Custom-make the list of segments and call QrCode::encodeSegments(). + * - Low level: Custom-make the array of data codeword bytes (including + * segment headers and final padding, excluding error correction codewords), + * supply the appropriate version number, and call the QrCode() constructor. + * (Note that all ways require supplying the desired error correction level.) + */ +class QrCode final { + + /*---- Public helper enumeration ----*/ + + /* + * The error correction level in a QR Code symbol. + */ + public: enum class Ecc { + LOW = 0 , // The QR Code can tolerate about 7% erroneous codewords + MEDIUM , // The QR Code can tolerate about 15% erroneous codewords + QUARTILE, // The QR Code can tolerate about 25% erroneous codewords + HIGH , // The QR Code can tolerate about 30% erroneous codewords + }; + + + // Returns a value in the range 0 to 3 (unsigned 2-bit integer). + private: static int getFormatBits(Ecc ecl); + + + + /*---- Static factory functions (high level) ----*/ + + /* + * Returns a QR Code representing the given Unicode text string at the given error correction level. + * As a conservative upper bound, this function is guaranteed to succeed for strings that have 2953 or fewer + * UTF-8 code units (not Unicode code points) if the low error correction level is used. The smallest possible + * QR Code version is automatically chosen for the output. The ECC level of the result may be higher than + * the ecl argument if it can be done without increasing the version. + */ + public: static QrCode encodeText(const char *text, Ecc ecl); + + + /* + * Returns a QR Code representing the given binary data at the given error correction level. + * This function always encodes using the binary segment mode, not any text mode. The maximum number of + * bytes allowed is 2953. The smallest possible QR Code version is automatically chosen for the output. + * The ECC level of the result may be higher than the ecl argument if it can be done without increasing the version. + */ + public: static QrCode encodeBinary(const std::vector &data, Ecc ecl); + + + /*---- Static factory functions (mid level) ----*/ + + /* + * Returns a QR Code representing the given segments with the given encoding parameters. + * The smallest possible QR Code version within the given range is automatically + * chosen for the output. Iff boostEcl is true, then the ECC level of the result + * may be higher than the ecl argument if it can be done without increasing the + * version. The mask number is either between 0 to 7 (inclusive) to force that + * mask, or -1 to automatically choose an appropriate mask (which may be slow). + * This function allows the user to create a custom sequence of segments that switches + * between modes (such as alphanumeric and byte) to encode text in less space. + * This is a mid-level API; the high-level API is encodeText() and encodeBinary(). + */ + public: static QrCode encodeSegments(const std::vector &segs, Ecc ecl, + int minVersion=1, int maxVersion=40, int mask=-1, bool boostEcl=true); // All optional parameters + + + + /*---- Instance fields ----*/ + + // Immutable scalar parameters: + + /* The version number of this QR Code, which is between 1 and 40 (inclusive). + * This determines the size of this barcode. */ + private: int version; + + /* The width and height of this QR Code, measured in modules, between + * 21 and 177 (inclusive). This is equal to version * 4 + 17. */ + private: int size; + + /* The error correction level used in this QR Code. */ + private: Ecc errorCorrectionLevel; + + /* The index of the mask pattern used in this QR Code, which is between 0 and 7 (inclusive). + * Even if a QR Code is created with automatic masking requested (mask = -1), + * the resulting object still has a mask value between 0 and 7. */ + private: int mask; + + // Private grids of modules/pixels, with dimensions of size*size: + + // The modules of this QR Code (false = white, true = black). + // Immutable after constructor finishes. Accessed through getModule(). + private: std::vector > modules; + + // Indicates function modules that are not subjected to masking. Discarded when constructor finishes. + private: std::vector > isFunction; + + + + /*---- Constructor (low level) ----*/ + + /* + * Creates a new QR Code with the given version number, + * error correction level, data codeword bytes, and mask number. + * This is a low-level API that most users should not use directly. + * A mid-level API is the encodeSegments() function. + */ + public: QrCode(int ver, Ecc ecl, const std::vector &dataCodewords, int msk); + + + + /*---- Public instance methods ----*/ + + /* + * Returns this QR Code's version, in the range [1, 40]. + */ + public: int getVersion() const; + + + /* + * Returns this QR Code's size, in the range [21, 177]. + */ + public: int getSize() const; + + + /* + * Returns this QR Code's error correction level. + */ + public: Ecc getErrorCorrectionLevel() const; + + + /* + * Returns this QR Code's mask, in the range [0, 7]. + */ + public: int getMask() const; + + + /* + * Returns the color of the module (pixel) at the given coordinates, which is false + * for white or true for black. The top left corner has the coordinates (x=0, y=0). + * If the given coordinates are out of bounds, then false (white) is returned. + */ + public: bool getModule(int x, int y) const; + + + /* + * Returns a string of SVG code for an image depicting this QR Code, with the given number + * of border modules. The string always uses Unix newlines (\n), regardless of the platform. + */ + public: std::string toSvgString(int border) const; + + + + /*---- Private helper methods for constructor: Drawing function modules ----*/ + + // Reads this object's version field, and draws and marks all function modules. + private: void drawFunctionPatterns(); + + + // Draws two copies of the format bits (with its own error correction code) + // based on the given mask and this object's error correction level field. + private: void drawFormatBits(int msk); + + + // Draws two copies of the version bits (with its own error correction code), + // based on this object's version field, iff 7 <= version <= 40. + private: void drawVersion(); + + + // Draws a 9*9 finder pattern including the border separator, + // with the center module at (x, y). Modules can be out of bounds. + private: void drawFinderPattern(int x, int y); + + + // Draws a 5*5 alignment pattern, with the center module + // at (x, y). All modules must be in bounds. + private: void drawAlignmentPattern(int x, int y); + + + // Sets the color of a module and marks it as a function module. + // Only used by the constructor. Coordinates must be in bounds. + private: void setFunctionModule(int x, int y, bool isBlack); + + + // Returns the color of the module at the given coordinates, which must be in range. + private: bool module(int x, int y) const; + + + /*---- Private helper methods for constructor: Codewords and masking ----*/ + + // Returns a new byte string representing the given data with the appropriate error correction + // codewords appended to it, based on this object's version and error correction level. + private: std::vector addEccAndInterleave(const std::vector &data) const; + + + // Draws the given sequence of 8-bit codewords (data and error correction) onto the entire + // data area of this QR Code. Function modules need to be marked off before this is called. + private: void drawCodewords(const std::vector &data); + + + // XORs the codeword modules in this QR Code with the given mask pattern. + // The function modules must be marked and the codeword bits must be drawn + // before masking. Due to the arithmetic of XOR, calling applyMask() with + // the same mask value a second time will undo the mask. A final well-formed + // QR Code needs exactly one (not zero, two, etc.) mask applied. + private: void applyMask(int msk); + + + // Calculates and returns the penalty score based on state of this QR Code's current modules. + // This is used by the automatic mask choice algorithm to find the mask pattern that yields the lowest score. + private: long getPenaltyScore() const; + + + + /*---- Private helper functions ----*/ + + // Returns an ascending list of positions of alignment patterns for this version number. + // Each position is in the range [0,177), and are used on both the x and y axes. + // This could be implemented as lookup table of 40 variable-length lists of unsigned bytes. + private: std::vector getAlignmentPatternPositions() const; + + + // Returns the number of data bits that can be stored in a QR Code of the given version number, after + // all function modules are excluded. This includes remainder bits, so it might not be a multiple of 8. + // The result is in the range [208, 29648]. This could be implemented as a 40-entry lookup table. + private: static int getNumRawDataModules(int ver); + + + // Returns the number of 8-bit data (i.e. not error correction) codewords contained in any + // QR Code of the given version number and error correction level, with remainder bits discarded. + // This stateless pure function could be implemented as a (40*4)-cell lookup table. + private: static int getNumDataCodewords(int ver, Ecc ecl); + + + // Returns a Reed-Solomon ECC generator polynomial for the given degree. This could be + // implemented as a lookup table over all possible parameter values, instead of as an algorithm. + private: static std::vector reedSolomonComputeDivisor(int degree); + + + // Returns the Reed-Solomon error correction codeword for the given data and divisor polynomials. + private: static std::vector reedSolomonComputeRemainder(const std::vector &data, const std::vector &divisor); + + + // Returns the product of the two given field elements modulo GF(2^8/0x11D). + // All inputs are valid. This could be implemented as a 256*256 lookup table. + private: static std::uint8_t reedSolomonMultiply(std::uint8_t x, std::uint8_t y); + + + // Can only be called immediately after a white run is added, and + // returns either 0, 1, or 2. A helper function for getPenaltyScore(). + private: int finderPenaltyCountPatterns(const std::array &runHistory) const; + + + // Must be called at the end of a line (row or column) of modules. A helper function for getPenaltyScore(). + private: int finderPenaltyTerminateAndCount(bool currentRunColor, int currentRunLength, std::array &runHistory) const; + + + // Pushes the given value to the front and drops the last value. A helper function for getPenaltyScore(). + private: void finderPenaltyAddHistory(int currentRunLength, std::array &runHistory) const; + + + // Returns true iff the i'th bit of x is set to 1. + private: static bool getBit(long x, int i); + + + /*---- Constants and tables ----*/ + + // The minimum version number supported in the QR Code Model 2 standard. + public: static constexpr int MIN_VERSION = 1; + + // The maximum version number supported in the QR Code Model 2 standard. + public: static constexpr int MAX_VERSION = 40; + + + // For use in getPenaltyScore(), when evaluating which mask is best. + private: static const int PENALTY_N1; + private: static const int PENALTY_N2; + private: static const int PENALTY_N3; + private: static const int PENALTY_N4; + + + private: static const std::int8_t ECC_CODEWORDS_PER_BLOCK[4][41]; + private: static const std::int8_t NUM_ERROR_CORRECTION_BLOCKS[4][41]; + +}; + + + +/*---- Public exception class ----*/ + +/* + * Thrown when the supplied data does not fit any QR Code version. Ways to handle this exception include: + * - Decrease the error correction level if it was greater than Ecc::LOW. + * - If the encodeSegments() function was called with a maxVersion argument, then increase + * it if it was less than QrCode::MAX_VERSION. (This advice does not apply to the other + * factory functions because they search all versions up to QrCode::MAX_VERSION.) + * - Split the text data into better or optimal segments in order to reduce the number of bits required. + * - Change the text or binary data to be shorter. + * - Change the text to fit the character set of a particular segment mode (e.g. alphanumeric). + * - Propagate the error upward to the caller/user. + */ +class data_too_long : public std::length_error { + + public: explicit data_too_long(const std::string &msg); + +}; + + + +/* + * An appendable sequence of bits (0s and 1s). Mainly used by QrSegment. + */ +class BitBuffer final : public std::vector { + + /*---- Constructor ----*/ + + // Creates an empty bit buffer (length 0). + public: BitBuffer(); + + + + /*---- Method ----*/ + + // Appends the given number of low-order bits of the given value + // to this buffer. Requires 0 <= len <= 31 and val < 2^len. + public: void appendBits(std::uint32_t val, int len); + +}; + +}