diff options
Diffstat (limited to 'externals/g3dlite')
39 files changed, 18889 insertions, 0 deletions
diff --git a/externals/g3dlite/AABox.cpp b/externals/g3dlite/AABox.cpp new file mode 100644 index 00000000000..035497aa3c4 --- /dev/null +++ b/externals/g3dlite/AABox.cpp @@ -0,0 +1,366 @@ +/** + @file AABox.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2004-01-10 + @edited 2006-01-11 +*/ + +#include "G3D/platform.h" +#include "G3D/AABox.h" +#include "G3D/Box.h" +#include "G3D/Plane.h" +#include "G3D/Sphere.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" + + +namespace G3D { + +const AABox& AABox::maxFinite() { + static const AABox b = AABox(Vector3::minFinite(), + Vector3::maxFinite()); + return b; +} + + +const AABox& AABox::large() { + static const AABox b = AABox(Vector3::minFinite() * 0.5f, + Vector3::maxFinite() * 0.5f); + return b; +} + + +const AABox& AABox::inf() { + static const AABox b = AABox(-Vector3::inf(), Vector3::inf()); + return b; +} + + +const AABox& AABox::zero() { + static const AABox b = AABox(Vector3::zero(), Vector3::zero()); + return b; +} + + +void AABox::serialize(class BinaryOutput& b) const { + b.writeVector3(lo); + b.writeVector3(hi); +} + + +void AABox::deserialize(class BinaryInput& b) { + lo = b.readVector3(); + hi = b.readVector3(); +} + + +void AABox::split(const Vector3::Axis& axis, float location, AABox& low, AABox& high) const { + // Low, medium, and high along the chosen axis + float L = G3D::min(location, lo[axis]); + float M = G3D::min(G3D::max(location, lo[axis]), hi[axis]); + float H = G3D::max(location, hi[axis]); + + // Copy over this box. + high = low = *this; + + // Now move the split points along the special axis + low.lo[axis] = L; + low.hi[axis] = M; + high.lo[axis] = M; + high.hi[axis] = H; +} + + +Vector3 AABox::randomSurfacePoint() const { + Vector3 extent = hi - lo; + float aXY = extent.x * extent.y; + float aYZ = extent.y * extent.z; + float aZX = extent.z * extent.x; + + float r = (float)uniformRandom(0.0f, aXY + aYZ + aZX); + + // Choose evenly between positive and negative face planes + float d = ((float)uniformRandom(0, 1) < 0.5f) ? 0.0f : 1.0f; + + // The probability of choosing a given face is proportional to + // its area. + if (r < aXY) { + return + lo + + Vector3( + (float)uniformRandom(0.0f, extent.x), + (float)uniformRandom(0.0f, extent.y), + d * extent.z); + } else if (r < aYZ) { + return + lo + + Vector3( + d * extent.x, + (float)uniformRandom(0, extent.y), + (float)uniformRandom(0, extent.z)); + } else { + return + lo + + Vector3( + (float)uniformRandom(0, extent.x), + d * extent.y, + (float)uniformRandom(0, extent.z)); + } +} + + +Vector3 AABox::randomInteriorPoint() const { + return Vector3( + (float)uniformRandom(lo.x, hi.x), + (float)uniformRandom(lo.y, hi.y), + (float)uniformRandom(lo.z, hi.z)); +} + + +bool AABox::intersects(const AABox& other) const { + // Must be overlap along all three axes. + // Try to find a separating axis. + + for (int a = 0; a < 3; ++a) { + + // |--------| + // |------| + + if ((lo[a] > other.hi[a]) || + (hi[a] < other.lo[a])) { + return false; + } + } + + return true; +} + +int AABox::dummy = 0; + +bool AABox::culledBy( + const Array<Plane>& plane, + int& cullingPlane, + const uint32 _inMask, + uint32& childMask) const { + + uint32 inMask = _inMask; + assert(plane.size() < 31); + + childMask = 0; + + const bool finite = + (abs(lo.x) < G3D::finf()) && + (abs(hi.x) < G3D::finf()) && + (abs(lo.y) < G3D::finf()) && + (abs(hi.y) < G3D::finf()) && + (abs(lo.z) < G3D::finf()) && + (abs(hi.z) < G3D::finf()); + + // See if there is one plane for which all of the + // vertices are in the negative half space. + for (int p = 0; p < plane.size(); ++p) { + + // Only test planes that are not masked + if ((inMask & 1) != 0) { + + Vector3 corner; + + int numContained = 0; + int v = 0; + + // We can early-out only if we have found one point on each + // side of the plane (i.e. if we are straddling). That + // occurs when (numContained < v) && (numContained > 0) + for (v = 0; (v < 8) && ((numContained == v) || (numContained == 0)); ++v) { + // Unrolling these 3 if's into a switch decreases performance + // by about 2x + corner.x = (v & 1) ? hi.x : lo.x; + corner.y = (v & 2) ? hi.y : lo.y; + corner.z = (v & 4) ? hi.z : lo.z; + + if (finite) { // this branch is highly predictable + if (plane[p].halfSpaceContainsFinite(corner)) { + ++numContained; + } + } else { + if (plane[p].halfSpaceContains(corner)) { + ++numContained; + } + } + } + + if (numContained == 0) { + // Plane p culled the box + cullingPlane = p; + + // The caller should not recurse into the children, + // since the parent is culled. If they do recurse, + // make them only test against this one plane, which + // will immediately cull the volume. + childMask = 1 << p; + return true; + + } else if (numContained < v) { + // The bounding volume straddled the plane; we have + // to keep testing against this plane + childMask |= (1 << p); + } + } + + // Move on to the next bit. + inMask = inMask >> 1; + } + + // None of the planes could cull this box + cullingPlane = -1; + return false; +} + + +bool AABox::culledBy( + const Array<Plane>& plane, + int& cullingPlane, + const uint32 _inMask) const { + + uint32 inMask = _inMask; + assert(plane.size() < 31); + + const bool finite = + (abs(lo.x) < G3D::finf()) && + (abs(hi.x) < G3D::finf()) && + (abs(lo.y) < G3D::finf()) && + (abs(hi.y) < G3D::finf()) && + (abs(lo.z) < G3D::finf()) && + (abs(hi.z) < G3D::finf()); + + // See if there is one plane for which all of the + // vertices are in the negative half space. + for (int p = 0; p < plane.size(); ++p) { + + // Only test planes that are not masked + if ((inMask & 1) != 0) { + + bool culled = true; + Vector3 corner; + + int v; + + // Assume this plane culls all points. See if there is a point + // not culled by the plane... early out when at least one point + // is in the positive half space. + for (v = 0; (v < 8) && culled; ++v) { + + // Unrolling these 3 if's into a switch decreases performance + // by about 2x + corner.x = (v & 1) ? hi.x : lo.x; + corner.y = (v & 2) ? hi.y : lo.y; + corner.z = (v & 4) ? hi.z : lo.z; + + if (finite) { // this branch is highly predictable + culled = ! plane[p].halfSpaceContainsFinite(corner); + } else { + culled = ! plane[p].halfSpaceContains(corner); + } + } + + if (culled) { + // Plane p culled the box + cullingPlane = p; + + return true; + } + } + + // Move on to the next bit. + inMask = inMask >> 1; + } + + // None of the planes could cull this box + cullingPlane = -1; + return false; +} + + +bool AABox::intersects(const class Sphere& sphere) const { + double d = 0; + + //find the square of the distance + //from the sphere to the box + for (int i = 0; i < 3; ++i) { + if (sphere.center[i] < lo[i]) { + d += square(sphere.center[i] - lo[i]); + } else if (sphere.center[i] > hi[i]) { + d += square(sphere.center[i] - hi[i]); + } + } + + return d <= square(sphere.radius); +} + +Vector3 AABox::corner(int index) const { + + // default constructor inits all components to 0 + Vector3 v; + + switch (index) + { + case 0: + v.x = lo.x; + v.y = lo.y; + v.z = hi.z; + break; + + case 1: + v.x = hi.x; + v.y = lo.y; + v.z = hi.z; + break; + + case 2: + v.x = hi.x; + v.y = hi.y; + v.z = hi.z; + break; + + case 3: + v.x = lo.x; + v.y = hi.y; + v.z = hi.z; + break; + + case 4: + v.x = lo.x; + v.y = lo.y; + v.z = lo.z; + break; + + case 5: + v.x = hi.x; + v.y = lo.y; + v.z = lo.z; + break; + + case 6: + v.x = hi.x; + v.y = hi.y; + v.z = lo.z; + break; + + case 7: + v.x = lo.x; + v.y = hi.y; + v.z = lo.z; + break; + + default: + debugAssertM(false, "Invalid corner index"); + break; + } + + return v; +} + + +} diff --git a/externals/g3dlite/Any.cpp b/externals/g3dlite/Any.cpp new file mode 100644 index 00000000000..de4d32e83ea --- /dev/null +++ b/externals/g3dlite/Any.cpp @@ -0,0 +1,1237 @@ +/** + @file Any.cpp + + @author Morgan McGuire + @author Shawn Yarbrough + + @created 2006-06-11 + @edited 2009-11-15 + + Copyright 2000-2009, Morgan McGuire. + All rights reserved. + */ + +#include "G3D/Any.h" +#include "G3D/TextOutput.h" +#include "G3D/TextInput.h" +#include "G3D/stringutils.h" +#include <deque> +#include <iostream> + +namespace G3D { + +void Any::beforeRead() const { + if (isPlaceholder()) { + // Tried to read from a placeholder--throw an exception as if + // the original operator[] had failed. + KeyNotFound e; + alwaysAssertM(m_data, "Corrupt placeholder"); + + e.filename = m_data->source.filename; + e.line = m_data->source.line; + e.character = m_data->source.character; + e.key = m_placeholderName; + e.message = + "This exception may have been thrown later than " + "the actual operator[] invocation."; + + throw e; + } +} + + +Any::Data* Any::Data::create(const Data* d) { + Data* p = create(d->type); + + p->comment = d->comment; + p->name = d->name; + + switch (d->type) { + case NONE: + case BOOLEAN: + case NUMBER: + // No clone needed + break; + + case STRING: + *(p->value.s) = *(d->value.s); + break; + + case ARRAY: + *(p->value.a) = *(d->value.a); + break; + + case TABLE: + *(p->value.t) = *(d->value.t); + // Note that placeholders may be copied; that is ok--they are still + // just placeholders. + break; + } + + return p; +} + + +Any::Data* Any::Data::create(Any::Type t) { + size_t s = sizeof(Data); + + switch (t) { + case NONE: + case BOOLEAN: + case NUMBER: + // No extra space needed + break; + + case STRING: + s += sizeof(std::string); + break; + + case ARRAY: + s += sizeof(AnyArray); + break; + + case TABLE: + s += sizeof(AnyTable); + break; + } + + // Allocate the data object + Data* p = new (MemoryManager::create()->alloc(s)) Data(t); + + // Create the (empyt) value object at the end of the Data object + switch (t) { + case NONE: + case BOOLEAN: + case NUMBER: + // No value + break; + + case STRING: + p->value.s = new (p + 1) std::string(); + break; + + case ARRAY: + p->value.a = new (p + 1) AnyArray(); + break; + + case TABLE: + p->value.t = new (p + 1) AnyTable(); + break; + } + + return p; +} + + +void Any::Data::destroy(Data* d) { + if (d != NULL) { + d->~Data(); + MemoryManager::create()->free(d); + } +} + + +Any::Data::~Data() { + debugAssertM(referenceCount.value() <= 0, "Deleted while still referenced."); + + // Destruct but do not deallocate children + switch (type) { + case STRING: + debugAssert(value.s != NULL); + value.s->~basic_string(); + break; + + case ARRAY: + debugAssert(value.a != NULL); + value.a->~Array(); + break; + + case TABLE: + debugAssert(value.t != NULL); + value.t->~Table(); + break; + + default: + // All other types should have a NULL value pointer (i.e., they were used just for name and comment fields) + debugAssertM(value.s == NULL, "Corrupt Any::Data::Value"); + } + + value.s = NULL; +} + + +////////////////////////////////////////////////////////////// + +bool Any::containsKey(const std::string& x) const { + beforeRead(); + verifyType(TABLE); + + Any* a = m_data->value.t->getPointer(x); + + // Don't return true for placeholder objects + return (a != NULL) && (! a->isPlaceholder()); +} + + +void Any::dropReference() { + if (m_data && m_data->referenceCount.decrement() <= 0) { + // This was the last reference to the shared data + Data::destroy(m_data); + } + m_data = NULL; +} + + +void Any::ensureMutable() { + if (m_data && (m_data->referenceCount.value() >= 1)) { + // Copy the data. We must do this before dropping the reference + // to avoid a race condition + Data* d = Data::create(m_data); + dropReference(); + m_data = d; + } +} + + +Any::Any() : m_type(NONE), m_data(NULL) { +} + + +Any::Any(TextInput& t) : m_type(NONE), m_data(NULL) { + deserialize(t); +} + + +Any::Any(const Any& x) : m_type(NONE), m_data(NULL) { + x.beforeRead(); + *this = x; +} + + +Any::Any(double x) : m_type(NUMBER), m_simpleValue(x), m_data(NULL) { +} + + +#ifdef G3D_32BIT +Any::Any(int64 x) : m_type(NUMBER), m_simpleValue((double)x), m_data(NULL) { +} +#endif // G3D_32BIT + + +Any::Any(long x) : m_type(NUMBER), m_simpleValue((double)x), m_data(NULL) { +} + + +Any::Any(int x) : m_type(NUMBER), m_simpleValue((double)x), m_data(NULL) { +} + + +Any::Any(short x) : m_type(NUMBER), m_simpleValue((double)x), m_data(NULL) { +} + + +Any::Any(bool x) : m_type(BOOLEAN), m_simpleValue(x), m_data(NULL) { +} + + +Any::Any(const std::string& s) : m_type(STRING), m_data(Data::create(STRING)) { + *(m_data->value.s) = s; +} + + +Any::Any(const char* s) : m_type(STRING), m_data(NULL) { + if (s == NULL) { + m_type = NONE; + } else { + ensureData(); + *(m_data->value.s) = s; + } +} + + +Any::Any(Type t, const std::string& name) : m_type(t), m_data(NULL) { + alwaysAssertM(t == ARRAY || t == TABLE, "Can only create ARRAY or TABLE from Type enum."); + + ensureData(); + if (name != "") { + m_data->name = name; + } +} + + +Any::~Any() { + dropReference(); +} + + +void Any::beforeWrite() { + if (isPlaceholder()) { + // This is no longer a placeholder + m_placeholderName = ""; + } +} + +Any& Any::operator=(const Any& x) { + x.beforeRead(); + + if (this == &x) { + return *this; + } + + beforeWrite(); + + dropReference(); + + m_type = x.m_type; + m_simpleValue = x.m_simpleValue; + + if (x.m_data != NULL) { + x.m_data->referenceCount.increment(); + m_data = x.m_data; + } + + return *this; +} + + +Any& Any::operator=(double x) { + *this = Any(x); + return *this; +} + + +Any& Any::operator=(int x) { + return (*this = Any(x)); +} + + +Any& Any::operator=(bool x) { + *this = Any(x); + return *this; +} + + +Any& Any::operator=(const std::string& x) { + *this = Any(x); + return *this; +} + + +Any& Any::operator=(const char* x) { + *this = Any(x); + return *this; +} + + +Any& Any::operator=(Type t) { + switch (t) { + case NONE: + *this = Any(); + break; + + case TABLE: + case ARRAY: + *this = Any(t); + break; + + default: + alwaysAssertM(false, "Can only assign NONE, TABLE, or ARRAY Type enum."); + } + + return *this; +} + + +Any::Type Any::type() const { + beforeRead(); + return m_type; +} + + +const std::string& Any::comment() const { + beforeRead(); + + static const std::string blank; + if (m_data != NULL) { + return m_data->comment; + } else { + return blank; + } +} + + +void Any::setComment(const std::string& c) { + beforeRead(); + ensureData(); + m_data->comment = c; +} + + +bool Any::isNone() const { + beforeRead(); + return (m_type == NONE); +} + + +double Any::number() const { + beforeRead(); + verifyType(NUMBER); + return m_simpleValue.n; +} + + +const std::string& Any::string() const { + beforeRead(); + verifyType(STRING); + return *(m_data->value.s); +} + + +bool Any::boolean() const { + beforeRead(); + verifyType(BOOLEAN); + return m_simpleValue.b; +} + + +const std::string& Any::name() const { + beforeRead(); + static const std::string blank; + if (m_data != NULL) { + return m_data->name; + } else { + return blank; + } +} + + +void Any::setName(const std::string& n) { + beforeRead(); + ensureData(); + m_data->name = n; +} + + +int Any::size() const { + beforeRead(); + verifyType(ARRAY, TABLE); + switch (m_type) { + case TABLE: + return m_data->value.t->size(); + + case ARRAY: + return m_data->value.a->size(); + + default:; + return 0; + } // switch (m_type) +} + + +int Any::length() const { + beforeRead(); + return size(); +} + + +void Any::resize(int n) { + beforeRead(); + alwaysAssertM(n >= 0, "Cannot resize less than 0."); + verifyType(ARRAY); + m_data->value.a->resize(n); +} + + +void Any::clear() { + beforeRead(); + verifyType(ARRAY, TABLE); + switch (m_type) { + case ARRAY: + m_data->value.a->clear(); + break; + + case TABLE: + m_data->value.t->clear(); + break; + + default:; + } +} + + +const Any& Any::operator[](int i) const { + beforeRead(); + verifyType(ARRAY); + debugAssert(m_data != NULL); + Array<Any>& array = *(m_data->value.a); + return array[i]; +} + + +Any& Any::next() { + beforeRead(); + verifyType(ARRAY); + int n = size(); + resize(n + 1); + return (*this)[n]; +} + + +Any& Any::operator[](int i) { + beforeRead(); + verifyType(ARRAY); + debugAssert(m_data != NULL); + Array<Any>& array = *(m_data->value.a); + return array[i]; +} + + +const Array<Any>& Any::array() const { + beforeRead(); + verifyType(ARRAY); + debugAssert(m_data != NULL); + return *(m_data->value.a); +} + + +void Any::append(const Any& x0) { + beforeRead(); + verifyType(ARRAY); + debugAssert(m_data != NULL); + m_data->value.a->append(x0); +} + + +void Any::append(const Any& x0, const Any& x1) { + beforeRead(); + append(x0); + append(x1); +} + + +void Any::append(const Any& x0, const Any& x1, const Any& x2) { + beforeRead(); + append(x0); + append(x1); + append(x2); +} + + +void Any::append(const Any& x0, const Any& x1, const Any& x2, const Any& x3) { + beforeRead(); + append(x0); + append(x1); + append(x2); + append(x3); +} + + +const Table<std::string, Any>& Any::table() const { + beforeRead(); + verifyType(TABLE); + debugAssert(m_data != NULL); + return *(m_data->value.t); +} + + +const Any& Any::operator[](const std::string& x) const { + beforeRead(); + verifyType(TABLE); + debugAssert(m_data != NULL); + const Table<std::string, Any>& table = *(m_data->value.t); + Any* value = table.getPointer(x); + if (value == NULL) { + KeyNotFound e; + if (m_data) { + e.filename = m_data->source.filename; + e.line = m_data->source.line; + e.character = m_data->source.character; + } + e.key = x; + throw e; + } + return *value; +} + + +Any& Any::operator[](const std::string& key) { + beforeRead(); + verifyType(TABLE); + + bool created = false; + Any& value = m_data->value.t->getCreate(key, created); + + if (created) { + // The entry was created by this method; do not allow it to be + // read before it is written. + value.m_placeholderName = key; + + // Write source data for the value + value.ensureData(); + value.m_data->source = source(); + } + + return value; +} + + +void Any::set(const std::string& k, const Any& v) { + beforeRead(); + v.beforeRead(); + verifyType(TABLE); + debugAssert(m_data != NULL); + Table<std::string, Any>& table = *(m_data->value.t); + table.set(k, v); +} + + +const Any& Any::get(const std::string& x, const Any& defaultVal) const { + beforeRead(); + defaultVal.beforeRead(); + try { + return operator[](x); + } catch(KeyNotFound) { + return defaultVal; + } +} + + +bool Any::operator==(const Any& x) const { + beforeRead(); + x.beforeRead(); + if (m_type != x.m_type) { + return false; + } + + switch (m_type) { + case NONE: + return true; + + case BOOLEAN: + return (m_simpleValue.b == x.m_simpleValue.b); + + case NUMBER: + return (m_simpleValue.n == x.m_simpleValue.n); + + case STRING: + debugAssert(m_data != NULL); + return (*(m_data->value.s) == *(x.m_data->value.s)); + + case TABLE: { + if (size() != x.size()) { + return false; + } + debugAssert(m_data != NULL); + if (m_data->name != x.m_data->name) { + return false; + } + Table<std::string, Any>& cmptable = *( m_data->value.t); + Table<std::string, Any>& xcmptable = *(x.m_data->value.t); + for (Table<std::string,Any>::Iterator it1 = cmptable.begin(), it2 = xcmptable.begin(); + it1 != cmptable.end() && it2 != xcmptable.end(); + ++it1, ++it2) { + if (*it1 != *it2) { + return false; + } + } + return true; + } + + case ARRAY: { + if (size() != x.size()) { + return false; + } + debugAssert(m_data != NULL); + if (m_data->name != x.m_data->name) { + return false; + } + + Array<Any>& cmparray = *( m_data->value.a); + Array<Any>& xcmparray = *(x.m_data->value.a); + + for (int ii = 0; ii < size(); ++ii) { + if (cmparray[ii] != xcmparray[ii]) { + return false; + } + } + return true; + } + + default: + alwaysAssertM(false, "Unknown type."); + return false; + } // switch (m_type) + +} + + +bool Any::operator!=(const Any& x) const { + beforeRead(); + x.beforeRead(); + return !operator==(x); +} + + +static void getDeserializeSettings(TextInput::Settings& settings) { + settings.cppBlockComments = true; + settings.cppLineComments = true; + settings.otherLineComments = true; + settings.otherCommentCharacter = '#'; + settings.generateCommentTokens = true; + settings.singleQuotedStrings = false; + settings.msvcSpecials = false; + settings.caseSensitive = false; +} + + +std::string Any::unparse() const { + beforeRead(); + TextOutput::Settings settings; + TextOutput to(settings); + serialize(to); + return to.commitString(); +} + + +void Any::parse(const std::string& src) { + beforeRead(); + TextInput::Settings settings; + getDeserializeSettings(settings); + + TextInput ti(TextInput::FROM_STRING, src, settings); + deserialize(ti); +} + + +void Any::load(const std::string& filename) { + beforeRead(); + TextInput::Settings settings; + getDeserializeSettings(settings); + + TextInput ti(filename, settings); + deserialize(ti); +} + + +void Any::save(const std::string& filename) const { + beforeRead(); + TextOutput::Settings settings; + settings.wordWrap = TextOutput::Settings::WRAP_NONE; + + TextOutput to(filename,settings); + serialize(to); + to.commit(); +} + + +static bool needsQuotes(const std::string& s) { + if (! isLetter(s[0]) && (s[0] != '_')) { + return true; + } + + for (int i = 0; i < (int)s.length(); ++i) { + char c = s[i]; + + // peek character + char p = (i == (int)s.length() - 1) ? '_' : s[i + 1]; + + // Identify separators + if ((c == '-' && p == '>') || + (c == ':' && p == ':')) { + // Skip over this symbol + ++i; + continue; + } + + if (! isDigit(c) && ! isLetter(c) & (c != '.')) { + // This is an illegal character for an identifier, so we need quotes + return true; + } + } + + return false; +} + + +// TODO: if the output will fit on one line, compress tables and arrays into a single line +void Any::serialize(TextOutput& to) const { + beforeRead(); + if (m_data && ! m_data->comment.empty()) { + to.printf("\n/* %s */\n", m_data->comment.c_str()); + } + + switch (m_type) { + case NONE: + to.writeSymbol("NONE"); + break; + + case BOOLEAN: + to.writeBoolean(m_simpleValue.b); + break; + + case NUMBER: + to.writeNumber(m_simpleValue.n); + break; + + case STRING: + debugAssert(m_data != NULL); + to.writeString(*(m_data->value.s)); + break; + + case TABLE: { + debugAssert(m_data != NULL); + if (! m_data->name.empty()) { + if (needsQuotes(m_data->name)) { + to.writeString(m_data->name); + } else { + to.writeSymbol(m_data->name); + } + } + to.writeSymbol("{"); + to.writeNewline(); + to.pushIndent(); + AnyTable& table = *(m_data->value.t); + Array<std::string> keys; + table.getKeys(keys); + keys.sort(); + + for (int i = 0; i < keys.size(); ++i) { + + to.writeSymbol(keys[i]); + to.writeSymbol("="); + table[keys[i]].serialize(to); + + if (i < keys.size() - 1) { + to.writeSymbol(","); + } + to.writeNewline(); + + // Skip a line between table entries + to.writeNewline(); + } + + to.popIndent(); + to.writeSymbol("}"); + break; + } + + case ARRAY: { + debugAssert(m_data != NULL); + if (! m_data->name.empty()) { + // For arrays, leave no trailing space between the name and the paren + to.writeSymbol(format("%s(", m_data->name.c_str())); + } else { + to.writeSymbol("("); + } + to.writeNewline(); + to.pushIndent(); + Array<Any>& array = *(m_data->value.a); + for (int ii = 0; ii < size(); ++ii) { + array[ii].serialize(to); + if (ii < size() - 1) { + to.writeSymbol(","); + to.writeNewline(); + } + + // Put the close paren on an array right behind the last element + } + to.popIndent(); + to.writeSymbol(")"); + break; + } + } +} + + +void Any::deserializeComment(TextInput& ti, Token& token, std::string& comment) { + // Parse comments + while (token.type() == Token::COMMENT) { + comment += trimWhitespace(token.string()) + "\n"; + + // Allow comments to contain newlines. + do { + token = ti.read(); + comment += "\n"; + } while (token.type() == Token::NEWLINE); + } + + comment = trimWhitespace(comment); +} + +/** True if \a c is an open paren of some form */ +static bool isOpen(const char c) { + return c == '(' || c == '[' || c == '{'; +} + + +/** True if \a c is an open paren of some form */ +static bool isClose(const char c) { + return c == ')' || c == ']' || c == '}'; +} + + +/** True if \a s is a C++ name operator */ +static bool isNameOperator(const std::string& s) { + return s == "." || s == "::" || s == "->"; +} + + +void Any::deserializeName(TextInput& ti, Token& token, std::string& name) { + debugAssert(token.type() == Token::SYMBOL); + std::string s = token.string(); + while (! isOpen(s[0])) { + name += s; + + // Skip newlines and comments + token = ti.readSignificant(); + + if (token.type() != Token::SYMBOL) { + throw ParseError(ti.filename(), token.line(), token.character(), + "Expected symbol while parsing Any"); + } + s = token.string(); + } +} + + +void Any::deserialize(TextInput& ti) { + beforeRead(); + Token token = ti.read(); + deserialize(ti, token); + // Restore the last token + ti.push(token); +} + + +void Any::deserialize(TextInput& ti, Token& token) { + // Deallocate old data + dropReference(); + m_type = NONE; + m_simpleValue.b = false; + + // Skip leading newlines + while (token.type() == Token::NEWLINE) { + token = ti.read(); + } + + std::string comment; + if (token.type() == Token::COMMENT) { + deserializeComment(ti, token, comment); + } + + if (token.type() == Token::END) { + // There should never be a comment without an Any following it; even + // if the file ends with some commented out stuff, + // that should not happen after a comma, so we'd never read that + // far in a proper file. + throw ParseError(ti.filename(), token.line(), token.character(), + "File ended without a properly formed Any"); + } + + switch (token.type()) { + case Token::STRING: + m_type = STRING; + ensureData(); + *(m_data->value.s) = token.string(); + m_data->source.set(ti, token); + break; + + case Token::NUMBER: + m_type = NUMBER; + m_simpleValue.n = token.number(); + ensureData(); + m_data->source.set(ti, token); + break; + + case Token::BOOLEAN: + m_type = BOOLEAN; + m_simpleValue.b = token.boolean(); + ensureData(); + m_data->source.set(ti, token); + break; + + case Token::SYMBOL: + // Named Array, Named Table, Array, Table, or NONE + if (toUpper(token.string()) == "NONE") { + // Nothing left to do; we initialized to NONE originally + ensureData(); + m_data->source.set(ti, token); + } else { + // Array or Table + + // Parse the name + + // s must have at least one element or this would not have + // been parsed as a symbol + std::string name; + deserializeName(ti, token, name); + if (token.type() != Token::SYMBOL) { + throw ParseError(ti.filename(), token.line(), token.character(), + "Malformed Any TABLE or ARRAY; must start with [, (, or {"); + } + + if (isOpen(token.string()[0])) { + // Array or table + deserializeBody(ti, token); + } else { + throw ParseError(ti.filename(), token.line(), token.character(), + "Malformed Any TABLE or ARRAY; must start with [, (, or {"); + } + + if (! name.empty()) { + ensureData(); + m_data->name = name; + } + } // if NONE + break; + + default: + throw ParseError(ti.filename(), token.line(), token.character(), + "Unexpected token"); + + } // switch + + if (! comment.empty()) { + ensureData(); + m_data->comment = comment; + } + + if (m_type != ARRAY && m_type != TABLE) { + // Array and table already consumed their last token + token = ti.read(); + } +} + + +void Any::ensureData() { + if (m_data == NULL) { + m_data = Data::create(m_type); + } +} + + +static bool isSeparator(char c) { + return c == ',' || c == ';'; +} + + +void Any::readUntilCommaOrClose(TextInput& ti, Token& token) { + while (! ((token.type() == Token::SYMBOL) && + (isClose(token.string()[0])) || + isSeparator(token.string()[0]))) { + switch (token.type()) { + case Token::NEWLINE: + case Token::COMMENT: + // Consume + token = ti.read(); + break; + + default: + throw ParseError(ti.filename(), token.line(), token.character(), + "Expected a comma or close paren"); + } + } +} + + +void Any::deserializeBody(TextInput& ti, Token& token) { + char closeSymbol = '}'; + m_type = TABLE; + + const char c = token.string()[0]; + + if (c != '{') { + m_type = ARRAY; + // Chose the appropriate close symbol + closeSymbol = (c == '(') ? ')' : ']'; + } + + // Allocate the underlying data structure + ensureData(); + m_data->source.set(ti, token); + + // Consume the open token + token = ti.read(); + + while (! ((token.type() == Token::SYMBOL) && (token.string()[0] == closeSymbol))) { + + // Read any leading comment. This must be done here (and not in the recursive deserialize + // call) in case the body contains only a comment. + std::string comment; + deserializeComment(ti, token, comment); + + if ((token.type() == Token::SYMBOL) && (token.string()[0] == closeSymbol)) { + // We're done; this catches the case where the array is empty + break; + } + + // Pointer the value being read + Any a = NULL; + std::string key; + + if (m_type == TABLE) { + // Read the key + if (token.type() != Token::SYMBOL && token.type() != Token::STRING) { + throw ParseError(ti.filename(), token.line(), token.character(), "Expected a name"); + } + + key = token.string(); + // Consume everything up to the = sign + token = ti.readSignificant(); + + if ((token.type() != Token::SYMBOL) || (token.string() != "=")) { + throw ParseError(ti.filename(), token.line(), token.character(), "Expected ="); + } else { + // Consume (don't consume comments--we want the value pointed to by a to get those). + token = ti.read(); + } + } + a.deserialize(ti, token); + + if (! comment.empty()) { + // Prepend the comment we read earlier + a.ensureData(); + a.m_data->comment = trimWhitespace(comment + "\n" + a.m_data->comment); + } + + if (m_type == TABLE) { + set(key, a); + } else { + append(a); + } + + // Read until the comma or close paren, discarding trailing comments and newlines + readUntilCommaOrClose(ti, token); + + // Consume the comma + if (isSeparator(token.string()[0])) { + token = ti.read(); + } + } + + // Consume the close paren (to match other deserialize methods) + token = ti.read(); +} + + +Any::operator int() const { + beforeRead(); + return iRound(number()); +} + + +Any::operator float() const { + beforeRead(); + return float(number()); +} + + +Any::operator double() const { + beforeRead(); + return number(); +} + + +Any::operator bool() const { + beforeRead(); + return boolean(); +} + + +Any::operator std::string() const { + beforeRead(); + return string(); +} + + +const Any::Source& Any::source() const { + static Source s; + if (m_data) { + return m_data->source; + } else { + return s; + } +} + + +void Any::verify(bool value, const std::string& message) const { + beforeRead(); + if (! value) { + ParseError p; + if (m_data) { + p.filename = m_data->source.filename; + p.line = m_data->source.line; + p.character = m_data->source.character; + } + + if (name().empty()) { + p.message = "Parse error"; + } else { + p.message = "Parse error while reading the contents of " + name(); + } + + if (! message.empty()) { + p.message = p.message + ": " + message; + } + + throw p; + } +} + + +void Any::verifyName(const std::string& n) const { + beforeRead(); + verify(beginsWith(toUpper(name()), toUpper(n)), "Name must begin with " + n); +} + + +void Any::verifyType(Type t) const { + beforeRead(); + if (type() != t) { + verify(false, "Must have type " + toString(t)); + } +} + + +void Any::verifyType(Type t0, Type t1) const { + beforeRead(); + if (type() != t0 && type() != t1) { + verify(false, "Must have type " + toString(t0) + " or " + toString(t1)); + } +} + + +void Any::verifySize(int low, int high) const { + beforeRead(); + verifyType(ARRAY, TABLE); + if (size() < low || size() > high) { + verify(false, format("Size must be between %d and %d", low, high)); + } +} + + +void Any::verifySize(int s) const { + beforeRead(); + verifyType(ARRAY, TABLE); + if (size() != s) { + verify(false, format("Size must be %d", s)); + } +} + + +std::string Any::toString(Type t) { + switch(t) { + case NONE: return "NONE"; + case BOOLEAN: return "BOOLEAN"; + case NUMBER: return "NUMBER"; + case STRING: return "STRING"; + case ARRAY: return "ARRAY"; + case TABLE: return "TABLE"; + default: + alwaysAssertM(false, "Illegal Any::Type"); + return ""; + } +} + +} // namespace G3D + diff --git a/externals/g3dlite/BinaryFormat.cpp b/externals/g3dlite/BinaryFormat.cpp new file mode 100644 index 00000000000..d3991378f45 --- /dev/null +++ b/externals/g3dlite/BinaryFormat.cpp @@ -0,0 +1,81 @@ +/** + @file BinaryFormat.cpp + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2005-06-10 + @edited 2005-06-10 + */ + +#include "G3D/BinaryFormat.h" + +namespace G3D { + +int32 byteSize(BinaryFormat f) { + switch (f) { + case BOOL8_BINFMT: + case UINT8_BINFMT: + case INT8_BINFMT: + return 1; + + case UINT16_BINFMT: + case INT16_BINFMT: + return 2; + + case FLOAT16_BINFMT: + return 2; + + case UINT32_BINFMT: + case INT32_BINFMT: + case FLOAT32_BINFMT: + return 4; + + case FLOAT64_BINFMT: + case UINT64_BINFMT: + case INT64_BINFMT: + return 8; + + case INT128_BINFMT: + case UINT128_BINFMT: + return 16; + + case VECTOR2_BINFMT: + return 2 * 4; + + case VECTOR2INT16_BINFMT: + return 2 * 2; + + case VECTOR3_BINFMT: + return 3 * 4; + + case VECTOR3INT16_BINFMT: + return 3 * 2; + + case VECTOR4_BINFMT: + return 4 * 4; + + case VECTOR4INT16_BINFMT: + return 4 * 4; + + case COLOR3_BINFMT: + return 3 * 4; + + case COLOR3UINT8_BINFMT: + return 3 * 1; + + case COLOR3INT16_BINFMT: + return 3 * 2; + + case COLOR4_BINFMT: + return 4 * 4; + + case COLOR4UINT8_BINFMT: + return 4 * 1; + + case COLOR4INT16_BINFMT: + return 4 * 2; + + default: + return -1; + } +} +} diff --git a/externals/g3dlite/BinaryInput.cpp b/externals/g3dlite/BinaryInput.cpp new file mode 100644 index 00000000000..65a9976fe04 --- /dev/null +++ b/externals/g3dlite/BinaryInput.cpp @@ -0,0 +1,568 @@ +/** + @file BinaryInput.cpp + + @author Morgan McGuire, graphics3d.com + Copyright 2001-2007, Morgan McGuire. All rights reserved. + + @created 2001-08-09 + @edited 2005-02-24 + + + <PRE> + { + BinaryOutput b("c:/tmp/test.b", BinaryOutput::LITTLE_ENDIAN); + + float f = 3.1415926; + int i = 1027221; + std::string s = "Hello World!"; + + b.writeFloat32(f); + b.writeInt32(i); + b.writeString(s); + b.commit(); + + + BinaryInput in("c:/tmp/test.b", BinaryInput::LITTLE_ENDIAN); + + debugAssert(f == in.readFloat32()); + int ii = in.readInt32(); + debugAssert(i == ii); + debugAssert(s == in.readString()); + } + </PRE> + */ + +#include "G3D/platform.h" +#include "G3D/BinaryInput.h" +#include "G3D/Array.h" +#include "G3D/fileutils.h" +#include "G3D/Log.h" +#include <zlib.h> + +#include <cstring> + +namespace G3D { + +void BinaryInput::readBool8(std::vector<bool>& out, int64 n) { + out.resize((int)n); + // std::vector optimizes bool in a way that prevents fast reading + for (int64 i = 0; i < n ; ++i) { + out[i] = readBool8(); + } +} + + +void BinaryInput::readBool8(Array<bool>& out, int64 n) { + out.resize(n); + readBool8(out.begin(), n); +} + + +#define IMPLEMENT_READER(ucase, lcase)\ +void BinaryInput::read##ucase(std::vector<lcase>& out, int64 n) {\ + out.resize(n);\ + read##ucase(&out[0], n);\ +}\ +\ +\ +void BinaryInput::read##ucase(Array<lcase>& out, int64 n) {\ + out.resize(n);\ + read##ucase(out.begin(), n);\ +} + + +IMPLEMENT_READER(UInt8, uint8) +IMPLEMENT_READER(Int8, int8) +IMPLEMENT_READER(UInt16, uint16) +IMPLEMENT_READER(Int16, int16) +IMPLEMENT_READER(UInt32, uint32) +IMPLEMENT_READER(Int32, int32) +IMPLEMENT_READER(UInt64, uint64) +IMPLEMENT_READER(Int64, int64) +IMPLEMENT_READER(Float32, float32) +IMPLEMENT_READER(Float64, float64) + +#undef IMPLEMENT_READER + +// Data structures that are one byte per element can be +// directly copied, regardles of endian-ness. +#define IMPLEMENT_READER(ucase, lcase)\ +void BinaryInput::read##ucase(lcase* out, int64 n) {\ + if (sizeof(lcase) == 1) {\ + readBytes(out, n);\ + } else {\ + for (int64 i = 0; i < n ; ++i) {\ + out[i] = read##ucase();\ + }\ + }\ +} + +IMPLEMENT_READER(Bool8, bool) +IMPLEMENT_READER(UInt8, uint8) +IMPLEMENT_READER(Int8, int8) + +#undef IMPLEMENT_READER + + +#define IMPLEMENT_READER(ucase, lcase)\ +void BinaryInput::read##ucase(lcase* out, int64 n) {\ + if (m_swapBytes) {\ + for (int64 i = 0; i < n; ++i) {\ + out[i] = read##ucase();\ + }\ + } else {\ + readBytes(out, sizeof(lcase) * n);\ + }\ +} + + +IMPLEMENT_READER(UInt16, uint16) +IMPLEMENT_READER(Int16, int16) +IMPLEMENT_READER(UInt32, uint32) +IMPLEMENT_READER(Int32, int32) +IMPLEMENT_READER(UInt64, uint64) +IMPLEMENT_READER(Int64, int64) +IMPLEMENT_READER(Float32, float32) +IMPLEMENT_READER(Float64, float64) + +#undef IMPLEMENT_READER + +void BinaryInput::loadIntoMemory(int64 startPosition, int64 minLength) { + // Load the next section of the file + debugAssertM(m_filename != "<memory>", "Read past end of file."); + + int64 absPos = m_alreadyRead + m_pos; + + if (m_bufferLength < minLength) { + // The current buffer isn't big enough to hold the chunk we want to read. + // This happens if there was little memory available during the initial constructor + // read but more memory has since been freed. + m_bufferLength = minLength; + debugAssert(m_freeBuffer); + m_buffer = (uint8*)System::realloc(m_buffer, m_bufferLength); + if (m_buffer == NULL) { + throw "Tried to read a larger memory chunk than could fit in memory. (2)"; + } + } + + m_alreadyRead = startPosition; + +# ifdef G3D_WIN32 + FILE* file = fopen(m_filename.c_str(), "rb"); + debugAssert(file); + int ret = fseek(file, (off_t)m_alreadyRead, SEEK_SET); + debugAssert(ret == 0); + size_t toRead = (size_t)G3D::min(m_bufferLength, m_length - m_alreadyRead); + ret = fread(m_buffer, 1, toRead, file); + debugAssert(ret == toRead); + fclose(file); + file = NULL; + +# else + FILE* file = fopen(m_filename.c_str(), "rb"); + debugAssert(file); + int ret = fseeko(file, (off_t)m_alreadyRead, SEEK_SET); + debugAssert(ret == 0); + size_t toRead = (size_t)G3D::min<int64>(m_bufferLength, m_length - m_alreadyRead); + ret = fread(m_buffer, 1, toRead, file); + debugAssert((size_t)ret == (size_t)toRead); + fclose(file); + file = NULL; +# endif + + m_pos = absPos - m_alreadyRead; + debugAssert(m_pos >= 0); +} + + + +const bool BinaryInput::NO_COPY = false; + +static bool needSwapBytes(G3DEndian fileEndian) { + return (fileEndian != System::machineEndian()); +} + + +/** Helper used by the constructors for decompression */ +static uint32 readUInt32(const uint8* data, bool swapBytes) { + if (swapBytes) { + uint8 out[4]; + out[0] = data[3]; + out[1] = data[2]; + out[2] = data[1]; + out[3] = data[0]; + return *((uint32*)out); + } else { + return *((uint32*)data); + } +} + + +void BinaryInput::setEndian(G3DEndian e) { + m_fileEndian = e; + m_swapBytes = needSwapBytes(m_fileEndian); +} + + +BinaryInput::BinaryInput( + const uint8* data, + int64 dataLen, + G3DEndian dataEndian, + bool compressed, + bool copyMemory) : + m_filename("<memory>"), + m_bitPos(0), + m_bitString(0), + m_beginEndBits(0), + m_alreadyRead(0), + m_bufferLength(0), + m_pos(0) { + + m_freeBuffer = copyMemory || compressed; + + setEndian(dataEndian); + + if (compressed) { + // Read the decompressed size from the first 4 bytes + m_length = G3D::readUInt32(data, m_swapBytes); + + debugAssert(m_freeBuffer); + m_buffer = (uint8*)System::alignedMalloc(m_length, 16); + + unsigned long L = m_length; + // Decompress with zlib + int64 result = uncompress(m_buffer, (unsigned long*)&L, data + 4, dataLen - 4); + m_length = L; + m_bufferLength = L; + debugAssert(result == Z_OK); (void)result; + + } else { + m_length = dataLen; + m_bufferLength = m_length; + if (! copyMemory) { + debugAssert(!m_freeBuffer); + m_buffer = const_cast<uint8*>(data); + } else { + debugAssert(m_freeBuffer); + m_buffer = (uint8*)System::alignedMalloc(m_length, 16); + System::memcpy(m_buffer, data, dataLen); + } + } +} + + +BinaryInput::BinaryInput( + const std::string& filename, + G3DEndian fileEndian, + bool compressed) : + m_filename(filename), + m_bitPos(0), + m_bitString(0), + m_beginEndBits(0), + m_alreadyRead(0), + m_length(0), + m_bufferLength(0), + m_buffer(NULL), + m_pos(0), + m_freeBuffer(true) { + + setEndian(fileEndian); + + // Update global file tracker + _internal::currentFilesUsed.insert(m_filename); + + + if (! fileExists(m_filename, false)) { + std::string zipfile; + std::string internalfile; + if (zipfileExists(m_filename, zipfile, internalfile)) { + // Load from zipfile + void* v; + size_t s; + zipRead(filename, v, s); + m_buffer = reinterpret_cast<uint8*>(v); + m_bufferLength = m_length = s; + if (compressed) { + decompress(); + } + m_freeBuffer = true; + } else { + Log::common()->printf("Warning: File not found: %s\n", m_filename.c_str()); + } + return; + } + + // Figure out how big the file is and verify that it exists. + m_length = fileLength(m_filename); + + // Read the file into memory + FILE* file = fopen(m_filename.c_str(), "rb"); + + if (! file || (m_length == -1)) { + throw format("File not found: \"%s\"", m_filename.c_str()); + return; + } + + if (! compressed && (m_length > INITIAL_BUFFER_LENGTH)) { + // Read only a subset of the file so we don't consume + // all available memory. + m_bufferLength = INITIAL_BUFFER_LENGTH; + } else { + // Either the length is fine or the file is compressed + // and requires us to read the whole thing for zlib. + m_bufferLength = m_length; + } + + debugAssert(m_freeBuffer); + m_buffer = (uint8*)System::alignedMalloc(m_bufferLength, 16); + if (m_buffer == NULL) { + if (compressed) { + throw "Not enough memory to load compressed file. (1)"; + } + + // Try to allocate a small array; not much memory is available. + // Give up if we can't allocate even 1k. + while ((m_buffer == NULL) && (m_bufferLength > 1024)) { + m_bufferLength /= 2; + m_buffer = (uint8*)System::alignedMalloc(m_bufferLength, 16); + } + } + debugAssert(m_buffer); + + fread(m_buffer, m_bufferLength, sizeof(int8), file); + fclose(file); + file = NULL; + + if (compressed) { + if (m_bufferLength != m_length) { + throw "Not enough memory to load compressed file. (2)"; + } + + decompress(); + } +} + +void BinaryInput::decompress() { + // Decompress + // Use the existing buffer as the source, allocate + // a new buffer to use as the destination. + + int64 tempLength = m_length; + m_length = G3D::readUInt32(m_buffer, m_swapBytes); + + // The file couldn't have better than 500:1 compression + alwaysAssertM(m_length < m_bufferLength * 500, "Compressed file header is corrupted"); + + uint8* tempBuffer = m_buffer; + m_buffer = (uint8*)System::alignedMalloc(m_length, 16); + + debugAssert(m_buffer); + debugAssert(isValidHeapPointer(tempBuffer)); + debugAssert(isValidHeapPointer(m_buffer)); + + unsigned long L = m_length; + int64 result = uncompress(m_buffer, &L, tempBuffer + 4, tempLength - 4); + m_length = L; + m_bufferLength = m_length; + + debugAssertM(result == Z_OK, "BinaryInput/zlib detected corruption in " + m_filename); + (void)result; + + System::alignedFree(tempBuffer); +} + + +void BinaryInput::readBytes(void* bytes, int64 n) { + prepareToRead(n); + debugAssert(isValidPointer(bytes)); + + memcpy(bytes, m_buffer + m_pos, n); + m_pos += n; +} + + +BinaryInput::~BinaryInput() { + + if (m_freeBuffer) { + System::alignedFree(m_buffer); + } + m_buffer = NULL; +} + + +uint64 BinaryInput::readUInt64() { + prepareToRead(8); + uint8 out[8]; + + if (m_swapBytes) { + out[0] = m_buffer[m_pos + 7]; + out[1] = m_buffer[m_pos + 6]; + out[2] = m_buffer[m_pos + 5]; + out[3] = m_buffer[m_pos + 4]; + out[4] = m_buffer[m_pos + 3]; + out[5] = m_buffer[m_pos + 2]; + out[6] = m_buffer[m_pos + 1]; + out[7] = m_buffer[m_pos + 0]; + } else { + *(uint64*)out = *(uint64*)(m_buffer + m_pos); + } + + m_pos += 8; + return *(uint64*)out; +} + + +std::string BinaryInput::readString(int64 n) { + prepareToRead(n); + debugAssertM((m_pos + n) <= m_length, "Read past end of file"); + + char *s = (char*)System::alignedMalloc(n + 1, 16); + assert(s != NULL); + + memcpy(s, m_buffer + m_pos, n); + // There may not be a null, so make sure + // we add one. + s[n] = '\0'; + + std::string out = s; + System::alignedFree(s); + s = NULL; + + m_pos += n; + + return out; + +} + + +std::string BinaryInput::readString() { + int64 n = 0; + + if ((m_pos + m_alreadyRead + n) < (m_length - 1)) { + prepareToRead(1); + } + + if ( ((m_pos + m_alreadyRead + n) < (m_length - 1)) && + (m_buffer[m_pos + n] != '\0')) { + + ++n; + while ( ((m_pos + m_alreadyRead + n) < (m_length - 1)) && + (m_buffer[m_pos + n] != '\0')) { + + prepareToRead(1); + ++n; + } + } + + // Consume NULL + ++n; + + return readString(n); +} + + +std::string BinaryInput::readStringEven() { + std::string x = readString(); + if (hasMore() && (G3D::isOdd(x.length() + 1))) { + skip(1); + } + return x; +} + + +std::string BinaryInput::readString32() { + int len = readUInt32(); + return readString(len); +} + + +Vector4 BinaryInput::readVector4() { + float x = readFloat32(); + float y = readFloat32(); + float z = readFloat32(); + float w = readFloat32(); + return Vector4(x, y, z, w); +} + + +Vector3 BinaryInput::readVector3() { + float x = readFloat32(); + float y = readFloat32(); + float z = readFloat32(); + return Vector3(x, y, z); +} + + +Vector2 BinaryInput::readVector2() { + float x = readFloat32(); + float y = readFloat32(); + return Vector2(x, y); +} + + +Color4 BinaryInput::readColor4() { + float r = readFloat32(); + float g = readFloat32(); + float b = readFloat32(); + float a = readFloat32(); + return Color4(r, g, b, a); +} + + +Color3 BinaryInput::readColor3() { + float r = readFloat32(); + float g = readFloat32(); + float b = readFloat32(); + return Color3(r, g, b); +} + + +void BinaryInput::beginBits() { + debugAssert(m_beginEndBits == 0); + m_beginEndBits = 1; + m_bitPos = 0; + + debugAssertM(hasMore(), "Can't call beginBits when at the end of a file"); + m_bitString = readUInt8(); +} + + +uint32 BinaryInput::readBits(int numBits) { + debugAssert(m_beginEndBits == 1); + + uint32 out = 0; + + const int total = numBits; + while (numBits > 0) { + if (m_bitPos > 7) { + // Consume a new byte for reading. We do this at the beginning + // of the loop so that we don't try to read past the end of the file. + m_bitPos = 0; + m_bitString = readUInt8(); + } + + // Slide the lowest bit of the bitString into + // the correct position. + out |= (m_bitString & 1) << (total - numBits); + + // Shift over to the next bit + m_bitString = m_bitString >> 1; + ++m_bitPos; + --numBits; + } + + return out; +} + + +void BinaryInput::endBits() { + debugAssert(m_beginEndBits == 1); + if (m_bitPos == 0) { + // Put back the last byte we read + --m_pos; + } + m_beginEndBits = 0; + m_bitPos = 0; +} + +} diff --git a/externals/g3dlite/BinaryOutput.cpp b/externals/g3dlite/BinaryOutput.cpp new file mode 100644 index 00000000000..2de46c6d4bb --- /dev/null +++ b/externals/g3dlite/BinaryOutput.cpp @@ -0,0 +1,522 @@ +/** + @file BinaryOutput.cpp + + @author Morgan McGuire, graphics3d.com + Copyright 2002-2007, Morgan McGuire, All rights reserved. + + @created 2002-02-20 + @edited 2008-01-07 + */ + +#include "G3D/platform.h" +#include "G3D/BinaryOutput.h" +#include "G3D/fileutils.h" +#include "G3D/stringutils.h" +#include "G3D/Array.h" +#include <zlib.h> + +#include <cstring> + +// Largest memory buffer that the system will use for writing to +// disk. After this (or if the system runs out of memory) +// chunks of the file will be dumped to disk. +// +// Currently 400 MB +#define MAX_BINARYOUTPUT_BUFFER_SIZE 400000000 + +namespace G3D { + +void BinaryOutput::writeBool8(const std::vector<bool>& out, int n) { + for (int i = 0; i < n; ++i) { + writeBool8(out[i]); + } +} + + +void BinaryOutput::writeBool8(const Array<bool>& out, int n) { + writeBool8(out.getCArray(), n); +} + +#define IMPLEMENT_WRITER(ucase, lcase)\ +void BinaryOutput::write##ucase(const std::vector<lcase>& out, int n) {\ + write##ucase(&out[0], n);\ +}\ +\ +\ +void BinaryOutput::write##ucase(const Array<lcase>& out, int n) {\ + write##ucase(out.getCArray(), n);\ +} + + +IMPLEMENT_WRITER(UInt8, uint8) +IMPLEMENT_WRITER(Int8, int8) +IMPLEMENT_WRITER(UInt16, uint16) +IMPLEMENT_WRITER(Int16, int16) +IMPLEMENT_WRITER(UInt32, uint32) +IMPLEMENT_WRITER(Int32, int32) +IMPLEMENT_WRITER(UInt64, uint64) +IMPLEMENT_WRITER(Int64, int64) +IMPLEMENT_WRITER(Float32, float32) +IMPLEMENT_WRITER(Float64, float64) + +#undef IMPLEMENT_WRITER + +// Data structures that are one byte per element can be +// directly copied, regardles of endian-ness. +#define IMPLEMENT_WRITER(ucase, lcase)\ +void BinaryOutput::write##ucase(const lcase* out, int n) {\ + if (sizeof(lcase) == 1) {\ + writeBytes((void*)out, n);\ + } else {\ + for (int i = 0; i < n ; ++i) {\ + write##ucase(out[i]);\ + }\ + }\ +} + +IMPLEMENT_WRITER(Bool8, bool) +IMPLEMENT_WRITER(UInt8, uint8) +IMPLEMENT_WRITER(Int8, int8) + +#undef IMPLEMENT_WRITER + + +#define IMPLEMENT_WRITER(ucase, lcase)\ +void BinaryOutput::write##ucase(const lcase* out, int n) {\ + if (m_swapBytes) {\ + for (int i = 0; i < n; ++i) {\ + write##ucase(out[i]);\ + }\ + } else {\ + writeBytes((const void*)out, sizeof(lcase) * n);\ + }\ +} + + +IMPLEMENT_WRITER(UInt16, uint16) +IMPLEMENT_WRITER(Int16, int16) +IMPLEMENT_WRITER(UInt32, uint32) +IMPLEMENT_WRITER(Int32, int32) +IMPLEMENT_WRITER(UInt64, uint64) +IMPLEMENT_WRITER(Int64, int64) +IMPLEMENT_WRITER(Float32, float32) +IMPLEMENT_WRITER(Float64, float64) + +#undef IMPLEMENT_WRITER + + +void BinaryOutput::reallocBuffer(size_t bytes, size_t oldBufferLen) { + //debugPrintf("reallocBuffer(%d, %d)\n", bytes, oldBufferLen); + + size_t newBufferLen = (int)(m_bufferLen * 1.5) + 100; + uint8* newBuffer = NULL; + + if ((m_filename == "<memory>") || (newBufferLen < MAX_BINARYOUTPUT_BUFFER_SIZE)) { + // We're either writing to memory (in which case we *have* to try and allocate) + // or we've been asked to allocate a reasonable size buffer. + + //debugPrintf(" realloc(%d)\n", newBufferLen); + newBuffer = (uint8*)System::realloc(m_buffer, newBufferLen); + if (newBuffer != NULL) { + m_maxBufferLen = newBufferLen; + } + } + + if ((newBuffer == NULL) && (bytes > 0)) { + // Realloc failed; we're probably out of memory. Back out + // the entire call and try to dump some data to disk. + m_bufferLen = oldBufferLen; + reserveBytesWhenOutOfMemory(bytes); + } else { + m_buffer = newBuffer; + debugAssert(isValidHeapPointer(m_buffer)); + } +} + + +void BinaryOutput::reserveBytesWhenOutOfMemory(size_t bytes) { + if (m_filename == "<memory>") { + throw "Out of memory while writing to memory in BinaryOutput (no RAM left)."; + } else if ((int)bytes > (int)m_maxBufferLen) { + throw "Out of memory while writing to disk in BinaryOutput (could not create a large enough buffer)."; + } else { + + // Dump the contents to disk. In order to enable seeking backwards, + // we keep the last 10 MB in memory. + int writeBytes = m_bufferLen - 10 * 1024 * 1024; + + if (writeBytes < m_bufferLen / 3) { + // We're going to write less than 1/3 of the file; + // give up and just write the whole thing. + writeBytes = m_bufferLen; + } + debugAssert(writeBytes > 0); + + //debugPrintf("Writing %d bytes to disk\n", writeBytes); + + const char* mode = (m_alreadyWritten > 0) ? "ab" : "wb"; + FILE* file = fopen(m_filename.c_str(), mode); + debugAssert(file); + + size_t count = fwrite(m_buffer, 1, writeBytes, file); + debugAssert((int)count == writeBytes); (void)count; + + fclose(file); + file = NULL; + + // Record that we saved this data. + m_alreadyWritten += writeBytes; + m_bufferLen -= writeBytes; + m_pos -= writeBytes; + + debugAssert(m_bufferLen < m_maxBufferLen); + debugAssert(m_bufferLen >= 0); + debugAssert(m_pos >= 0); + debugAssert(m_pos <= m_bufferLen); + + // Shift the unwritten data back appropriately in the buffer. + debugAssert(isValidHeapPointer(m_buffer)); + System::memcpy(m_buffer, m_buffer + writeBytes, m_bufferLen); + debugAssert(isValidHeapPointer(m_buffer)); + + // *now* we allocate bytes (there should presumably be enough + // space in the buffer; if not, we'll come back through this + // code and dump the last 10MB to disk as well. Note that the + // bytes > maxBufferLen case above would already have triggered + // if this call couldn't succeed. + reserveBytes(bytes); + } +} + + +BinaryOutput::BinaryOutput() { + m_alreadyWritten = 0; + m_swapBytes = false; + m_pos = 0; + m_filename = "<memory>"; + m_buffer = NULL; + m_bufferLen = 0; + m_maxBufferLen = 0; + m_beginEndBits = 0; + m_bitString = 0; + m_bitPos = 0; + m_ok = true; + m_committed = false; +} + + +BinaryOutput::BinaryOutput( + const std::string& filename, + G3DEndian fileEndian) { + + m_pos = 0; + m_alreadyWritten = 0; + setEndian(fileEndian); + m_filename = filename; + m_buffer = NULL; + m_bufferLen = 0; + m_maxBufferLen = 0; + m_beginEndBits = 0; + m_bitString = 0; + m_bitPos = 0; + m_committed = false; + + m_ok = true; + /** Verify ability to write to disk */ + commit(false); + m_committed = false; +} + + +void BinaryOutput::reset() { + debugAssert(m_beginEndBits == 0); + alwaysAssertM(m_filename == "<memory>", + "Can only reset a BinaryOutput that writes to memory."); + + // Do not reallocate, just clear the size of the buffer. + m_pos = 0; + m_alreadyWritten = 0; + m_bufferLen = 0; + m_beginEndBits = 0; + m_bitString = 0; + m_bitPos = 0; + m_committed = false; +} + + +BinaryOutput::~BinaryOutput() { + debugAssert((m_buffer == NULL) || isValidHeapPointer(m_buffer)); + System::free(m_buffer); + m_buffer = NULL; + m_bufferLen = 0; + m_maxBufferLen = 0; +} + + +void BinaryOutput::setEndian(G3DEndian fileEndian) { + m_fileEndian = fileEndian; + m_swapBytes = (fileEndian != System::machineEndian()); +} + + +bool BinaryOutput::ok() const { + return m_ok; +} + + +void BinaryOutput::compress() { + if (m_alreadyWritten > 0) { + throw "Cannot compress huge files (part of this file has already been written to disk)."; + } + + // Old buffer size + int L = m_bufferLen; + uint8* convert = (uint8*)&L; + + // Zlib requires the output buffer to be this big + unsigned long newSize = iCeil(m_bufferLen * 1.01) + 12; + uint8* temp = (uint8*)System::malloc(newSize); + int result = compress2(temp, &newSize, m_buffer, m_bufferLen, 9); + + debugAssert(result == Z_OK); (void)result; + + // Write the header + if (m_swapBytes) { + m_buffer[0] = convert[3]; + m_buffer[1] = convert[2]; + m_buffer[2] = convert[1]; + m_buffer[3] = convert[0]; + } else { + m_buffer[0] = convert[0]; + m_buffer[1] = convert[1]; + m_buffer[2] = convert[2]; + m_buffer[3] = convert[3]; + } + + // Write the data + if ((int64)newSize + 4 > (int64)m_maxBufferLen) { + m_maxBufferLen = newSize + 4; + m_buffer = (uint8*)System::realloc(m_buffer, m_maxBufferLen); + } + m_bufferLen = newSize + 4; + System::memcpy(m_buffer + 4, temp, newSize); + m_pos = m_bufferLen; + + System::free(temp); +} + + +void BinaryOutput::commit(bool flush) { + debugAssertM(! m_committed, "Cannot commit twice"); + m_committed = true; + debugAssertM(m_beginEndBits == 0, "Missing endBits before commit"); + + // Make sure the directory exists. + std::string root, base, ext, path; + Array<std::string> pathArray; + parseFilename(m_filename, root, pathArray, base, ext); + + path = root + stringJoin(pathArray, '/'); + if (! fileExists(path, false)) { + createDirectory(path); + } + + const char* mode = (m_alreadyWritten > 0) ? "ab" : "wb"; + + FILE* file = fopen(m_filename.c_str(), mode); + + m_ok = (file != NULL) && m_ok; + + if (m_ok) { + debugAssertM(file, std::string("Could not open '") + m_filename + "'"); + + if (m_buffer != NULL) { + m_alreadyWritten += m_bufferLen; + + int success = fwrite(m_buffer, m_bufferLen, 1, file); + (void)success; + debugAssertM(success == 1, std::string("Could not write to '") + m_filename + "'"); + } + if (flush) { + fflush(file); + } + fclose(file); + file = NULL; + } +} + + +void BinaryOutput::commit( + uint8* out) { + debugAssertM(! m_committed, "Cannot commit twice"); + m_committed = true; + + System::memcpy(out, m_buffer, m_bufferLen); +} + + +void BinaryOutput::writeUInt16(uint16 u) { + reserveBytes(2); + + uint8* convert = (uint8*)&u; + + if (m_swapBytes) { + m_buffer[m_pos] = convert[1]; + m_buffer[m_pos + 1] = convert[0]; + } else { + *(uint16*)(m_buffer + m_pos) = u; + } + + m_pos += 2; +} + + +void BinaryOutput::writeUInt32(uint32 u) { + reserveBytes(4); + + uint8* convert = (uint8*)&u; + + debugAssert(m_beginEndBits == 0); + + if (m_swapBytes) { + m_buffer[m_pos] = convert[3]; + m_buffer[m_pos + 1] = convert[2]; + m_buffer[m_pos + 2] = convert[1]; + m_buffer[m_pos + 3] = convert[0]; + } else { + *(uint32*)(m_buffer + m_pos) = u; + } + + m_pos += 4; +} + + +void BinaryOutput::writeUInt64(uint64 u) { + reserveBytes(8); + + uint8* convert = (uint8*)&u; + + if (m_swapBytes) { + m_buffer[m_pos] = convert[7]; + m_buffer[m_pos + 1] = convert[6]; + m_buffer[m_pos + 2] = convert[5]; + m_buffer[m_pos + 3] = convert[4]; + m_buffer[m_pos + 4] = convert[3]; + m_buffer[m_pos + 5] = convert[2]; + m_buffer[m_pos + 6] = convert[1]; + m_buffer[m_pos + 7] = convert[0]; + } else { + *(uint64*)(m_buffer + m_pos) = u; + } + + m_pos += 8; +} + + +void BinaryOutput::writeString(const char* s) { + // +1 is because strlen doesn't count the null + int len = strlen(s) + 1; + + debugAssert(m_beginEndBits == 0); + reserveBytes(len); + System::memcpy(m_buffer + m_pos, s, len); + m_pos += len; +} + + +void BinaryOutput::writeStringEven(const char* s) { + // +1 is because strlen doesn't count the null + int len = strlen(s) + 1; + + reserveBytes(len); + System::memcpy(m_buffer + m_pos, s, len); + m_pos += len; + + // Pad with another NULL + if ((len % 2) == 1) { + writeUInt8(0); + } +} + + +void BinaryOutput::writeString32(const char* s) { + writeUInt32(strlen(s) + 1); + writeString(s); +} + + +void BinaryOutput::writeVector4(const Vector4& v) { + writeFloat32(v.x); + writeFloat32(v.y); + writeFloat32(v.z); + writeFloat32(v.w); +} + + +void BinaryOutput::writeVector3(const Vector3& v) { + writeFloat32(v.x); + writeFloat32(v.y); + writeFloat32(v.z); +} + + +void BinaryOutput::writeVector2(const Vector2& v) { + writeFloat32(v.x); + writeFloat32(v.y); +} + + +void BinaryOutput::writeColor4(const Color4& v) { + writeFloat32(v.r); + writeFloat32(v.g); + writeFloat32(v.b); + writeFloat32(v.a); +} + + +void BinaryOutput::writeColor3(const Color3& v) { + writeFloat32(v.r); + writeFloat32(v.g); + writeFloat32(v.b); +} + + +void BinaryOutput::beginBits() { + debugAssertM(m_beginEndBits == 0, "Already in beginBits...endBits"); + m_bitString = 0x00; + m_bitPos = 0; + m_beginEndBits = 1; +} + + +void BinaryOutput::writeBits(uint32 value, int numBits) { + + while (numBits > 0) { + // Extract the current bit of value and + // insert it into the current byte + m_bitString |= (value & 1) << m_bitPos; + ++m_bitPos; + value = value >> 1; + --numBits; + + if (m_bitPos > 7) { + // We've reached the end of this byte + writeUInt8(m_bitString); + m_bitString = 0x00; + m_bitPos = 0; + } + } +} + + +void BinaryOutput::endBits() { + debugAssertM(m_beginEndBits == 1, "Not in beginBits...endBits"); + if (m_bitPos > 0) { + writeUInt8(m_bitString); + } + m_bitString = 0; + m_bitPos = 0; + m_beginEndBits = 0; +} + +} diff --git a/externals/g3dlite/Box.cpp b/externals/g3dlite/Box.cpp new file mode 100644 index 00000000000..f7c112ae3a5 --- /dev/null +++ b/externals/g3dlite/Box.cpp @@ -0,0 +1,393 @@ +/** + @file Box.cpp + Box class + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2001-06-02 + @edited 2006-02-05 +*/ + +#include "G3D/Box.h" +#include "G3D/debug.h" +#include "G3D/Plane.h" +#include "G3D/AABox.h" +#include "G3D/CoordinateFrame.h" + +namespace G3D { + +/** + Sets a field on four vertices. Used by the constructor. + */ +#define setMany(i0, i1, i2, i3, field, extreme) \ + _corner[i0].field = _corner[i1].field = \ + _corner[i2].field = _corner[i3].field = \ + (extreme).field + +Box::Box() { +} + + +Box::Box(const AABox& b) { + init(b.low(), b.high()); +} + +Box::Box(class BinaryInput& b) { + deserialize(b); +} + + +void Box::serialize(class BinaryOutput& b) const { + int i; + for (i = 0; i < 8; ++i) { + _corner[i].serialize(b); + } + + // Other state can be reconstructed +} + + +void Box::deserialize(class BinaryInput& b) { + int i; + + _center = Vector3::zero(); + for (i = 0; i < 8; ++i) { + _corner[i].deserialize(b); + _center += _corner[i]; + } + + _center = _center / 8; + + // Reconstruct other state from the corners + _axis[0] = _corner[5] - _corner[4]; + _axis[1] = _corner[7] - _corner[4]; + _axis[2] = _corner[0] - _corner[4]; + + for (i = 0; i < 3; ++i) { + _extent[i] = _axis[i].magnitude(); + _axis[i] /= _extent[i]; + } + + _volume = _extent.x * _extent.y * _extent.z; + + _area = 2 * + (_extent.x * _extent.y + + _extent.y * _extent.z + + _extent.z * _extent.x); +} + + +Box::Box( + const Vector3& min, + const Vector3& max) { + + init(min.min(max), min.max(max)); + +} + +void Box::init( + const Vector3& min, + const Vector3& max) { + + debugAssert( + (min.x <= max.x) && + (min.y <= max.y) && + (min.z <= max.z)); + + setMany(0, 1, 2, 3, z, max); + setMany(4, 5, 6, 7, z, min); + + setMany(1, 2, 5, 6, x, max); + setMany(0, 3, 4, 7, x, min); + + setMany(3, 2, 6, 7, y, max); + setMany(0, 1, 5, 4, y, min); + + _extent = max - min; + + _axis[0] = Vector3::unitX(); + _axis[1] = Vector3::unitY(); + _axis[2] = Vector3::unitZ(); + + if (_extent.isFinite()) { + _volume = _extent.x * _extent.y * _extent.z; + } else { + _volume = G3D::finf(); + } + + debugAssert(! isNaN(_extent.x)); + + _area = 2 * + (_extent.x * _extent.y + + _extent.y * _extent.z + + _extent.z * _extent.x); + + _center = (max + min) * 0.5f; + + // If the extent is infinite along an axis, make the center zero to avoid NaNs + for (int i = 0; i < 3; ++i) { + if (! G3D::isFinite(_extent[i])) { + _center[i] = 0.0f; + } + } +} + + +float Box::volume() const { + return _volume; +} + + +float Box::area() const { + return _area; +} + + +void Box::getLocalFrame(CoordinateFrame& frame) const { + + frame.rotation = Matrix3( + _axis[0][0], _axis[1][0], _axis[2][0], + _axis[0][1], _axis[1][1], _axis[2][1], + _axis[0][2], _axis[1][2], _axis[2][2]); + + frame.translation = _center; +} + + +CoordinateFrame Box::localFrame() const { + CoordinateFrame out; + getLocalFrame(out); + return out; +} + + +void Box::getFaceCorners(int f, Vector3& v0, Vector3& v1, Vector3& v2, Vector3& v3) const { + switch (f) { + case 0: + v0 = _corner[0]; v1 = _corner[1]; v2 = _corner[2]; v3 = _corner[3]; + break; + + case 1: + v0 = _corner[1]; v1 = _corner[5]; v2 = _corner[6]; v3 = _corner[2]; + break; + + case 2: + v0 = _corner[7]; v1 = _corner[6]; v2 = _corner[5]; v3 = _corner[4]; + break; + + case 3: + v0 = _corner[2]; v1 = _corner[6]; v2 = _corner[7]; v3 = _corner[3]; + break; + + case 4: + v0 = _corner[3]; v1 = _corner[7]; v2 = _corner[4]; v3 = _corner[0]; + break; + + case 5: + v0 = _corner[1]; v1 = _corner[0]; v2 = _corner[4]; v3 = _corner[5]; + break; + + default: + debugAssert((f >= 0) && (f < 6)); + } +} + + + +int Box::dummy = 0; + +bool Box::culledBy( + const Array<Plane>& plane, + int& cullingPlane, + const uint32 _inMask, + uint32& childMask) const { + + uint32 inMask = _inMask; + assert(plane.size() < 31); + + childMask = 0; + + // See if there is one plane for which all of the + // vertices are in the negative half space. + for (int p = 0; p < plane.size(); ++p) { + + // Only test planes that are not masked + if ((inMask & 1) != 0) { + + Vector3 corner; + + int numContained = 0; + int v = 0; + + // We can early-out only if we have found one point on each + // side of the plane (i.e. if we are straddling). That + // occurs when (numContained < v) && (numContained > 0) + for (v = 0; (v < 8) && ((numContained == v) || (numContained == 0)); ++v) { + if (plane[p].halfSpaceContains(_corner[v])) { + ++numContained; + } + } + + if (numContained == 0) { + // Plane p culled the box + cullingPlane = p; + + // The caller should not recurse into the children, + // since the parent is culled. If they do recurse, + // make them only test against this one plane, which + // will immediately cull the volume. + childMask = 1 << p; + return true; + + } else if (numContained < v) { + // The bounding volume straddled the plane; we have + // to keep testing against this plane + childMask |= (1 << p); + } + } + + // Move on to the next bit. + inMask = inMask >> 1; + } + + // None of the planes could cull this box + cullingPlane = -1; + return false; +} + + +bool Box::culledBy( + const Array<Plane>& plane, + int& cullingPlane, + const uint32 _inMask) const { + + uint32 inMask = _inMask; + assert(plane.size() < 31); + + // See if there is one plane for which all of the + // vertices are in the negative half space. + for (int p = 0; p < plane.size(); ++p) { + + // Only test planes that are not masked + if ((inMask & 1) != 0) { + + bool culled = true; + + int v; + + // Assume this plane culls all points. See if there is a point + // not culled by the plane... early out when at least one point + // is in the positive half space. + for (v = 0; (v < 8) && culled; ++v) { + culled = ! plane[p].halfSpaceContains(corner(v)); + } + + if (culled) { + // Plane p culled the box + cullingPlane = p; + + return true; + } + } + + // Move on to the next bit. + inMask = inMask >> 1; + } + + // None of the planes could cull this box + cullingPlane = -1; + return false; +} + + +bool Box::contains( + const Vector3& point) const { + + // Form axes from three edges, transform the point into that + // space, and perform 3 interval tests + + Vector3 u = _corner[4] - _corner[0]; + Vector3 v = _corner[3] - _corner[0]; + Vector3 w = _corner[1] - _corner[0]; + + Matrix3 M = Matrix3(u.x, v.x, w.x, + u.y, v.y, w.y, + u.z, v.z, w.z); + + // M^-1 * (point - _corner[0]) = point in unit cube's object space + // compute the inverse of M + Vector3 osPoint = M.inverse() * (point - _corner[0]); + + return + (osPoint.x >= 0) && + (osPoint.y >= 0) && + (osPoint.z >= 0) && + (osPoint.x <= 1) && + (osPoint.y <= 1) && + (osPoint.z <= 1); +} + +#undef setMany + + +void Box::getRandomSurfacePoint(Vector3& P, Vector3& N) const { + float aXY = _extent.x * _extent.y; + float aYZ = _extent.y * _extent.z; + float aZX = _extent.z * _extent.x; + + float r = (float)uniformRandom(0, aXY + aYZ + aZX); + + // Choose evenly between positive and negative face planes + float d = (uniformRandom(0, 1) < 0.5f) ? -1.0f : 1.0f; + + // The probability of choosing a given face is proportional to + // its area. + if (r < aXY) { + P = _axis[0] * (float)uniformRandom(-0.5, 0.5) * _extent.x + + _axis[1] * (float)uniformRandom(-0.5, 0.5) * _extent.y + + _center + _axis[2] * d * _extent.z * 0.5f; + N = _axis[2] * d; + } else if (r < aYZ) { + P = _axis[1] * (float)uniformRandom(-0.5, 0.5) * _extent.y + + _axis[2] * (float)uniformRandom(-0.5, 0.5) * _extent.z + + _center + _axis[0] * d * _extent.x * 0.5f; + N = _axis[0] * d; + } else { + P = _axis[2] * (float)uniformRandom(-0.5, 0.5) * _extent.z + + _axis[0] *(float) uniformRandom(-0.5, 0.5) * _extent.x + + _center + _axis[1] * d * _extent.y * 0.5f; + N = _axis[1] * d; + } +} + + +Vector3 Box::randomInteriorPoint() const { + Vector3 sum = _center; + + for (int a = 0; a < 3; ++a) { + sum += _axis[a] * (float)uniformRandom(-0.5, 0.5) * _extent[a]; + } + + return sum; +} + +Box Box::inf() { + return Box(-Vector3::inf(), Vector3::inf()); +} + +void Box::getBounds(class AABox& aabb) const { + + Vector3 lo = _corner[0]; + Vector3 hi = lo; + + for (int v = 1; v < 8; ++v) { + const Vector3& C = _corner[v]; + lo = lo.min(C); + hi = hi.max(C); + } + + aabb = AABox(lo, hi); +} + + +} // namespace diff --git a/externals/g3dlite/CMakeLists.txt b/externals/g3dlite/CMakeLists.txt new file mode 100644 index 00000000000..fa80bb75445 --- /dev/null +++ b/externals/g3dlite/CMakeLists.txt @@ -0,0 +1,11 @@ +file(GLOB sources *.cpp) + +SET(g3dlib_STAT_SRCS + ${sources} + ) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} + ) + +add_library(g3dlib STATIC ${g3dlib_STAT_SRCS}) diff --git a/externals/g3dlite/Capsule.cpp b/externals/g3dlite/Capsule.cpp new file mode 100644 index 00000000000..2ad3891c960 --- /dev/null +++ b/externals/g3dlite/Capsule.cpp @@ -0,0 +1,179 @@ +/** + @file Capsule.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2003-02-07 + @edited 2005-08-18 + + Copyright 2000-2009, Morgan McGuire. + All rights reserved. + */ + +#include "G3D/Capsule.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/LineSegment.h" +#include "G3D/Sphere.h" +#include "G3D/CoordinateFrame.h" +#include "G3D/Line.h" +#include "G3D/AABox.h" + +namespace G3D { + +Capsule::Capsule(class BinaryInput& b) { + deserialize(b); +} + + +Capsule::Capsule() { +} + + +Capsule::Capsule(const Vector3& _p1, const Vector3& _p2, float _r) + : p1(_p1), p2(_p2), _radius(_r) { +} + + +void Capsule::serialize(class BinaryOutput& b) const { + p1.serialize(b); + p2.serialize(b); + b.writeFloat64(_radius); +} + + +void Capsule::deserialize(class BinaryInput& b) { + p1.deserialize(b); + p2.deserialize(b); + _radius = b.readFloat64(); +} + + +Line Capsule::axis() const { + return Line::fromTwoPoints(p1, p2); +} + + +float Capsule::volume() const { + return + // Sphere volume + pow(_radius, 3) * pi() * 4 / 3 + + + // Cylinder volume + pow(_radius, 2) * (p1 - p2).magnitude(); +} + + +float Capsule::area() const { + + return + // Sphere area + pow(_radius, 2) * 4 * pi() + + + // Cylinder area + twoPi() * _radius * (p1 - p2).magnitude(); +} + + +void Capsule::getBounds(AABox& out) const { + Vector3 min = p1.min(p2) - (Vector3(1, 1, 1) * _radius); + Vector3 max = p1.max(p2) + (Vector3(1, 1, 1) * _radius); + + out = AABox(min, max); +} + + +bool Capsule::contains(const Vector3& p) const { + return LineSegment::fromTwoPoints(p1, p2).distanceSquared(p) <= square(radius()); +} + + +void Capsule::getRandomSurfacePoint(Vector3& p, Vector3& N) const { + float h = height(); + float r = radius(); + + // Create a random point on a standard capsule and then rotate to the global frame. + + // Relative areas + float capRelArea = sqrt(r) / 2.0f; + float sideRelArea = r * h; + + float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea); + + if (r1 < capRelArea * 2) { + + // Select a point uniformly at random on a sphere + N = Sphere(Vector3::zero(), 1).randomSurfacePoint(); + p = N * r; + p.y += sign(p.y) * h / 2.0f; + } else { + // Side + float a = uniformRandom(0, (float)twoPi()); + N.x = cos(a); + N.y = 0; + N.z = sin(a); + p.x = N.x * r; + p.z = N.y * r; + p.y = uniformRandom(-h / 2.0f, h / 2.0f); + } + + // Transform to world space + CoordinateFrame cframe; + getReferenceFrame(cframe); + + p = cframe.pointToWorldSpace(p); + N = cframe.normalToWorldSpace(N); +} + + +void Capsule::getReferenceFrame(CoordinateFrame& cframe) const { + cframe.translation = center(); + + Vector3 Y = (p1 - p2).direction(); + Vector3 X = (abs(Y.dot(Vector3::unitX())) > 0.9) ? Vector3::unitY() : Vector3::unitX(); + Vector3 Z = X.cross(Y).direction(); + X = Y.cross(Z); + cframe.rotation.setColumn(0, X); + cframe.rotation.setColumn(1, Y); + cframe.rotation.setColumn(2, Z); +} + + +Vector3 Capsule::randomInteriorPoint() const { + float h = height(); + float r = radius(); + + // Create a random point in a standard capsule and then rotate to the global frame. + + Vector3 p; + + float hemiVolume = pi() * (r*r*r) * 4 / 6.0; + float cylVolume = pi() * square(r) * h; + + float r1 = uniformRandom(0, 2.0 * hemiVolume + cylVolume); + + if (r1 < 2.0 * hemiVolume) { + + p = Sphere(Vector3::zero(), r).randomInteriorPoint(); + + p.y += sign(p.y) * h / 2.0f; + + } else { + + // Select a point uniformly at random on a disk + float a = uniformRandom(0, (float)twoPi()); + float r2 = sqrt(uniformRandom(0, 1)) * r; + + p = Vector3(cos(a) * r2, + uniformRandom(-h / 2.0f, h / 2.0f), + sin(a) * r2); + } + + // Transform to world space + CoordinateFrame cframe; + getReferenceFrame(cframe); + + return cframe.pointToWorldSpace(p); +} + +} // namespace diff --git a/externals/g3dlite/CollisionDetection.cpp b/externals/g3dlite/CollisionDetection.cpp new file mode 100644 index 00000000000..77eef0a5500 --- /dev/null +++ b/externals/g3dlite/CollisionDetection.cpp @@ -0,0 +1,2455 @@ +/** + @file CollisionDetection.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @cite Bounce direction based on Paul Nettle's ftp://ftp.3dmaileffects.com/pub/FluidStudios/CollisionDetection/Fluid_Studios_Generic_Collision_Detection_for_Games_Using_Ellipsoids.pdf and comments by Max McGuire. Ray-sphere code by Eric Haines. + + @created 2001-11-24 + @edited 2008-12-29 + */ + +#include "G3D/CoordinateFrame.h" +#include "G3D/platform.h" +#include "G3D/CollisionDetection.h" +#include "G3D/debugAssert.h" +#include "G3D/vectorMath.h" +#include "G3D/Capsule.h" +#include "G3D/Plane.h" +#include "G3D/Line.h" +#include "G3D/LineSegment.h" +#include "G3D/Sphere.h" +#include "G3D/Box.h" +#include "G3D/Triangle.h" +#include "G3D/Vector3.h" +#include "G3D/AABox.h" + +#ifdef _MSC_VER +// Turn on fast floating-point optimizations +#pragma float_control( push ) +#pragma fp_contract( on ) +#pragma fenv_access( off ) +#pragma float_control( except, off ) +#pragma float_control( precise, off ) +#endif + + +namespace G3D { + +bool CollisionDetection::ignoreBool; +Vector3 CollisionDetection::ignore; +Array<Vector3> CollisionDetection::ignoreArray; + + + +Vector3 CollisionDetection::separatingAxisForSolidBoxSolidBox( + const int separatingAxisIndex, + const Box & box1, + const Box & box2) { + debugAssert(separatingAxisIndex >= 0); + debugAssert(separatingAxisIndex < 15); + Vector3 axis; + if (separatingAxisIndex < 3) { + axis = box1.axis(separatingAxisIndex); + } else if (separatingAxisIndex < 6) { + axis = box2.axis(separatingAxisIndex - 3); + } else { + int box1Index = (separatingAxisIndex - 6) / 3; + int box2Index = (separatingAxisIndex - 6) % 3; + axis = cross(box1.axis(box1Index), box2.axis(box2Index)); + } + return axis; +} + +#ifdef _MSC_VER +# pragma warning (push) +# pragma warning (disable : 4244) +#endif + +float CollisionDetection::projectedDistanceForSolidBoxSolidBox( + const int separatingAxisIndex, + const Vector3 & a, + const Vector3 & b, + const Vector3 & D, + const double* c, + const double* ca, + const double* ad, + const double* bd) +{ + (void)D; + + float R0 = 0.0f; + float R1 = 0.0f; + float R = 0.0f; + switch (separatingAxisIndex) { + case 0: + // A0 + R0 = a[0]; + R1 = b[0] * ca[0] + b[1] * ca[1] + b[2] * ca[2]; + R = fabs(ad[0]); + break; + case 1: + // A1 + R0 = a[1]; + R1 = b[0] * ca[3] + b[1] * ca[4] + b[2] * ca[5]; + R = fabs(ad[1]); + break; + case 2: + // A2 + R0 = a[2]; + R1 = b[0] * ca[6] + b[1] * ca[7] + b[2] * ca[8]; + R = fabs(ad[2]); + break; + case 3: + // B0 + R0 = a[0] * ca[0] + a[1] * ca[3] + a[2] * ca[6]; + R1 = b[0]; + R = fabs(bd[0]); + break; + case 4: + // B1 + R0 = a[0] * ca[1] + a[1] * ca[4] + a[2] * ca[7]; + R1 = b[1]; + R = fabs(bd[1]); + break; + case 5: + // B2 + R0 = a[0] * ca[2] + a[1] * ca[5] + a[2] * ca[8]; + R1 = b[2]; + R = fabs(bd[2]); + break; + case 6: + // A0 x B0 + R0 = a[1] * ca[6] + a[2] * ca[3]; + R1 = b[1] * ca[2] + b[2] * ca[1]; + R = fabs(c[3] * ad[2] - c[6] * ad[1]); + break; + case 7: + // A0 x B1 + R0 = a[1] * ca[7] + a[2] * ca[4]; + R1 = b[0] * ca[2] + b[2] * ca[0]; + R = fabs(c[4] * ad[2] - c[7] * ad[1]); + break; + case 8: + // A0 x B2 + R0 = a[1] * ca[8] + a[2] * ca[5]; + R1 = b[0] * ca[1] + b[1] * ca[0]; + R = fabs(c[5] * ad[2] - c[8] * ad[1]); + break; + case 9: + // A1 x B0 + R0 = a[0] * ca[6] + a[2] * ca[0]; + R1 = b[1] * ca[5] + b[2] * ca[4]; + R = fabs(c[6] * ad[0] - c[0] * ad[2]); + break; + case 10: + // A1 x B1 + R0 = a[0] * ca[7] + a[2] * ca[1]; + R1 = b[0] * ca[5] + b[2] * ca[3]; + R = fabs(c[7] * ad[0] - c[1] * ad[2]); + break; + case 11: + // A1 x B2 + R0 = a[0] * ca[8] + a[2] * ca[2]; + R1 = b[0] * ca[4] + b[1] * ca[3]; + R = fabs(c[8] * ad[0] - c[2] * ad[2]); + break; + case 12: + // A2 x B0 + R0 = a[0] * ca[3] + a[1] * ca[0]; + R1 = b[1] * ca[8] + b[2] * ca[7]; + R = fabs(c[0] * ad[1] - c[3] * ad[0]); + break; + case 13: + // A2 x B1 + R0 = a[0] * ca[4] + a[1] * ca[1]; + R1 = b[0] * ca[8] + b[2] * ca[6]; + R = fabs(c[1] * ad[1] - c[4] * ad[0]); + break; + case 14: + // A2 x B2 + R0 = a[0] * ca[5] + a[1] * ca[2]; + R1 = b[0] * ca[7] + b[1] * ca[6]; + R = fabs(c[2] * ad[1] - c[5] * ad[0]); + break; + default: + debugAssertM(false, "fell through switch statement"); + } + + return (R - (R0 + R1)); +} + + +bool CollisionDetection::parallelAxisForSolidBoxSolidBox( + const double* ca, + const double epsilon, + int & axis1, + int & axis2) { + const double parallelDot = 1.0 - epsilon; + for (int i = 0; i < 9; i++) { + if (ca[i] >= parallelDot) { + axis1 = i / 3; + axis2 = i % 3; + return true; + } + } + return false; +} + + + + +void CollisionDetection::fillSolidBoxSolidBoxInfo( + const Box & box1, + const Box & box2, + Vector3 & a, + Vector3 & b, + Vector3 & D, + double* c, + double* ca, + double* ad, + double* bd) { + // length between center and each side of box1 and box2 + a = box1.extent() * 0.5; + b = box2.extent() * 0.5; + + // difference between centers of box1 and box2 + D = box2.center() - box1.center(); + + // store the value of all possible dot products between the + // axes of box1 and box2, c_{row, col} in the Eberly paper + // corresponds to c[row * 3 + col] for this 9 element array. + // + // c[] holds signed values, ca[] hold absolute values + for (int i = 0; i < 9; i++) { + c[i] = dot(box1.axis(i / 3), box2.axis(i % 3)); + ca[i] = fabs(c[i]); + } + + // store all possible dot products between the axes of box1 and D, + // as well as the axes of box2 and D + for (int i = 0; i < 3; i++) { + ad[i] = dot(box1.axis(i), D); + bd[i] = dot(box2.axis(i), D); + } +} + + + +bool CollisionDetection::conservativeBoxBoxTest( + const Vector3 & a, const Vector3 & b, const Vector3 & D) { + // do a quick bounding sphere test because it is relatively + // cheap, (three dot products, two sqrts, and a few others) + double boxRadius1 = a.magnitude(); + double boxRadius2 = b.magnitude(); + return (D.squaredMagnitude() < square(boxRadius1 + boxRadius2)); +} + + + + +bool CollisionDetection::fixedSolidBoxIntersectsFixedSolidBox( + const Box& box1, + const Box& box2, + const int lastSeparatingAxis) { + // for explanations of the variable please refer to the + // paper and fillSolidBoxSolidBoxInfo() + Vector3 a; + Vector3 b; + Vector3 D; + double c[9]; + double ca[9]; + double ad[3]; + double bd[3]; + + fillSolidBoxSolidBoxInfo(box1, box2, a, b, D, c, ca, ad, bd); + + int dummy1, dummy2; + bool parallelAxes = parallelAxisForSolidBoxSolidBox(ca, 0.00001, + dummy1, dummy2); + + // check the separating axis from the last time step + if (lastSeparatingAxis != -1 && + (lastSeparatingAxis < 6 || !parallelAxes)) { + double projectedDistance = projectedDistanceForSolidBoxSolidBox( + lastSeparatingAxis, a, b, D, c, ca, ad, bd); + + // the separating axis from the last time step is still + // valid, the boxes do not intersect + if (projectedDistance > 0.0) { + return false; + } + } + + // test if the boxes can be separated by a plane normal to + // any of the three axes of box1, any of the three axes of box2, + // or any of the 9 possible cross products of axes from box1 + // and box2 + for (int i = 0; i < 15; i++) { + // do not need to check edge-edge cases if any two of + // the axes are parallel + if (parallelAxes && i == 6) { + return true; + } + + double projectedDistance = + projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd); + + // found a separating axis, the boxes do not intersect + if (projectedDistance > 0.0) { + return false; + } + } + + return true; +} + + + +void CollisionDetection::closestPointsBetweenLineAndLine( + const Line & line1, + const Line & line2, + Vector3 & closest1, + Vector3 & closest2) { + // TODO make accessors for Line that don't make a copy of data + Vector3 P0 = line1.point(); + Vector3 u = line1.direction(); + Vector3 Q0 = line2.point(); + Vector3 v = line2.direction(); + Vector3 w0 = P0 - Q0; + + // a = 1.0, c = 1.0 + double b = dot(u, v); + double d = dot(u, w0); + double e = dot(v, w0); + double D = 1.0 - b * b; + double sc, tc; + + static const double epsilon = 0.00001; + + if (D < epsilon) { + // lines are parallel, choose P0 as one point, find the point + // on line2 that is closest to P0 + sc = 0.0; + tc = (b > 1.0) ? (d / b) : (e / 1.0); + } else { + // lines are not parallel + sc = (b * e - 1.0 * d) / D; + tc = (1.0 * e - b * d) / D; + } + + closest1 = P0 + (sc * u); + closest2 = Q0 + (tc * v); +} + + + +float CollisionDetection::penetrationDepthForFixedBoxFixedBox( + const Box& box1, + const Box& box2, + Array<Vector3>& contactPoints, + Array<Vector3>& contactNormals, + const int lastSeparatingAxis) { + + contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + + Vector3 a; + Vector3 b; + Vector3 D; + double c[9]; + double ca[9]; + double ad[3]; + double bd[3]; + + debugAssert(lastSeparatingAxis >= -1); + debugAssert(lastSeparatingAxis < 15); + + fillSolidBoxSolidBoxInfo(box1, box2, a, b, D, c, ca, ad, bd); + + int axis1, axis2; + bool parallelAxes = parallelAxisForSolidBoxSolidBox(ca, 0.00001, + axis1, axis2); + + + // check the separating axis from the last time step + if (lastSeparatingAxis != -1 && + (lastSeparatingAxis < 6 || !parallelAxes)) { + float projectedDistance = projectedDistanceForSolidBoxSolidBox( + lastSeparatingAxis, a, b, D, c, ca, ad, bd); + + // the separating axis from the last time step is still + // valid, the boxes do not intersect + if (projectedDistance > 0.0) { + return -projectedDistance; + } + } + + // test if the boxes can be separated by a plane normal to + // any of the three axes of box1, any of the three axes of box2, + // (test 9 possible cross products later) + float penetration = -finf(); + int penetrationAxisIndex = -1; + + for (int i = 0; i < 6; i++) { + float projectedDistance = + projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd); + + // found a separating axis, the boxes do not intersect + if (projectedDistance > 0.0) { + return -projectedDistance; + } + + // keep track of the axis that is least violated + if (projectedDistance > penetration) { + penetration = projectedDistance; + penetrationAxisIndex = i; + } + } + + + // for each edge-edge case we have to adjust the magnitude of + // penetration since we did not include the dot(L, L) denominator + // that can be smaller than 1.0 for the edge-edge cases. + if (!parallelAxes) { + double edgeDistances[9]; + + // run through edge-edge cases to see if we can find a separating axis + for (int i = 6; i < 15; i++) { + float projectedDistance = + projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd); + + // found a separating axis, the boxes do not intersect, + // correct magnitude and return projected distance + if (projectedDistance > 0.0) { + Vector3 L = separatingAxisForSolidBoxSolidBox(i, box1, box2); + projectedDistance /= dot(L, L); + return -projectedDistance; + } + + edgeDistances[i - 6] = projectedDistance; + } + + // no separating axis found, the boxes do intersect, + // correct the magnitudes of the projectedDistance values + for (int i = 6; i < 15; i++) { + // find the negative penetration value with the smallest magnitude, + // the adjustment done for the edge-edge cases only increases + // magnitude by dividing by a number smaller than 1 and greater than 0 + float projectedDistance = (float)edgeDistances[i - 6]; + if (projectedDistance > penetration) { + Vector3 L = separatingAxisForSolidBoxSolidBox(i, box1, box2); + projectedDistance /= dot(L, L); + if (projectedDistance > penetration) { + penetration = projectedDistance; + penetrationAxisIndex = i; + } + } + } + } + + // get final separating axis vector + Vector3 L = separatingAxisForSolidBoxSolidBox(penetrationAxisIndex, + box1, box2); + + // set L to be the normal that faces away from box1 + if (dot(L, D) < 0) { + L = -L; + } + + Vector3 contactPoint; + + if (penetrationAxisIndex < 6) { + // vertex to face collision, find deepest colliding vertex + const Box* vertexBox; + const Box* faceBox; + Vector3 faceNormal = L; + + // L will be the outward facing normal for the faceBox + if (penetrationAxisIndex < 3) { + faceBox = & box1; + vertexBox = & box2; + if (dot(L, D) < 0) { + faceNormal = -L; + } + } else { + faceBox = & box2; + vertexBox = & box1; + if (dot(L, D) > 0) { + faceNormal = -L; + } + } + + // find the vertex that is farthest away in the direction + // face normal direction + int deepestPointIndex = 0; + float deepestPointDot = dot(faceNormal, vertexBox->corner(0)); + for (int i = 1; i < 8; i++) { + float dotProduct = dot(faceNormal, vertexBox->corner(i)); + if (dotProduct < deepestPointDot) { + deepestPointDot = dotProduct; + deepestPointIndex = i; + } + } + + // return the point half way between the deepest point and the + // contacting face + contactPoint = vertexBox->corner(deepestPointIndex) + + (-penetration * 0.5 * faceNormal); + } else { + // edge-edge case, find the two ege lines + int edge1 = (penetrationAxisIndex - 6) / 3; + int edge2 = (penetrationAxisIndex - 6) % 3; + Vector3 linePoint1 = box1.center(); + Vector3 linePoint2 = box2.center(); + Vector3 lineDir1; + Vector3 lineDir2; + + // find edge line by finding the edge axis, and the + // other two axes that are closest to the other box + for (int i = 0; i < 3; i++ ) { + if (i == edge1) { + lineDir1 = box1.axis(i); + } else { + Vector3 axis = box1.axis(i); + if (dot(axis, L) < 0) { + axis = -axis; + } + linePoint1 += axis * a[i]; + } + + if (i == edge2) { + lineDir2 = box2.axis(i); + } else { + Vector3 axis = box2.axis(i); + if (dot(axis, L) > 0) { + axis = -axis; + } + linePoint2 += axis * b[i]; + } + } + + // make lines from the two closest edges, and find + // the points that on each line that are closest to the other + Line line1 = Line::fromPointAndDirection(linePoint1, lineDir1); + Line line2 = Line::fromPointAndDirection(linePoint2, lineDir2); + Vector3 closest1; + Vector3 closest2; + + closestPointsBetweenLineAndLine(line1, line2, closest1, closest2); + + // take the average of the two closest edge points for the final + // contact point + contactPoint = (closest1 + closest2) * 0.5; + } + + contactPoints.push(contactPoint); + contactNormals.push(L); + + return -penetration; + +} + + + + +float CollisionDetection::penetrationDepthForFixedSphereFixedBox( + const Sphere& sphere, + const Box& box, + Array<Vector3>& contactPoints, + Array<Vector3>& contactNormals) { + + contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + + // In its local coordinate frame, the box measures + // 2 * halfExtent[a] along dimesion a. + Vector3 halfExtent(box.extent(0), box.extent(1), box.extent(2)); + halfExtent *= 0.5f; + + CoordinateFrame boxFrame; + box.getLocalFrame(boxFrame); + + // Transform the sphere to the box's coordinate frame. + Vector3 center = boxFrame.pointToObjectSpace(sphere.center); + + // Find the square of the distance from the sphere to the box + + + // Distance along each axis from the closest side of the box + // to the sphere center. Negative values are *inside* the box. + Vector3 distOutsideBox; + + // Divide space up into the 27 regions corresponding + // to {+|-|0}X, {+|-|0}Y, {+|-|0}Z and classify the + // sphere center into one of them. + Vector3 centerRegion; + + // In the edge collision case, the edge is between vertices + // (constant + variable) and (constant - variable). + Vector3 constant, variable; + + int numNonZero = 0; + + // Iterate over axes + for (int a = 0; a < 3; ++a) { + // For each (box side), see which direction the sphere + // is outside the box (positive or negative). Add the + // square of that distance to the total distance from + // the box. + + float distanceFromLow = -halfExtent[a] - center[a]; + float distanceFromHigh = center[a] - halfExtent[a]; + + if (fabsf(distanceFromLow) < fabsf(distanceFromHigh)) { + distOutsideBox[a] = distanceFromLow; + } else { + distOutsideBox[a] = distanceFromHigh; + } + + if (distanceFromLow < 0.0) { + if (distanceFromHigh < 0.0) { + // Inside the box + centerRegion[a] = 0.0; + variable[a] = 1.0; + } else { + // Off the high side + centerRegion[a] = 1.0; + constant[a] = halfExtent[a]; + ++numNonZero; + } + } else if (distanceFromHigh < 0.0) { + // Off the low side + centerRegion[a] = -1.0; + constant[a] = -halfExtent[a]; + ++numNonZero; + } else { + debugAssertM(false, + "distanceFromLow and distanceFromHigh cannot both be positive"); + } + } + + // Squared distance between the outside of the box and the + // sphere center. + float d2 = Vector3::zero().max(distOutsideBox).squaredMagnitude(); + + if (d2 > square(sphere.radius)) { + // There is no penetration because the distance is greater + // than the radius of the sphere. This is the common case + // and we quickly exit. + return -1; + } + + // We know there is some penetration but need to classify it. + // + // Examine the region that contains the center of the sphere. If + // there is exactly one non-zero axis, the collision is with a + // plane. If there are exactly two non-zero axes, the collision + // is with an edge. If all three axes are non-zero, the collision is + // with a vertex. If there are no non-zero axes, the center is inside + // the box. + + double depth = -1; + switch (numNonZero) { + case 3: // Vertex collision + // The collision point is the vertex at constant, the normal + // is the vector from there to the sphere center. + contactNormals.append(boxFrame.normalToWorldSpace(constant - center)); + contactPoints.append(boxFrame.pointToWorldSpace(constant)); + depth = sphere.radius - sqrt(d2); + break; + + case 2: // Edge collision + { + // TODO: unwrapping the edge constructor and closest point + // code will probably make it faster. + + // Determine the edge + Line line = Line::fromPointAndDirection(constant, variable); + + // Penetration depth: + depth = sphere.radius - sqrt(d2); + + // The contact point is the closes point to the sphere on the line + Vector3 X = line.closestPoint(center); + contactNormals.append(boxFrame.normalToWorldSpace(X - center).direction()); + contactPoints.append(boxFrame.pointToWorldSpace(X)); + } + break; + + case 1: // Plane collision + { + // The plane normal is the centerRegion vector, + // so the sphere normal is the negative. Take + // it to world space from box-space. + + // Center region doesn't need to be normalized because + // it is known to contain only one non-zero value + // and that value is +/- 1. + Vector3 N = boxFrame.normalToWorldSpace(-centerRegion); + contactNormals.append(N); + + // Penetration depth: + depth = sphere.radius - sqrtf(d2); + + // Compute the contact point from the penetration depth + contactPoints.append(sphere.center + N * (sphere.radius - depth)); + } + break; + + case 0: // Volume collision + + // The sphere center is inside the box. This is an easy case + // to handle. Note that all axes of distOutsideBox must + // be negative. + + // Arbitratily choose the sphere center as a contact point + contactPoints.append(sphere.center); + + // Find the least-negative penetration axis. + // + // We could have computed this during the loop over the axes, + // but since volume collisions are rare (they only occur with + // large time steps), this case will seldom be executed and + // should not be optimized at the expense of the others. + if (distOutsideBox.x > distOutsideBox.y) { + if (distOutsideBox.x > distOutsideBox.z) { + // Smallest penetration on x-axis + // Chose normal based on which side we're closest to. + // Keep in mind that this is a normal to the sphere, + // so it is the inverse of the box normal. + if (center.x > 0) { + contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitX())); + } else { + contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitX())); + } + depth = -distOutsideBox.x; + } else { + // Smallest penetration on z-axis + goto ZAXIS; + } + } else if (distOutsideBox.y > distOutsideBox.z) { + // Smallest penetration on y-axis + // Chose normal based on which side we're closest to. + // Keep in mind that this is a normal to the sphere, + // so it is the inverse of the box normal. + if (center.y > 0) { + contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitY())); + } else { + contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitY())); + } + depth = -distOutsideBox.y; + } else { + // Smallest on z-axis +ZAXIS: + // Chose normal based on which side we're closest to. + // Keep in mind that this is a normal to the sphere, + // so it is the inverse of the box normal. + if (center.z > 0) { + contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitZ())); + } else { + contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitZ())); + } + depth = -distOutsideBox.z; + } + break; + + default: + debugAssertM(false, "Fell through switch"); + break; + } + + return depth; +} + + +float CollisionDetection::penetrationDepthForFixedSphereFixedSphere( + const Sphere& sphereA, + const Sphere& sphereB, + Array<Vector3>& contactPoints, + Array<Vector3>& contactNormals) { + + Vector3 axis = sphereB.center - sphereA.center; + double radius = sphereA.radius + sphereB.radius; + double mag = axis.magnitude(); + axis /= mag; + double depth = -(mag - radius); + + contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + + if (depth >= 0) { + contactPoints.append(sphereA.center + axis * (sphereA.radius - depth / 2)); + contactNormals.append(axis); + } + + return depth; +} + + +float CollisionDetection::penetrationDepthForFixedSphereFixedPlane( + const Sphere& sphereA, + const Plane& planeB, + Array<Vector3>& contactPoints, + Array<Vector3>& contactNormals) { + + Vector3 N; + double d; + + planeB.getEquation(N, d); + + double depth = -(sphereA.center.dot(N) + d - sphereA.radius); + + contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + + if (depth >= 0) { + contactPoints.append(N * (depth - sphereA.radius) + sphereA.center); + contactNormals.append(N); + } + + return depth; +} + + +float CollisionDetection::penetrationDepthForFixedBoxFixedPlane( + const Box& box, + const Plane& plane, + Array<Vector3>& contactPoints, + Array<Vector3>& contactNormals) { + + Vector3 N; + double d; + + plane.getEquation(N, d); + + contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY); + + float lowest = finf(); + for (int i = 0; i < 8; ++i) { + const Vector3 vertex = box.corner(i); + + float x = vertex.dot(N) + (float)d; + + if (x <= 0) { + // All vertices below the plane should be contact points. + contactPoints.append(vertex); + contactNormals.append(-N); + } + + lowest = min(lowest, x); + } + + // Depth should be a positive number + return -lowest; +} + + +float CollisionDetection::collisionTimeForMovingPointFixedPlane( + const Vector3& point, + const Vector3& velocity, + const Plane& plane, + Vector3& location, + Vector3& outNormal) { + + // Solve for the time at which normal.dot(point + velocity) + d == 0. + double d; + Vector3 normal; + plane.getEquation(normal, d); + + float vdotN = velocity.dot(normal); + float pdotN = point.dot(normal); + + if (fuzzyEq(pdotN + d, 0)) { + // The point is *in* the plane. + location = point; + outNormal = normal; + return 0; + } + + if (vdotN >= 0) { + // no collision will occur + location = Vector3::inf(); + return finf(); + } + + float t = -(pdotN + d) / vdotN; + if (t < 0) { + location = Vector3::inf(); + return finf(); + } else { + location = point + velocity * t; + outNormal = normal; + return t; + } +} + +bool __fastcall CollisionDetection::rayAABox( + const Ray& ray, + const Vector3& invDir, + const AABox& box, + const Vector3& boxCenter, + float boundingRadiusSquared, + Vector3& location, + bool& inside) { + + debugAssertM(fabs(ray.direction().squaredLength() - 1.0f) < 0.01f, format("Length = %f", ray.direction().length())); + { + // Pre-emptive partial bounding sphere test + const Vector3 L(boxCenter - ray.origin()); + float d = L.dot(ray.direction()); + + float L2 = L.dot(L); + float D2 = square(d); + float M2 = L2 - D2; + + if (((d < 0) && (L2 > boundingRadiusSquared)) || (M2 > boundingRadiusSquared)) { + inside = false; + return false; + } + // Passing here does not mean that the ray hits the bounding sphere; + // we would still have to perform more expensive tests to determine + // that. + } + + inside = true; + const Vector3& MinB = box.low(); + const Vector3& MaxB = box.high(); + Vector3 MaxT(-1.0f, -1.0f, -1.0f); + + // Find candidate planes. + for (int i = 0; i < 3; ++i) { + if (ray.origin()[i] < MinB[i]) { + location[i] = MinB[i]; + inside = false; + + // Calculate T distances to candidate planes + if (ray.direction()[i] != 0) { + MaxT[i] = (MinB[i] - ray.origin()[i]) * invDir[i]; + } + } else if (ray.origin()[i] > MaxB[i]) { + location[i] = MaxB[i]; + inside = false; + + // Calculate T distances to candidate planes + if (ray.direction()[i] != 0) { + MaxT[i] = (MaxB[i] - ray.origin()[i]) * invDir[i]; + } + } + } + + if (inside) { + // Ray origin inside bounding box + location = ray.origin(); + return true; + } + + // Get largest of the maxT's for final choice of intersection + int WhichPlane = 0; + if (MaxT[1] > MaxT[WhichPlane]) { + WhichPlane = 1; + } + + if (MaxT[2] > MaxT[WhichPlane]) { + WhichPlane = 2; + } + + // Check final candidate actually inside box + if (MaxT[WhichPlane] < 0.0f) { + // Miss the box + return false; + } + + for (int i = 0; i < 3; ++i) { + if (i != WhichPlane) { + location[i] = ray.origin()[i] + MaxT[WhichPlane] * ray.direction()[i]; + if ((location[i] < MinB[i]) || + (location[i] > MaxB[i])) { + // On this plane we're outside the box extents, so + // we miss the box + return false; + } + } + } + + return true; +} + +float CollisionDetection::collisionTimeForMovingPointFixedSphere( + const Vector3& point, + const Vector3& velocity, + const Sphere& sphere, + Vector3& location, + Vector3& outNormal, + bool solid) { + + if (solid && sphere.contains(point)) { + location = point; + outNormal = (point - sphere.center).direction(); + return 0.0f; + } + + float speed = velocity.magnitude(); + const Vector3& direction = velocity / speed; + + // length of the axis between the start and the sphere + const Vector3& L = sphere.center - point; + float d = L.dot(direction); + + float L2 = L.dot(L); + float R2 = square(sphere.radius); + float D2 = square(d); + + if ((d < 0.0f) && (L2 > R2)) { + location = Vector3::inf(); + return finf(); + } + + const float M2 = L2 - D2; + + if (M2 > R2) { + location = Vector3::inf(); + return finf(); + } + + float q = sqrt(R2 - M2); + float time; + + if (L2 > R2) { + time = d - q; + } else { + time = d + q; + } + + time /= speed; + + location = point + velocity * time; + outNormal = (location - sphere.center).direction(); + + return time; +} + + +float CollisionDetection::collisionTimeForMovingSphereFixedSphere( + const Sphere& movingSphere, + const Vector3& velocity, + const Sphere& fixedSphere, + Vector3& location, + Vector3& outNormal) { + + const Vector3& sep = (fixedSphere.center - movingSphere.center); + float sepLen = sep.squaredLength(); + if (sepLen < square(movingSphere.radius + fixedSphere.radius)) { + // Interpenetrating + outNormal = sep.directionOrZero(); + location = fixedSphere.center - outNormal * fixedSphere.radius; + return 0; + } + + float time = collisionTimeForMovingPointFixedSphere + (movingSphere.center, velocity, + Sphere(fixedSphere.center, fixedSphere.radius + movingSphere.radius), + location, outNormal); + + if (time < finf()) { + // Location is now the center of the moving sphere at the collision time. + // Adjust for the size of the moving sphere. Two spheres always collide + // along a line between their centers. + location += (location - fixedSphere.center) * movingSphere.radius / fixedSphere.radius; + } + + return time; +} + + +/* +float CollisionDetection::collisionTimeForMovingPointFixedTriangle( + const Vector3& point, + const Vector3& velocity, + const Triangle& triangle, + Vector3& outLocation, + Vector3& outNormal) { + + double time = collisionTimeForMovingPointFixedPlane(point, velocity, triangle.plane(), outLocation, outNormal); + + if (time == finf()) { + // No collision with the plane of the triangle. + return finf(); + } + + if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), outLocation, triangle.primaryAxis())) { + // Collision occured inside the triangle + return time; + } else { + // Missed the triangle + outLocation = Vector3::inf(); + return finf(); + } +}*/ + +/* +float CollisionDetection::collisionTimeForMovingPointFixedTriangle( + const Vector3& orig, + const Vector3& dir, + const Vector3& vert0, + const Vector3& vert1, + const Vector3& vert2) { + + // Barycenteric coords + double u, v; + #define EPSILON 0.000001 + #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]; + + double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3]; + + // 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 + const double det = DOT(edge1, pvec); + + if (det < EPSILON) { + return finf(); + } + + // calculate distance from vert0 to ray origin + SUB(tvec, orig, vert0); + + // calculate U parameter and test bounds + u = DOT(tvec, pvec); + if ((u < 0.0) || (u > det)) { + // Hit the plane outside the triangle + return finf(); + } + + // prepare to test V parameter + CROSS(qvec, tvec, edge1); + + // calculate V parameter and test bounds + v = DOT(dir, qvec); + if ((v < 0.0) || (u + v > det)) { + // Hit the plane outside the triangle + return finf(); + } + + // calculate t, scale parameters, ray intersects triangle + // If we want u,v, we can compute this + // double t = DOT(edge2, qvec); + //const double inv_det = 1.0 / det; + //t *= inv_det; + //u *= inv_det; + //v *= inv_det; + // return t; + + // Case where we don't need correct (u, v): + + const double t = DOT(edge2, qvec); + + if (t >= 0) { + // Note that det must be positive + return t / det; + } else { + // We had to travel backwards in time to intersect + return finf(); + } + + #undef EPSILON + #undef CROSS + #undef DOT + #undef SUB +} +*/ + +float CollisionDetection::collisionTimeForMovingPointFixedBox( + const Vector3& point, + const Vector3& velocity, + const Box& box, + Vector3& location, + Vector3& outNormal) { + + double bestTime; + + Vector3 normal; + Vector3 v[4]; + + // Prime the loop + int f = 0; + box.getFaceCorners(f, v[0], v[1], v[2], v[3]); + bestTime = collisionTimeForMovingPointFixedRectangle(point, velocity, v[0], v[1], v[2], v[3], location, normal); + outNormal = normal; + + // Check other faces + for (f = 1; f < 6; ++f) { + Vector3 pos; + box.getFaceCorners(f, v[0], v[1], v[2], v[3]); + float time = collisionTimeForMovingPointFixedRectangle(point, velocity, v[0], v[1], v[2], v[3], pos, normal); + if (time < bestTime) { + bestTime = time; + outNormal = normal; + location = pos; + } + } + + return bestTime; +} + + +float CollisionDetection::collisionTimeForMovingPointFixedAABox( + const Vector3& origin, + const Vector3& dir, + const AABox& box, + Vector3& location, + bool& Inside, + Vector3& normal) { + + if (collisionLocationForMovingPointFixedAABox(origin, dir, box, location, Inside, normal)) { + return (location - origin).magnitude(); + } else { + return (float)finf(); + } +} + + +bool CollisionDetection::collisionLocationForMovingPointFixedAABox( + const Vector3& origin, + const Vector3& dir, + const AABox& box, + Vector3& location, + bool& Inside, + Vector3& normal) { + + // Integer representation of a floating-point value. + #define IR(x) ((uint32&)x) + + Inside = true; + const Vector3& MinB = box.low(); + const Vector3& MaxB = box.high(); + Vector3 MaxT(-1.0f, -1.0f, -1.0f); + + // Find candidate planes. + for (int i = 0; i < 3; ++i) { + if (origin[i] < MinB[i]) { + location[i] = MinB[i]; + Inside = false; + + // Calculate T distances to candidate planes + if (IR(dir[i])) { + MaxT[i] = (MinB[i] - origin[i]) / dir[i]; + } + } else if (origin[i] > MaxB[i]) { + location[i] = MaxB[i]; + Inside = false; + + // Calculate T distances to candidate planes + if (IR(dir[i])) { + MaxT[i] = (MaxB[i] - origin[i]) / dir[i]; + } + } + } + + if (Inside) { + // Ray origin inside bounding box + location = origin; + return false; + } + + // Get largest of the maxT's for final choice of intersection + int WhichPlane = 0; + if (MaxT[1] > MaxT[WhichPlane]) { + WhichPlane = 1; + } + + if (MaxT[2] > MaxT[WhichPlane]) { + WhichPlane = 2; + } + + // Check final candidate actually inside box + if (IR(MaxT[WhichPlane]) & 0x80000000) { + // Miss the box + return false; + } + + for (int i = 0; i < 3; ++i) { + if (i != WhichPlane) { + location[i] = origin[i] + MaxT[WhichPlane] * dir[i]; + if ((location[i] < MinB[i]) || + (location[i] > MaxB[i])) { + // On this plane we're outside the box extents, so + // we miss the box + return false; + } + } + } + + // Choose the normal to be the plane normal facing into the ray + normal = Vector3::zero(); + normal[WhichPlane] = (dir[WhichPlane] > 0) ? -1.0 : 1.0; + + return true; + + #undef IR +} + + + +float CollisionDetection::collisionTimeForMovingPointFixedRectangle( + const Vector3& point, + const Vector3& velocity, + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& v3, + Vector3& location, + Vector3& outNormal) { + + Plane plane = Plane(v0, v1, v2); + + float time = collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal); + + if (time == finf()) { + // No collision is ever going to happen + return time; + } + + if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), location)) { + // The intersection point is inside the rectangle; that is the location where + // the point hits the rectangle. + return time; + } else { + return finf(); + } +} + +/** Used by findRayCapsuleIntersection. + @cite From magic software http://www.magic-software.com/Source/Intersection3D/MgcIntr3DLinCap.cpp */ +static int findRayCapsuleIntersectionAux( + const Vector3& rkOrigin, + const Vector3& rkDirection, + const Capsule& rkCapsule, + double afT[2]) { + + Vector3 capsuleDirection = rkCapsule.point(1) - rkCapsule.point(0); + + // set up quadratic Q(t) = a*t^2 + 2*b*t + c + Vector3 kU, kV, kW = capsuleDirection; + float fWLength = kW.unitize(); + Vector3::generateOrthonormalBasis(kU, kV, kW); + Vector3 kD(kU.dot(rkDirection), kV.dot(rkDirection), kW.dot(rkDirection)); + float fDLength = kD.unitize(); + + float fEpsilon = 1e-6f; + + float fInvDLength = 1.0f/fDLength; + Vector3 kDiff = rkOrigin - rkCapsule.point(0); + Vector3 kP(kU.dot(kDiff),kV.dot(kDiff),kW.dot(kDiff)); + float fRadiusSqr = square(rkCapsule.radius()); + + float fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp; + + // Is the velocity parallel to the capsule direction? (or zero) + if ((abs(kD.z) >= 1.0f - fEpsilon) || (fDLength < fEpsilon)) { + + float fAxisDir = rkDirection.dot(capsuleDirection); + + fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y; + if ((fAxisDir < 0) && (fDiscr >= 0.0f)) { + // Velocity anti-parallel to the capsule direction + fRoot = sqrt(fDiscr); + afT[0] = (kP.z + fRoot)*fInvDLength; + afT[1] = -(fWLength - kP.z + fRoot)*fInvDLength; + return 2; + } else if ((fAxisDir > 0) && (fDiscr >= 0.0f)) { + // Velocity parallel to the capsule direction + fRoot = sqrt(fDiscr); + afT[0] = -(kP.z + fRoot)*fInvDLength; + afT[1] = (fWLength - kP.z + fRoot)*fInvDLength; + return 2; + } else { + // sphere heading wrong direction, or no velocity at all + return 0; + } + } + + // test intersection with infinite cylinder + fA = kD.x*kD.x + kD.y*kD.y; + fB = kP.x*kD.x + kP.y*kD.y; + fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr; + fDiscr = fB*fB - fA*fC; + if (fDiscr < 0.0f) { + // line does not intersect infinite cylinder + return 0; + } + + int iQuantity = 0; + + if (fDiscr > 0.0f) { + // line intersects infinite cylinder in two places + fRoot = sqrt(fDiscr); + fInv = 1.0f/fA; + fT = (-fB - fRoot)*fInv; + fTmp = kP.z + fT*kD.z; + if ((0.0f <= fTmp) && (fTmp <= fWLength)) { + afT[iQuantity] = fT * fInvDLength; + iQuantity++; + } + + fT = (-fB + fRoot)*fInv; + fTmp = kP.z + fT*kD.z; + + if ((0.0f <= fTmp) && (fTmp <= fWLength)) { + afT[iQuantity++] = fT*fInvDLength; + } + + if (iQuantity == 2) { + // line intersects capsule wall in two places + return 2; + } + } else { + // line is tangent to infinite cylinder + fT = -fB/fA; + fTmp = kP.z + fT*kD.z; + if ((0.0f <= fTmp) && (fTmp <= fWLength)) { + afT[0] = fT*fInvDLength; + return 1; + } + } + + // test intersection with bottom hemisphere + // fA = 1 + fB += kP.z*kD.z; + fC += kP.z*kP.z; + fDiscr = fB*fB - fC; + if (fDiscr > 0.0f) { + fRoot = sqrt(fDiscr); + fT = -fB - fRoot; + fTmp = kP.z + fT*kD.z; + if (fTmp <= 0.0f) { + afT[iQuantity++] = fT*fInvDLength; + if (iQuantity == 2) { + return 2; + } + } + + fT = -fB + fRoot; + fTmp = kP.z + fT*kD.z; + if (fTmp <= 0.0f) { + afT[iQuantity++] = fT*fInvDLength; + if (iQuantity == 2) { + return 2; + } + } + } else if (fDiscr == 0.0f) { + fT = -fB; + fTmp = kP.z + fT*kD.z; + if (fTmp <= 0.0f) { + afT[iQuantity++] = fT*fInvDLength; + if (iQuantity == 2) { + return 2; + } + } + } + + // test intersection with top hemisphere + // fA = 1 + fB -= kD.z*fWLength; + fC += fWLength*(fWLength - 2.0f*kP.z); + + fDiscr = fB*fB - fC; + if (fDiscr > 0.0f) { + fRoot = sqrt(fDiscr); + fT = -fB - fRoot; + fTmp = kP.z + fT*kD.z; + if (fTmp >= fWLength) { + afT[iQuantity++] = fT*fInvDLength; + if (iQuantity == 2) { + return 2; + } + } + + fT = -fB + fRoot; + fTmp = kP.z + fT*kD.z; + if (fTmp >= fWLength) { + afT[iQuantity++] = fT*fInvDLength; + if (iQuantity == 2) { + return 2; + } + } + } else if (fDiscr == 0.0f) { + fT = -fB; + fTmp = kP.z + fT*kD.z; + if (fTmp >= fWLength) { + afT[iQuantity++] = fT*fInvDLength; + if (iQuantity == 2) { + return 2; + } + } + } + + return iQuantity; +} + + +/** Used by collisionTimeForMovingPointFixedCapsule. + @cite From magic software http://www.magic-software.com/Source/Intersection3D/MgcIntr3DLinCap.cpp + + @param rkRay The ray + @param rkCapsule The capsule + @param riQuantity The number of intersections found + @param akPoint The intersections found + @return True if there is at least one intersection + */ +static bool findRayCapsuleIntersection( + const Ray& rkRay, + const Capsule& rkCapsule, + int& riQuantity, + Vector3 akPoint[2]) { + + double afT[2]; + riQuantity = findRayCapsuleIntersectionAux(rkRay.origin(), rkRay.direction(), rkCapsule, afT); + + // Only return intersections that occur in the future + int iClipQuantity = 0; + int i; + for (i = 0; i < riQuantity; ++i) { + if (afT[i] >= 0.0f) { + akPoint[iClipQuantity] = rkRay.origin() + afT[i] * rkRay.direction(); + ++iClipQuantity; + } + } + + riQuantity = iClipQuantity; + return (riQuantity > 0); +} + +float CollisionDetection::collisionTimeForMovingPointFixedCapsule( + const Vector3& _point, + const Vector3& velocity, + const Capsule& capsule, + Vector3& location, + Vector3& outNormal) { + + float timeScale = velocity.magnitude(); + + if (timeScale == 0.0f) { + timeScale = 1; + } + + Vector3 direction = velocity / timeScale; + int numIntersections; + Vector3 intersection[2]; + findRayCapsuleIntersection(Ray::fromOriginAndDirection(_point, direction), capsule, numIntersections, intersection); + + if (numIntersections == 2) { + // A collision can only occur if there are two intersections. If there is one + // intersection, that one is exiting the capsule. + + // Find the entering intersection (the first one that occurs). + float d0 = (intersection[0] - _point).squaredMagnitude(); + float d1 = (intersection[1] - _point).squaredMagnitude(); + + // Compute the surface normal (if we aren't ignoring the result) + if (&outNormal != &ignore) { + Vector3 p2 = LineSegment::fromTwoPoints(capsule.point(0), capsule.point(1)).closestPoint(_point); + outNormal = (_point - p2).direction(); + } + + if (d0 > d1) { + location = intersection[1]; + return sqrt(d1) / timeScale; + } else { + location = intersection[0]; + return sqrt(d0) / timeScale; + } + } else { + // No entering intersection discovered; return no intersection. + location = Vector3::inf(); + return finf(); + } +} + + +float CollisionDetection::collisionTimeForMovingSphereFixedPlane( + const Sphere& sphere, + const Vector3& velocity, + const Plane& plane, + Vector3& location, + Vector3& outNormal) { + + if (sphere.radius == 0) { + // Optimization for zero radius sphere + return collisionTimeForMovingPointFixedPlane(sphere.center, velocity, plane, location, outNormal); + } + + // The collision point on the sphere will be the point at + // center - (radius * normal). Collisions only occur when + // the sphere is travelling into the plane. + + double d; + plane.getEquation(outNormal, d); + + double vdotN = velocity.dot(outNormal); + + if (fuzzyGt(vdotN, 0)) { + // No collision when the sphere is moving towards a backface. + location = Vector3::inf(); + return (float)finf(); + } + + float cdotN = sphere.center.dot(outNormal); + + // Distance from the center to the plane + float distance = cdotN + (float)d; + + // Where is the collision on the sphere? + Vector3 point = sphere.center - (sphere.radius * outNormal); + + if (fuzzyLe(G3D::abs(distance), sphere.radius)) { + // Already interpenetrating + location = sphere.center - distance * outNormal; + return 0; + } else { + return collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal); + } + +} + + +float CollisionDetection::collisionTimeForMovingSphereFixedTriangle( + const class Sphere& sphere, + const Vector3& velocity, + const Triangle& triangle, + Vector3& outLocation, + float b[3]) { + + Vector3 dummy; + float time = collisionTimeForMovingSphereFixedPlane(sphere, velocity, triangle.plane(), + outLocation, dummy); + + if (time == finf()) { + // No collision is ever going to happen + return time; + } + + // We will hit the plane of the triangle at *time*. See if + // the intersection point actually is within the triangle. + + if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), + outLocation, b, triangle.primaryAxis())) { + + // The intersection point is inside the triangle; that is the location where + // the sphere hits the triangle. + +# ifdef G3D_DEBUG + { + // Internal consistency checks + debugAssertM(b[0] >= 0.0 && b[0] <= 1.0f, "Intersection is outside triangle."); + debugAssertM(b[1] >= 0.0 && b[1] <= 1.0f, "Intersection is outside triangle."); + debugAssertM(b[2] >= 0.0 && b[2] <= 1.0f, "Intersection is outside triangle."); + Vector3 blend = + b[0] * triangle.vertex(0) + + b[1] * triangle.vertex(1) + + b[2] * triangle.vertex(2); + debugAssertM(blend.fuzzyEq(outLocation), "Barycentric coords don't match intersection."); + // Call again so that we can debug the problem + // isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), + // outLocation, b, triangle.primaryAxis()); + } +# endif + + return time; + } + + // The collision (if it exists) is with a point on the triangle perimeter. + // Switch over to moving the triangle towards a fixed sphere and see at what time + // they will hit. + + // Closest point on the triangle to the sphere intersection with the plane. + int edgeIndex; + const Vector3& point = closestPointOnTrianglePerimeter(triangle._vertex, triangle.edgeDirection, + triangle.edgeMagnitude, outLocation, edgeIndex); + + float t = 0; + if (! sphere.contains(point)) { + // The point is outside the sphere--see when it will hit + t = collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, dummy, dummy); + } + + if (t < finf()) { + outLocation = point; + // Compute Barycentric coords + + // Index of the next vertex + static const int next[] = {1, 2, 0}; + + // Project along the edge in question. + // Avoid sqrt by taking advantage of the existing edgeDirection unit vector. + b[next[edgeIndex]] = (outLocation - triangle._vertex[edgeIndex]).dot + (triangle.edgeDirection[edgeIndex]) / triangle.edgeMagnitude[edgeIndex]; + + b[edgeIndex] = 1.0f - b[next[edgeIndex]]; + + b[next[next[edgeIndex]]] = 0.0f; + +# ifdef G3D_DEBUG + { + // Internal consistency checks + for (int i = 0; i < 3; ++i) { + debugAssertM(fuzzyGe(b[i], 0.0f) && fuzzyLe(b[i], 1.0f), "Intersection is outside triangle."); + } + Vector3 blend = + b[0] * triangle.vertex(0) + + b[1] * triangle.vertex(1) + + b[2] * triangle.vertex(2); + debugAssertM(blend.fuzzyEq(outLocation), + format("Barycentric coords don't match intersection. %s != %s", + blend.toString().c_str(), + outLocation.toString().c_str())); + + // Call again so that we can debug the problem + collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, dummy, dummy); + } +# endif + + // Due to tiny roundoffs, these values might be slightly out of bounds. + // Ensure that they are legal. Note that the above debugging code + // verifies that we are not clamping truly illegal values. + for (int i = 0; i < 3; ++i) { + b[i] = clamp(b[i], 0.0f, 1.0f); + } + } + + // The collision occured at the point, if it occured. The normal + // was the plane normal, computed above. + + return t; +} + + +float CollisionDetection::collisionTimeForMovingSphereFixedRectangle( + const Sphere& sphere, + const Vector3& velocity, + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& v3, + Vector3& location, + Vector3& outNormal) { + + Plane plane(v0, v1, v2); + + float time = collisionTimeForMovingSphereFixedPlane(sphere, velocity, plane, location, outNormal); + + if (time == finf()) { + // No collision is ever going to happen + return time; + } + + if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), location)) { + // The intersection point is inside the rectangle; that is the location where + // the sphere hits the rectangle. + return time; + } + + // Switch over to moving the rectangle towards a fixed sphere and see at what time + // they will hit. + + Vector3 point = closestPointToRectanglePerimeter(v0, v1, v2, v3, sphere.center); + + Vector3 dummy; + double t = collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, location, dummy); + + // Normal is the plane normal, location is the original location of the point. + location = point; + + return t; +} + + +float CollisionDetection::collisionTimeForMovingSphereFixedBox( + const Sphere& sphere, + const Vector3& velocity, + const Box& box, + Vector3& location, + Vector3& outNormal) { + + if (fixedSolidSphereIntersectsFixedSolidBox(sphere, box)) { + // TODO: Compute more useful location and normal? + location = sphere.center; + outNormal = Vector3::zero(); + return 0; + } + + float bestTime; + + Vector3 v[4]; + int f = 0; + box.getFaceCorners(f, v[0], v[1], v[2], v[3]); + bestTime = collisionTimeForMovingSphereFixedRectangle(sphere, velocity, v[0], v[1], v[2], v[3], location, outNormal); + + for (f = 1; f < 6; ++f) { + Vector3 pos, normal; + box.getFaceCorners(f, v[0], v[1], v[2], v[3]); + float time = collisionTimeForMovingSphereFixedRectangle(sphere, velocity, v[0], v[1], v[2], v[3], pos, normal); + if (time < bestTime) { + bestTime = time; + location = pos; + outNormal = normal; + } + } + + return bestTime; +} + + +float CollisionDetection::collisionTimeForMovingSphereFixedCapsule( + const Sphere& sphere, + const Vector3& velocity, + const Capsule& capsule, + Vector3& location, + Vector3& outNormal) { + + (void)outNormal; + + Capsule _capsule(capsule.point(0), capsule.point(1), capsule.radius() + sphere.radius); + + Vector3 normal; + double time = collisionTimeForMovingPointFixedCapsule(sphere.center, velocity, _capsule, location, normal); + + if (time < finf()) { + // Location is now the position of the center of the sphere at the time of collision. + // We have to adjust the collision location for the size of the sphere. + location -= sphere.radius * normal; + } + + return time; +} + + +Vector3 CollisionDetection::bounceDirection( + const Sphere& sphere, + const Vector3& velocity, + const float collisionTime, + const Vector3& collisionLocation, + const Vector3& collisionNormal) { + + // Location when the collision occurs + Vector3 sphereLocation = sphere.center + velocity * collisionTime; + + Vector3 normal = (sphereLocation - collisionLocation); + if (fuzzyEq(normal.squaredMagnitude(), 0)) { + normal = collisionNormal; + } else { + normal.unitize(); + } + + Vector3 direction = velocity.direction(); + + // Reflect direction about the normal + return direction - 2.0 * normal * normal.dot(direction); +} + + +Vector3 CollisionDetection::slideDirection( + const Sphere& sphere, + const Vector3& velocity, + const float collisionTime, + const Vector3& collisionLocation) { + + Vector3 sphereLocation = sphere.center + velocity * collisionTime; + Vector3 normal = (sphereLocation - collisionLocation).direction(); + Vector3 direction = velocity.direction(); + + // subtract off the part in the direction away from the normal. + return direction - normal * normal.dot(direction); +} + + +Vector3 CollisionDetection::closestPointOnLineSegment( + const Vector3& v0, + const Vector3& v1, + const Vector3& point) { + + const Vector3& edge = (v1 - v0); + float edgeLength = edge.magnitude(); + + if (edgeLength == 0) { + // The line segment is a point + return v0; + } + + return closestPointOnLineSegment(v0, v1, edge / edgeLength, edgeLength, point); +} + + +Vector3 CollisionDetection::closestPointOnLineSegment( + const Vector3& v0, + const Vector3& v1, + const Vector3& edgeDirection, + const float edgeLength, + const Vector3& point) { + + debugAssert((v1 - v0).direction().fuzzyEq(edgeDirection)); + debugAssert(fuzzyEq((v1 - v0).magnitude(), edgeLength)); + + // Vector towards the point + const Vector3& c = point - v0; + + // Projected onto the edge itself + float t = edgeDirection.dot(c); + + if (t <= 0) { + // Before the start + return v0; + } else if (t >= edgeLength) { + // After the end + return v1; + } else { + // At distance t along the edge + return v0 + edgeDirection * t; + } +} + + +Vector3 CollisionDetection::closestPointOnTrianglePerimeter( + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& point) { + + Vector3 v[3] = {v0, v1, v2}; + Vector3 edgeDirection[3] = {(v1 - v0), (v2 - v1), (v0 - v2)}; + float edgeLength[3]; + + for (int i = 0; i < 3; ++i) { + edgeLength[i] = edgeDirection[i].magnitude(); + edgeDirection[i] /= edgeLength[i]; + } + + int edgeIndex; + return closestPointOnTrianglePerimeter(v, edgeDirection, edgeLength, point, edgeIndex); +} + + +Vector3 CollisionDetection::closestPointOnTrianglePerimeter( + const Vector3 v[3], + const Vector3 edgeDirection[3], + const float edgeLength[3], + const Vector3& point, + int& edgeIndex) { + + // Closest point on segment from v[i] to v[i + 1] + Vector3 r[3]; + + // Distance squared from r[i] to point + float d[3]; + + // Index of the next point + static const int next[] = {1, 2, 0}; + + for (int i = 0; i < 3; ++i) { + r[i] = closestPointOnLineSegment(v[i], v[next[i]], edgeDirection[i], edgeLength[i], point); + d[i] = (r[i] - point).squaredMagnitude(); + } + + if (d[0] < d[1]) { + if (d[0] < d[2]) { + // Between v0 and v1 + edgeIndex = 0; + } else { + // Between v2 and v0 + edgeIndex = 2; + } + } else { + if (d[1] < d[2]) { + // Between v1 and v2 + edgeIndex = 1; + } else { + // Between v2 and v0 + edgeIndex = 2; + } + } + +# ifdef G3D_DEBUG + { + Vector3 diff = r[edgeIndex] - v[edgeIndex]; + debugAssertM(fuzzyEq(diff.direction().dot(edgeDirection[edgeIndex]), 1.0f) || + diff.fuzzyEq(Vector3::zero()), "Point not on correct triangle edge"); + float frac = diff.dot(edgeDirection[edgeIndex])/edgeLength[edgeIndex]; + debugAssertM(frac >= -0.000001, "Point off low side of edge."); + debugAssertM(frac <= 1.000001, "Point off high side of edge."); + } +# endif + + return r[edgeIndex]; +} + + +bool CollisionDetection::isPointInsideTriangle( + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& normal, + const Vector3& point, + float b[3], + Vector3::Axis primaryAxis) { + + if (primaryAxis == Vector3::DETECT_AXIS) { + primaryAxis = normal.primaryAxis(); + } + + // Check that the point is within the triangle using a Barycentric + // coordinate test on a two dimensional plane. + int i, j; + + switch (primaryAxis) { + case Vector3::X_AXIS: + i = Vector3::Y_AXIS; + j = Vector3::Z_AXIS; + break; + + case Vector3::Y_AXIS: + i = Vector3::Z_AXIS; + j = Vector3::X_AXIS; + break; + + case Vector3::Z_AXIS: + i = Vector3::X_AXIS; + j = Vector3::Y_AXIS; + break; + + default: + // This case is here to supress a warning on Linux + i = j = 0; + debugAssertM(false, "Should not get here."); + break; + } + + // See if all barycentric coordinates are non-negative + + // 2D area via cross product +# define AREA2(d, e, f) (((e)[i] - (d)[i]) * ((f)[j] - (d)[j]) - ((f)[i] - (d)[i]) * ((e)[j] - (d)[j])) + + // Area of the polygon + float area = AREA2(v0, v1, v2); + if (area == 0) { + // This triangle has zero area, so the point must not + // be in it unless the triangle point is the test point. + return (v0 == point); + } + + debugAssert(area != 0); + + float invArea = 1.0f / area; + + // (avoid normalization until absolutely necessary) + b[0] = AREA2(point, v1, v2) * invArea; + + if ((b[0] < 0.0f) || (b[0] > 1.0f)) { + return false; + } + + b[1] = AREA2(v0, point, v2) * invArea; + if ((b[1] < 0.0f) || (b[1] > 1.0f)) { + return false; + } + + b[2] = 1.0f - b[0] - b[1]; + +# undef AREA2 + + return (b[2] >= 0.0f) && (b[2] <= 1.0f); +} + + +bool CollisionDetection::isPointInsideRectangle( + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& v3, + const Vector3& normal, + const Vector3& point) { + + return isPointInsideTriangle(v0, v1, v2, normal, point) || + isPointInsideTriangle(v2, v3, v0, normal, point); +} + + +Vector3 CollisionDetection::closestPointToRectanglePerimeter( + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& v3, + const Vector3& point) { + + Vector3 r0 = closestPointOnLineSegment(v0, v1, point); + Vector3 r1 = closestPointOnLineSegment(v1, v2, point); + Vector3 r2 = closestPointOnLineSegment(v2, v3, point); + Vector3 r3 = closestPointOnLineSegment(v3, v0, point); + + double d0 = (r0 - point).squaredMagnitude(); + double d1 = (r1 - point).squaredMagnitude(); + double d2 = (r2 - point).squaredMagnitude(); + double d3 = (r3 - point).squaredMagnitude(); + + if (d0 < d1) { + if (d0 < d2) { + if (d0 < d3) { + return r0; + } else { + return r3; + } + } else { + if (d2 < d3) { + return r2; + } else { + return r3; + } + } + } else { + if (d1 < d2) { + if (d1 < d3) { + return r1; + } else { + return r3; + } + } else { + if (d2 < d3) { + return r2; + } else { + return r3; + } + } + } +} + + +Vector3 CollisionDetection::closestPointToRectangle( + const Vector3& v0, + const Vector3& v1, + const Vector3& v2, + const Vector3& v3, + const Vector3& point) { + + Plane plane(v0, v1, v2); + + // Project the point into the plane + double a, b, c, d; + plane.getEquation(a, b, c, d); + + double distance = a*point.x + b*point.y + c*point.z + d; + Vector3 planePoint = point - distance * plane.normal(); + + if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), planePoint)) { + return planePoint; + } else { + return closestPointToRectanglePerimeter(v0, v1, v2, v3, planePoint); + } +} + + +bool CollisionDetection::fixedSolidSphereIntersectsFixedSolidSphere( + const Sphere& sphere1, + const Sphere& sphere2) { + + return (sphere1.center - sphere2.center).squaredMagnitude() < square(sphere1.radius + sphere2.radius); +} + + +bool CollisionDetection::fixedSolidSphereIntersectsFixedSolidBox( + const Sphere& sphere, + const Box& box) { + + // If the center of the sphere is within the box, the whole + // sphere is within the box. + if (box.contains(sphere.center)) { + return true; + } + + float r2 = square(sphere.radius); + + // Find the closest point on the surface of the box to the sphere. If + // this point is within the sphere's radius, they intersect. + int f; + for (f = 0; f < 6; ++f) { + Vector3 v0, v1, v2, v3; + box.getFaceCorners(f, v0, v1, v2, v3); + if ((closestPointToRectangle(v0, v1, v2, v3, sphere.center) - sphere.center).squaredMagnitude() <= r2) { + return true; + } + } + + return false; +} + + +bool CollisionDetection::movingSpherePassesThroughFixedBox( + const Sphere& sphere, + const Vector3& velocity, + const Box& box, + double timeLimit) { + + // If they intersect originally, they definitely pass through each other. + if (fixedSolidSphereIntersectsFixedSolidBox(sphere, box)) { + return true; + } + + // See if the sphere hits the box during the time period. + Vector3 dummy1, dummy2; + + return (collisionTimeForMovingSphereFixedBox(sphere, velocity, box, dummy1, dummy2) < timeLimit); +} + + +bool CollisionDetection::movingSpherePassesThroughFixedSphere( + const Sphere& sphere, + const Vector3& velocity, + const Sphere& fixedSphere, + double timeLimit) { + + if (fixedSolidSphereIntersectsFixedSolidSphere(sphere, fixedSphere)) { + return true; + } + + // Extend the fixed sphere by the radius of the moving sphere + Sphere bigFixed(fixedSphere.center, fixedSphere.radius + sphere.radius); + Vector3 dummy1, dummy2; + + // If the sphere collides with the other sphere during the time limit, it passes through + return (collisionTimeForMovingPointFixedSphere(sphere.center, velocity, bigFixed, dummy1, dummy2) < timeLimit); +} + + + +bool CollisionDetection::fixedSolidSphereIntersectsFixedTriangle( + const Sphere& sphere, + const Triangle& triangle) { + + // How far is the sphere from the plane of the triangle + const Plane& plane = triangle.plane(); + + // Does the closest point to the sphere center lie within the triangle? + Vector3 v = plane.closestPoint(sphere.center); + + // Is the closest point to the plane within the sphere? + if ((v - sphere.center).squaredLength() <= square(sphere.radius)) { + // Is it also within the triangle? + float b[3]; + if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), + v, b, triangle.primaryAxis())){ + // The closest point is inside the triangle + return true; + } + } + + // ignored + int edgeIndex; + + v = closestPointOnTrianglePerimeter(triangle._vertex, triangle.edgeDirection, triangle.edgeMagnitude, sphere.center, edgeIndex); + + // Is the closest point within the sphere? + return ((v - sphere.center).squaredLength() <= square(sphere.radius)); +} + + +//////////////////////////////////////////////////////////////////////////////// +// AABB-triangle overlap test code based on Tomas Akenine-Möller's +// http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/tribox3.txt +// Ported 2008-12-28 + +#define X 0 +#define Y 1 +#define Z 2 + +#define FINDMINMAX(x0, x1, x2, min, max) \ + min = max = x0; \ + if(x1<min) min=x1;\ + if(x1>max) max=x1;\ + if(x2<min) min=x2;\ + if(x2>max) max=x2; + +static bool planeBoxOverlap(const Vector3& normal, const Vector3& vert, const Vector3& maxbox) { + Vector3 vmin, vmax; + float v; + + // for each axis + for(int a = 0; a < 3; ++a) { + v = vert[a]; + + if (normal[a] > 0.0f) { + vmin[a] = -maxbox[a] - v; + vmax[a] = maxbox[a] - v; + } else { + vmin[a] = maxbox[a] - v; + vmax[a] = -maxbox[a] - v; + } + } + + if (normal.dot(vmin) > 0.0f) { + return false; + } else if (normal.dot(vmax) >= 0.0f) { + return true; + } else { + return false; + } +} + +/*======================== X-tests ========================*/ + +#define AXISTEST_X01(a, b, fa, fb) \ + p0 = a*v0[Y] - b*v0[Z]; \ + p2 = a*v2[Y] - b*v2[Z]; \ + if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \ + rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \ + if(min>rad || max<-rad) return false; + + +#define AXISTEST_X2(a, b, fa, fb) \ + p0 = a*v0[Y] - b*v0[Z]; \ + p1 = a*v1[Y] - b*v1[Z]; \ + if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \ + rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \ + if(min>rad || max<-rad) return false; + +/*======================== Y-tests ========================*/ + +#define AXISTEST_Y02(a, b, fa, fb) \ + p0 = -a*v0[X] + b*v0[Z]; \ + p2 = -a*v2[X] + b*v2[Z]; \ + if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \ + if(min>rad || max<-rad) return false; + +#define AXISTEST_Y1(a, b, fa, fb) \ + p0 = -a*v0[X] + b*v0[Z]; \ + p1 = -a*v1[X] + b*v1[Z]; \ + if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \ + if(min>rad || max<-rad) return false; + +/*======================== Z-tests ========================*/ + +#define AXISTEST_Z12(a, b, fa, fb) \ + p1 = a*v1[X] - b*v1[Y]; \ + p2 = a*v2[X] - b*v2[Y]; \ + if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \ + if(min>rad || max<-rad) return false; + +#define AXISTEST_Z0(a, b, fa, fb) \ + p0 = a*v0[X] - b*v0[Y]; \ + p1 = a*v1[X] - b*v1[Y]; \ + if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \ + rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \ + if(min>rad || max<-rad) return false; + +bool CollisionDetection::fixedSolidBoxIntersectsFixedTriangle( + const AABox& box, const Triangle& tri) { + + // 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}-direction) + // this gives 3x3=9 more tests + + // This is the fastest branch (on Sun). + // Move the triangle to the object space of the box + // Triangle vertices in box object space + + const Vector3& boxcenter = box.center(); + const Vector3& boxhalfsize = box.extent() * 0.5f; + + const Vector3& v0 = tri.vertex(0) - boxcenter; + const Vector3& v1 = tri.vertex(1) - boxcenter; + const Vector3& v2 = tri.vertex(2) - boxcenter; + + // Compute triangle edges in object space + const Vector3& e0 = v1 - v0; + const Vector3& e1 = v2 - v1; + const Vector3& e2 = v0 - v2; + + // Bullet 3: + // test the 9 tests first (this was faster) + float min,max,p0,p1,p2,rad; + Vector3 fe; + + fe = abs(e0); + AXISTEST_X01(e0[Z], e0[Y], fe[Z], fe[Y]); + AXISTEST_Y02(e0[Z], e0[X], fe[Z], fe[X]); + AXISTEST_Z12(e0[Y], e0[X], fe[Y], fe[X]); + + fe = abs(e1); + AXISTEST_X01(e1[Z], e1[Y], fe[Z], fe[Y]); + AXISTEST_Y02(e1[Z], e1[X], fe[Z], fe[X]); + AXISTEST_Z0 (e1[Y], e1[X], fe[Y], fe[X]); + + fe = abs(e2); + AXISTEST_X2 (e2[Z], e2[Y], fe[Z], fe[Y]); + AXISTEST_Y1 (e2[Z], e2[X], fe[Z], fe[X]); + AXISTEST_Z12(e2[Y], e2[X], fe[Y], fe[X]); + + // Bullet 1: + // first test overlap in the {x,y,z}-directions + // find min, max of the triangle each direction, and test for overlap in + // that direction -- this is equivalent to testing a minimal AABB around + // the triangle against the AABB + + // test in X-direction + FINDMINMAX(v0[X],v1[X],v2[X],min,max); + if (min > boxhalfsize[X] || max < -boxhalfsize[X]) { + return false; + } + + // test in Y-direction + FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max); + if (min > boxhalfsize[Y] || max < -boxhalfsize[Y]) { + return false; + } + + // test in Z-direction + FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max); + if (min > boxhalfsize[Z] || max < -boxhalfsize[Z]) { + return false; + } + + // Bullet 2: + // test if the box intersects the plane of the triangle + // compute plane equation of triangle: normal*x+d=0 + + if (! planeBoxOverlap(tri.normal(), v0, boxhalfsize)) { + return false; + } + + // box and triangle overlap + return true; +} +#undef X +#undef Y +#undef Z + +//////////////////////////////////////////////////////////////////////////////// + + +} // namespace + +#ifdef _MSC_VER +// Turn off fast floating-point optimizations +#pragma float_control( pop ) +#pragma warning (pop) +#endif diff --git a/externals/g3dlite/CoordinateFrame.cpp b/externals/g3dlite/CoordinateFrame.cpp new file mode 100644 index 00000000000..9b639b62082 --- /dev/null +++ b/externals/g3dlite/CoordinateFrame.cpp @@ -0,0 +1,436 @@ +/** + @file CoordinateFrame.cpp + + Coordinate frame class + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2001-06-02 + @edited 2009-11-13 + + Copyright 2000-2010, Morgan McGuire. + All rights reserved. +*/ + +#include "G3D/platform.h" +#include "G3D/CoordinateFrame.h" +#include "G3D/Quat.h" +#include "G3D/Matrix4.h" +#include "G3D/Box.h" +#include "G3D/AABox.h" +#include "G3D/Sphere.h" +#include "G3D/Triangle.h" +#include "G3D/Ray.h" +#include "G3D/Capsule.h" +#include "G3D/Cylinder.h" +#include "G3D/UprightFrame.h" +#include "G3D/Any.h" +#include "G3D/stringutils.h" + +namespace G3D { + +CoordinateFrame::CoordinateFrame(const Any& any) { + any.verifyName("CFrame"); + if (toUpper(any.name()) == "CFRAME") { + any.verifyType(Any::TABLE, Any::ARRAY); + if (any.type() == Any::TABLE) { + rotation = any["rotation"]; + translation = any["translation"]; + } else { + any.verifySize(2); + rotation = any[0]; + translation = any[1]; + } + } else { + any.verifyName("CFrame::fromXYZYPRDegrees"); + any.verifyType(Any::ARRAY); + any.verifySize(3, 6); + + int s = any.size(); + + *this = fromXYZYPRDegrees(any[0], any[1], any[2], + (s > 3) ? any[3].number() : 0.0f, + (s > 4) ? any[4].number() : 0.0f, + (s > 5) ? any[5].number() : 0.0f); + } +} + + +CoordinateFrame::operator Any() const { + float x, y, z, yaw, pitch, roll; + getXYZYPRDegrees(x, y, z, yaw, pitch, roll); + Any a(Any::ARRAY, "CFrame::fromXYZYPRDegrees"); + a.append(x, y, z, yaw); + if ( ! G3D::fuzzyEq(yaw, 0.0f) || ! G3D::fuzzyEq(pitch, 0.0f) || ! G3D::fuzzyEq(roll, 0.0f)) { + a.append(yaw); + if (! G3D::fuzzyEq(pitch, 0.0f) || ! G3D::fuzzyEq(roll, 0.0f)) { + a.append(pitch); + if (! G3D::fuzzyEq(roll, 0.0f)) { + a.append(roll); + } + } + } + return a; +} + + +CoordinateFrame::CoordinateFrame(const class UprightFrame& f) { + *this = f.toCoordinateFrame(); +} + + +CoordinateFrame::CoordinateFrame() : + rotation(Matrix3::identity()), translation(Vector3::zero()) { +} + +CoordinateFrame CoordinateFrame::fromXYZYPRRadians(float x, float y, float z, float yaw, + float pitch, float roll) { + Matrix3 rotation = Matrix3::fromAxisAngle(Vector3::unitY(), yaw); + + rotation = Matrix3::fromAxisAngle(rotation.column(0), pitch) * rotation; + rotation = Matrix3::fromAxisAngle(rotation.column(2), roll) * rotation; + + const Vector3 translation(x, y, z); + + return CoordinateFrame(rotation, translation); +} + + +void CoordinateFrame::getXYZYPRRadians(float& x, float& y, float& z, + float& yaw, float& pitch, float& roll) const { + x = translation.x; + y = translation.y; + z = translation.z; + + const Vector3& look = lookVector(); + + if (abs(look.y) > 0.99f) { + // Looking nearly straight up or down + + yaw = G3D::pi() + atan2(look.x, look.z); + pitch = asin(look.y); + roll = 0.0f; + + } else { + + // Yaw cannot be affected by others, so pull it first + yaw = G3D::pi() + atan2(look.x, look.z); + + // Pitch is the elevation of the yaw vector + pitch = asin(look.y); + + Vector3 actualRight = rightVector(); + Vector3 expectedRight = look.cross(Vector3::unitY()); + + roll = 0;//acos(actualRight.dot(expectedRight)); TODO + } +} + + +void CoordinateFrame::getXYZYPRDegrees(float& x, float& y, float& z, + float& yaw, float& pitch, float& roll) const { + getXYZYPRRadians(x, y, z, yaw, pitch, roll); + yaw = toDegrees(yaw); + pitch = toDegrees(pitch); + roll = toDegrees(roll); +} + + +CoordinateFrame CoordinateFrame::fromXYZYPRDegrees(float x, float y, float z, + float yaw, float pitch, float roll) { + return fromXYZYPRRadians(x, y, z, toRadians(yaw), toRadians(pitch), toRadians(roll)); +} + + +Ray CoordinateFrame::lookRay() const { + return Ray::fromOriginAndDirection(translation, lookVector()); +} + + +bool CoordinateFrame::fuzzyEq(const CoordinateFrame& other) const { + + for (int c = 0; c < 3; ++c) { + for (int r = 0; r < 3; ++r) { + if (! G3D::fuzzyEq(other.rotation[r][c], rotation[r][c])) { + return false; + } + } + if (! G3D::fuzzyEq(translation[c], other.translation[c])) { + return false; + } + } + + return true; +} + + +bool CoordinateFrame::fuzzyIsIdentity() const { + const Matrix3& I = Matrix3::identity(); + + for (int c = 0; c < 3; ++c) { + for (int r = 0; r < 3; ++r) { + if (fuzzyNe(I[r][c], rotation[r][c])) { + return false; + } + } + if (fuzzyNe(translation[c], 0)) { + return false; + } + } + + return true; +} + + +bool CoordinateFrame::isIdentity() const { + return + (translation == Vector3::zero()) && + (rotation == Matrix3::identity()); +} + + +Matrix4 CoordinateFrame::toMatrix4() const { + return Matrix4(*this); +} + + +std::string CoordinateFrame::toXML() const { + return G3D::format( + "<COORDINATEFRAME>\n %lf,%lf,%lf,%lf,\n %lf,%lf,%lf,%lf,\n %lf,%lf,%lf,%lf,\n %lf,%lf,%lf,%lf\n</COORDINATEFRAME>\n", + rotation[0][0], rotation[0][1], rotation[0][2], translation.x, + rotation[1][0], rotation[1][1], rotation[1][2], translation.y, + rotation[2][0], rotation[2][1], rotation[2][2], translation.z, + 0.0, 0.0, 0.0, 1.0); +} + + +Plane CoordinateFrame::toObjectSpace(const Plane& p) const { + Vector3 N, P; + double d; + p.getEquation(N, d); + P = N * (float)d; + P = pointToObjectSpace(P); + N = normalToObjectSpace(N); + return Plane(N, P); +} + + +Plane CoordinateFrame::toWorldSpace(const Plane& p) const { + Vector3 N, P; + double d; + p.getEquation(N, d); + P = N * (float)d; + P = pointToWorldSpace(P); + N = normalToWorldSpace(N); + return Plane(N, P); +} + + +Triangle CoordinateFrame::toObjectSpace(const Triangle& t) const { + return Triangle(pointToObjectSpace(t.vertex(0)), + pointToObjectSpace(t.vertex(1)), + pointToObjectSpace(t.vertex(2))); +} + + +Triangle CoordinateFrame::toWorldSpace(const Triangle& t) const { + return Triangle(pointToWorldSpace(t.vertex(0)), + pointToWorldSpace(t.vertex(1)), + pointToWorldSpace(t.vertex(2))); +} + + +Cylinder CoordinateFrame::toWorldSpace(const Cylinder& c) const { + return Cylinder( + pointToWorldSpace(c.point(0)), + pointToWorldSpace(c.point(1)), + c.radius()); +} + + +Capsule CoordinateFrame::toWorldSpace(const Capsule& c) const { + return Capsule( + pointToWorldSpace(c.point(0)), + pointToWorldSpace(c.point(1)), + c.radius()); +} + + +Box CoordinateFrame::toWorldSpace(const AABox& b) const { + Box b2(b); + return toWorldSpace(b2); +} + + +Box CoordinateFrame::toWorldSpace(const Box& b) const { + Box out(b); + + for (int i = 0; i < 8; ++i) { + out._corner[i] = pointToWorldSpace(b._corner[i]); + debugAssert(! isNaN(out._corner[i].x)); + } + + for (int i = 0; i < 3; ++i) { + out._axis[i] = vectorToWorldSpace(b._axis[i]); + } + + out._center = pointToWorldSpace(b._center); + + return out; +} + + +Box CoordinateFrame::toObjectSpace(const Box &b) const { + return inverse().toWorldSpace(b); +} + + +Box CoordinateFrame::toObjectSpace(const AABox& b) const { + return toObjectSpace(Box(b)); +} + + +CoordinateFrame::CoordinateFrame(class BinaryInput& b) : rotation(Matrix3::zero()) { + deserialize(b); +} + + +void CoordinateFrame::deserialize(class BinaryInput& b) { + rotation.deserialize(b); + translation.deserialize(b); +} + + +void CoordinateFrame::serialize(class BinaryOutput& b) const { + rotation.serialize(b); + translation.serialize(b); +} + + +Sphere CoordinateFrame::toWorldSpace(const Sphere &b) const { + return Sphere(pointToWorldSpace(b.center), b.radius); +} + + +Sphere CoordinateFrame::toObjectSpace(const Sphere &b) const { + return Sphere(pointToObjectSpace(b.center), b.radius); +} + + +Ray CoordinateFrame::toWorldSpace(const Ray& r) const { + return Ray::fromOriginAndDirection(pointToWorldSpace(r.origin()), vectorToWorldSpace(r.direction())); +} + + +Ray CoordinateFrame::toObjectSpace(const Ray& r) const { + return Ray::fromOriginAndDirection(pointToObjectSpace(r.origin()), vectorToObjectSpace(r.direction())); +} + + +void CoordinateFrame::lookAt(const Vector3 &target) { + lookAt(target, Vector3::unitY()); +} + + +void CoordinateFrame::lookAt( + const Vector3& target, + Vector3 up) { + + up = up.direction(); + + Vector3 look = (target - translation).direction(); + if (fabs(look.dot(up)) > .99f) { + up = Vector3::unitX(); + if (fabs(look.dot(up)) > .99f) { + up = Vector3::unitY(); + } + } + + up -= look * look.dot(up); + up.unitize(); + + Vector3 z = -look; + Vector3 x = -z.cross(up); + x.unitize(); + + Vector3 y = z.cross(x); + + rotation.setColumn(0, x); + rotation.setColumn(1, y); + rotation.setColumn(2, z); +} + + +CoordinateFrame CoordinateFrame::lerp( + const CoordinateFrame& other, + float alpha) const { + + if (alpha == 1.0f) { + return other; + } else if (alpha == 0.0f) { + return *this; + } else { + Quat q1 = Quat(this->rotation); + Quat q2 = Quat(other.rotation); + + return CoordinateFrame( + q1.slerp(q2, alpha).toRotationMatrix(), + this->translation * (1 - alpha) + other.translation * alpha); + } +} + + +void CoordinateFrame::pointToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const { + vout.resize(v.size()); + + for (int i = v.size() - 1; i >= 0; --i) { + vout[i] = pointToWorldSpace(v[i]); + } +} + + +void CoordinateFrame::normalToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const { + vout.resize(v.size()); + + for (int i = v.size() - 1; i >= 0; --i) { + vout[i] = normalToWorldSpace(v[i]); + } +} + + +void CoordinateFrame::vectorToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const { + vout.resize(v.size()); + + for (int i = v.size() - 1; i >= 0; --i) { + vout[i] = vectorToWorldSpace(v[i]); + } +} + + +void CoordinateFrame::pointToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const { + vout.resize(v.size()); + + for (int i = v.size() - 1; i >= 0; --i) { + vout[i] = pointToObjectSpace(v[i]); + } +} + + +void CoordinateFrame::normalToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const { + vout.resize(v.size()); + + for (int i = v.size() - 1; i >= 0; --i) { + vout[i] = normalToObjectSpace(v[i]); + } +} + + +void CoordinateFrame::vectorToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const { + vout.resize(v.size()); + + for (int i = v.size() - 1; i >= 0; --i) { + vout[i] = vectorToObjectSpace(v[i]); + } +} + +} // namespace diff --git a/externals/g3dlite/Crypto.cpp b/externals/g3dlite/Crypto.cpp new file mode 100644 index 00000000000..c69b23375ce --- /dev/null +++ b/externals/g3dlite/Crypto.cpp @@ -0,0 +1,70 @@ +/** + @file Crypto.cpp + + @author Morgan McGuire, http://graphics.cs.williams.edu + + + @created 2006-03-28 + @edited 2006-04-06 + */ + +#include "G3D/platform.h" +#include "G3D/Crypto.h" +#include "G3D/g3dmath.h" +#include <zlib.h> + +namespace G3D { + + +int Crypto::smallPrime(int n) { + debugAssert(n < numSmallPrimes() && n >= 0); + + // From: + // http://primes.utm.edu/lists/small/1000.txt + + static const int table[] = { + 2, 3, 5, 7, 11, 13, 17, 19, 23, 29, + 31, 37, 41, 43, 47, 53, 59, 61, 67, 71, + 73, 79, 83, 89, 97, 101, 103, 107, 109, 113, + 127, 131, 137, 139, 149, 151, 157, 163, 167, 173, + 179, 181, 191, 193, 197, 199, 211, 223, 227, 229, + 233, 239, 241, 251, 257, 263, 269, 271, 277, 281, + 283, 293, 307, 311, 313, 317, 331, 337, 347, 349, + 353, 359, 367, 373, 379, 383, 389, 397, 401, 409, + 419, 421, 431, 433, 439, 443, 449, 457, 461, 463, + 467, 479, 487, 491, 499, 503, 509, 521, 523, 541, + 547, 557, 563, 569, 571, 577, 587, 593, 599, 601, + 607, 613, 617, 619, 631, 641, 643, 647, 653, 659, + 661, 673, 677, 683, 691, 701, 709, 719, 727, 733, + 739, 743, 751, 757, 761, 769, 773, 787, 797, 809, + 811, 821, 823, 827, 829, 839, 853, 857, 859, 863, + 877, 881, 883, 887, 907, 911, 919, 929, 937, 941, + 947, 953, 967, 971, 977, 983, 991, 997, 1009, 1013, + 1019, 1021, 1031, 1033, 1039, 1049, 1051, 1061, 1063, 1069, + 1087, 1091, 1093, 1097, 1103, 1109, 1117, 1123, 1129, 1151, + 1153, 1163, 1171, 1181, 1187, 1193, 1201, 1213, 1217, 1223, + 1229, 1231, 1237, 1249, 1259, 1277, 1279, 1283, 1289, 1291, + 1297, 1301, 1303, 1307, 1319, 1321, 1327, 1361, 1367, 1373, + 1381, 1399, 1409, 1423, 1427, 1429, 1433, 1439, 1447, 1451, + 1453, 1459, 1471, 1481, 1483, 1487, 1489, 1493, 1499, 1511, + 1523, 1531, 1543, 1549, 1553, 1559, 1567, 1571, 1579, 1583, + 1597, 1601, 1607, 1609, 1613, 1619, 1621, 1627, 1637, 1657, + 1663, 1667, 1669, 1693, 1697, 1699, 1709, 1721, 1723, 1733, + 1741, 1747, 1753, 1759, 1777, 1783, 1787, 1789, 1801, 1811, + 1823, 1831, 1847, 1861, 1867, 1871, 1873, 1877, 1879, 1889, + 1901, 1907, 1913, 1931, 1933, 1949, 1951, 1973, 1979, 1987, + 1993, 1997, 1999}; + + return table[n]; +} + + +int Crypto::numSmallPrimes() { + return 303; +} + +uint32 Crypto::crc32(const void* byte, size_t numBytes) { + return ::crc32(::crc32(0, Z_NULL, 0), static_cast<const Bytef *>(byte), numBytes); +} + +} // G3D diff --git a/externals/g3dlite/Cylinder.cpp b/externals/g3dlite/Cylinder.cpp new file mode 100644 index 00000000000..7a7b9f9440d --- /dev/null +++ b/externals/g3dlite/Cylinder.cpp @@ -0,0 +1,176 @@ +/** + @file Cylinder.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2003-02-07 + @edited 2006-02-18 + + Copyright 2000-2006, Morgan McGuire. + All rights reserved. + */ + +#include "G3D/platform.h" +#include "G3D/Cylinder.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/LineSegment.h" +#include "G3D/CoordinateFrame.h" +#include "G3D/Line.h" +#include "G3D/AABox.h" + +namespace G3D { + +Cylinder::Cylinder(class BinaryInput& b) { + deserialize(b); +} + + +Cylinder::Cylinder() { +} + + +Cylinder::Cylinder(const Vector3& _p1, const Vector3& _p2, float _r) + : p1(_p1), p2(_p2), mRadius(_r) { +} + + +void Cylinder::serialize(class BinaryOutput& b) const { + p1.serialize(b); + p2.serialize(b); + b.writeFloat64(mRadius); +} + + +void Cylinder::deserialize(class BinaryInput& b) { + p1.deserialize(b); + p2.deserialize(b); + mRadius = b.readFloat64(); +} + + +Line Cylinder::axis() const { + return Line::fromTwoPoints(p1, p2); +} + + + +float Cylinder::radius() const { + return mRadius; +} + + +float Cylinder::volume() const { + return + (float)pi() * square(mRadius) * (p1 - p2).magnitude(); +} + + +float Cylinder::area() const { + return + // Sides + (twoPi() * mRadius) * height() + + + // Caps + twoPi() * square(mRadius); +} + +void Cylinder::getBounds(AABox& out) const { + Vector3 min = p1.min(p2) - (Vector3(1, 1, 1) * mRadius); + Vector3 max = p1.max(p2) + (Vector3(1, 1, 1) * mRadius); + out = AABox(min, max); +} + +bool Cylinder::contains(const Vector3& p) const { + return LineSegment::fromTwoPoints(p1, p2).distanceSquared(p) <= square(mRadius); +} + + +void Cylinder::getReferenceFrame(CoordinateFrame& cframe) const { + cframe.translation = center(); + + Vector3 Y = (p1 - p2).direction(); + Vector3 X = (abs(Y.dot(Vector3::unitX())) > 0.9) ? Vector3::unitY() : Vector3::unitX(); + Vector3 Z = X.cross(Y).direction(); + X = Y.cross(Z); + cframe.rotation.setColumn(0, X); + cframe.rotation.setColumn(1, Y); + cframe.rotation.setColumn(2, Z); +} + + +void Cylinder::getRandomSurfacePoint(Vector3& p, Vector3& N) const { + float h = height(); + float r = radius(); + + // Create a random point on a standard cylinder and then rotate to the global frame. + + // Relative areas (factor of 2PI already taken out) + float capRelArea = square(r) / 2.0f; + float sideRelArea = r * h; + + float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea); + + if (r1 < capRelArea * 2) { + + // Select a point uniformly at random on a disk + // @cite http://mathworld.wolfram.com/DiskPointPicking.html + float a = uniformRandom(0, (float)twoPi()); + float r2 = sqrt(uniformRandom(0, 1)) * r; + p.x = cos(a) * r2; + p.z = sin(a) * r2; + + N.x = 0; + N.z = 0; + if (r1 < capRelArea) { + // Top + p.y = h / 2.0f; + N.y = 1; + } else { + // Bottom + p.y = -h / 2.0f; + N.y = -1; + } + } else { + // Side + float a = uniformRandom(0, (float)twoPi()); + N.x = cos(a); + N.y = 0; + N.z = sin(a); + p.x = N.x * r; + p.z = N.y * r; + p.y = uniformRandom(-h / 2.0f, h / 2.0f); + } + + // Transform to world space + CoordinateFrame cframe; + getReferenceFrame(cframe); + + p = cframe.pointToWorldSpace(p); + N = cframe.normalToWorldSpace(N); +} + + +Vector3 Cylinder::randomInteriorPoint() const { + float h = height(); + float r = radius(); + + // Create a random point in a standard cylinder and then rotate to the global frame. + + // Select a point uniformly at random on a disk + // @cite http://mathworld.wolfram.com/DiskPointPicking.html + float a = uniformRandom(0, (float)twoPi()); + float r2 = sqrt(uniformRandom(0, 1)) * r; + + Vector3 p( cos(a) * r2, + uniformRandom(-h / 2.0f, h / 2.0f), + sin(a) * r2); + + // Transform to world space + CoordinateFrame cframe; + getReferenceFrame(cframe); + + return cframe.pointToWorldSpace(p); +} + +} // namespace diff --git a/externals/g3dlite/Line.cpp b/externals/g3dlite/Line.cpp new file mode 100644 index 00000000000..195ae7197f2 --- /dev/null +++ b/externals/g3dlite/Line.cpp @@ -0,0 +1,89 @@ +/** + @file Line.cpp + + Line class + + @maintainer Morgan McGuire, graphics3d.com + + @created 2001-06-02 + @edited 2006-01-28 + */ + +#include "G3D/Line.h" +#include "G3D/Plane.h" + +namespace G3D { + +Vector3 Line::intersection(const Plane& plane) const { + float d; + Vector3 normal = plane.normal(); + plane.getEquation(normal, d); + float rate = _direction.dot(normal); + + if (rate == 0) { + + return Vector3::inf(); + + } else { + float t = -(d + _point.dot(normal)) / rate; + + return _point + _direction * t; + } +} + + +Line::Line(class BinaryInput& b) { + deserialize(b); +} + + +void Line::serialize(class BinaryOutput& b) const { + _point.serialize(b); + _direction.serialize(b); +} + + +void Line::deserialize(class BinaryInput& b) { + _point.deserialize(b); + _direction.deserialize(b); +} + + +Vector3 Line::closestPoint(const Vector3& pt) const { + float t = _direction.dot(pt - _point); + return _point + _direction * t; +} + + +Vector3 Line::point() const { + return _point; +} + + +Vector3 Line::direction() const { + return _direction; +} + + +Vector3 Line::closestPoint(const Line& B, float& minDist) const { + const Vector3& P1 = _point; + const Vector3& U1 = _direction; + + Vector3 P2 = B.point(); + Vector3 U2 = B.direction(); + + const Vector3& P21 = P2 - P1; + const Vector3& M = U2.cross(U1); + float m2 = M.length(); + + Vector3 R = P21.cross(M) / m2; + + float t1 = R.dot(U2); + + minDist = abs(P21.dot(M)) / sqrt(m2); + + return P1 + t1 * U1; +} + +} + diff --git a/externals/g3dlite/LineSegment.cpp b/externals/g3dlite/LineSegment.cpp new file mode 100644 index 00000000000..754600ad554 --- /dev/null +++ b/externals/g3dlite/LineSegment.cpp @@ -0,0 +1,236 @@ +/** + @file LineSegment.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2003-02-08 + @edited 2008-02-02 + */ + +#include "G3D/platform.h" +#include "G3D/LineSegment.h" +#include "G3D/Sphere.h" +#include "G3D/debug.h" + +namespace G3D { + + +Vector3 LineSegment::closestPoint(const Vector3& p) const { + + // The vector from the end of the capsule to the point in question. + Vector3 v(p - _point); + + // Projection of v onto the line segment scaled by + // the length of direction. + float t = direction.dot(v); + + // Avoid some square roots. Derivation: + // t/direction.length() <= direction.length() + // t <= direction.squaredLength() + + if ((t >= 0) && (t <= direction.squaredMagnitude())) { + + // The point falls within the segment. Normalize direction, + // divide t by the length of direction. + return _point + direction * t / direction.squaredMagnitude(); + + } else { + + // The point does not fall within the segment; see which end is closer. + + // Distance from 0, squared + float d0Squared = v.squaredMagnitude(); + + // Distance from 1, squared + float d1Squared = (v - direction).squaredMagnitude(); + + if (d0Squared < d1Squared) { + + // Point 0 is closer + return _point; + + } else { + + // Point 1 is closer + return _point + direction; + + } + } + +} + +Vector3 LineSegment::point(int i) const { + switch (i) { + case 0: + return _point; + + case 1: + return _point + direction; + + default: + debugAssertM(i == 0 || i == 1, "Argument to point must be 0 or 1"); + return _point; + } +} + + +bool LineSegment::intersectsSolidSphere(const class Sphere& s) const { + return distanceSquared(s.center) <= square(s.radius); +} + + +LineSegment::LineSegment(class BinaryInput& b) { + deserialize(b); +} + + +void LineSegment::serialize(class BinaryOutput& b) const { + _point.serialize(b); + direction.serialize(b); +} + + +void LineSegment::deserialize(class BinaryInput& b) { + _point.deserialize(b); + direction.deserialize(b); +} + + +Vector3 LineSegment::randomPoint() const { + return _point + uniformRandom(0, 1) * direction; +} + + +///////////////////////////////////////////////////////////////////////////////////// + +LineSegment2D LineSegment2D::fromTwoPoints(const Vector2& p0, const Vector2& p1) { + LineSegment2D s; + s.m_origin = p0; + s.m_direction = p1 - p0; + s.m_length = s.m_direction.length(); + return s; +} + + +Vector2 LineSegment2D::point(int i) const { + debugAssert(i == 0 || i == 1); + if (i == 0) { + return m_origin; + } else { + return m_direction + m_origin; + } +} + + +Vector2 LineSegment2D::closestPoint(const Vector2& Q) const { + // Two constants that appear in the result + const Vector2 k1(m_origin - Q); + const Vector2& k2 = m_direction; + + if (fuzzyEq(m_length, 0)) { + // This line segment has no length + return m_origin; + } + + // Time [0, 1] at which we hit the closest point travelling from p0 to p1. + // Derivation can be obtained by minimizing the expression + // ||P0 + (P1 - P0)t - Q||. + const float t = -k1.dot(k2) / (m_length * m_length); + + if (t < 0) { + // Clipped to low end point + return m_origin; + } else if (t > 1) { + // Clipped to high end point + return m_origin + m_direction; + } else { + // Subsitute into the line equation to find + // the point on the segment. + return m_origin + k2 * t; + } +} + + +float LineSegment2D::distance(const Vector2& p) const { + Vector2 closest = closestPoint(p); + return (closest - p).length(); +} + + +float LineSegment2D::length() const { + return m_length; +} + + +Vector2 LineSegment2D::intersection(const LineSegment2D& other) const { + + if ((m_origin == other.m_origin) || + (m_origin == other.m_origin + other.m_direction)) { + return m_origin; + } + + if (m_origin + m_direction == other.m_origin) { + return other.m_origin; + } + + // Note: Now that we've checked the endpoints, all other parallel lines can now be assumed + // to not intersect (within numerical precision) + + Vector2 dir1 = m_direction; + Vector2 dir2 = other.m_direction; + Vector2 origin1 = m_origin; + Vector2 origin2 = other.m_origin; + + if (dir1.x == 0) { + // Avoid an upcoming divide by zero + dir1 = dir1.yx(); + dir2 = dir2.yx(); + origin1 = origin1.yx(); + origin2 = origin2.yx(); + } + + // t1 = ((other.m_origin.x - m_origin.x) + other.m_direction.x * t2) / m_direction.x + // + // ((other.m_origin.x - m_origin.x) + other.m_direction.x * t2) * m_direction.y / m_direction.x = + // (other.m_origin.y - m_origin.y) + other.m_direction.y * t2 + // + // m = m_direction.y / m_direction.x + // d = other.m_origin - m_origin + // + // (d.x + other.m_direction.x * t2) * m = d.y + other.m_direction.y * t2 + // + // d.x * m + other.m_direction.x * m * t2 = d.y + other.m_direction.y * t2 + // + // d.x * m - d.y = (other.m_direction.y - other.m_direction.x * m) * t2 + // + // (d.x * m - d.y) / (other.m_direction.y - other.m_direction.x * m) = t2 + // + + Vector2 d = origin2 - origin1; + float m = dir1.y / dir1.x; + + float t2 = (d.x * m - d.y) / (dir2.y - dir2.x * m); + if (! isFinite(t2)) { + // Parallel lines: no intersection + return Vector2::inf(); + } + + if ((t2 < 0.0f) || (t2 > 1.0f)) { + // Intersection occurs past the end of the line segments + return Vector2::inf(); + } + + float t1 = (d.x + dir2.x * t2) / dir1.x; + if ((t1 < 0.0f) || (t1 > 1.0f)) { + // Intersection occurs past the end of the line segments + return Vector2::inf(); + } + + // Return the intersection point (computed from non-transposed + // variables even if we flipped above) + return m_origin + m_direction * t1; + +} + +} + diff --git a/externals/g3dlite/Log.cpp b/externals/g3dlite/Log.cpp new file mode 100644 index 00000000000..07614fcf563 --- /dev/null +++ b/externals/g3dlite/Log.cpp @@ -0,0 +1,146 @@ +/** + @file Log.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + @created 2001-08-04 + @edited 2009-01-15 + */ + +#include "G3D/platform.h" +#include "G3D/Log.h" +#include "G3D/format.h" +#include "G3D/Array.h" +#include "G3D/fileutils.h" +#include <time.h> + +#ifdef G3D_WIN32 + #include <imagehlp.h> +#else + #include <stdarg.h> +#endif + +namespace G3D { + +void logPrintf(const char* fmt, ...) { + va_list arg_list; + va_start(arg_list, fmt); + Log::common()->vprintf(fmt, arg_list); + va_end(arg_list); +} + + +void logLazyPrintf(const char* fmt, ...) { + va_list arg_list; + va_start(arg_list, fmt); + Log::common()->lazyvprintf(fmt, arg_list); + va_end(arg_list); +} + +Log* Log::commonLog = NULL; + +Log::Log(const std::string& filename, int stripFromStackBottom) : + stripFromStackBottom(stripFromStackBottom) { + + this->filename = filename; + + logFile = fopen(filename.c_str(), "w"); + + if (logFile == NULL) { + std::string drive, base, ext; + Array<std::string> path; + parseFilename(filename, drive, path, base, ext); + std::string logName = base + ((ext != "") ? ("." + ext) : ""); + + // Write time is greater than 1ms. This may be a network drive.... try another file. + #ifdef G3D_WIN32 + logName = std::string(std::getenv("TEMP")) + logName; + #else + logName = std::string("/tmp/") + logName; + #endif + + logFile = fopen(logName.c_str(), "w"); + } + + // Use a large buffer (although we flush in logPrintf) + setvbuf(logFile, NULL, _IOFBF, 2048); + + fprintf(logFile, "Application Log\n"); + time_t t; + time(&t); + fprintf(logFile, "Start: %s\n", ctime(&t)); + fflush(logFile); + + if (commonLog == NULL) { + commonLog = this; + } +} + + +Log::~Log() { + section("Shutdown"); + println("Closing log file"); + + // Make sure we don't leave a dangling pointer + if (Log::commonLog == this) { + Log::commonLog = NULL; + } + + fclose(logFile); +} + + +FILE* Log::getFile() const { + return logFile; +} + + +Log* Log::common() { + if (commonLog == NULL) { + commonLog = new Log(); + } + return commonLog; +} + + +std::string Log::getCommonLogFilename() { + return common()->filename; +} + + +void Log::section(const std::string& s) { + fprintf(logFile, "_____________________________________________________\n"); + fprintf(logFile, "\n ### %s ###\n\n", s.c_str()); +} + + +void __cdecl Log::printf(const char* fmt, ...) { + va_list arg_list; + va_start(arg_list, fmt); + print(vformat(fmt, arg_list)); + va_end(arg_list); +} + + +void __cdecl Log::vprintf(const char* fmt, va_list argPtr) { + vfprintf(logFile, fmt, argPtr); + fflush(logFile); +} + + +void __cdecl Log::lazyvprintf(const char* fmt, va_list argPtr) { + vfprintf(logFile, fmt, argPtr); +} + + +void Log::print(const std::string& s) { + fprintf(logFile, "%s", s.c_str()); + fflush(logFile); +} + + +void Log::println(const std::string& s) { + fprintf(logFile, "%s\n", s.c_str()); + fflush(logFile); +} + +} diff --git a/externals/g3dlite/Matrix3.cpp b/externals/g3dlite/Matrix3.cpp new file mode 100644 index 00000000000..b32d938f0f9 --- /dev/null +++ b/externals/g3dlite/Matrix3.cpp @@ -0,0 +1,1927 @@ +/** + @file Matrix3.cpp + + 3x3 matrix class + + @author Morgan McGuire, graphics3d.com + + @created 2001-06-02 + @edited 2009-11-15 + + Copyright 2000-2009, Morgan McGuire. + All rights reserved. +*/ + +#include "G3D/platform.h" +#include <memory.h> +#include <assert.h> +#include "G3D/Matrix3.h" +#include "G3D/g3dmath.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/Quat.h" +#include "G3D/Any.h" + +namespace G3D { + +const float Matrix3::EPSILON = 1e-06f; + +Matrix3::Matrix3(const Any& any) { + any.verifyName("Matrix3"); + any.verifyType(Any::ARRAY); + any.verifySize(9); + + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + elt[r][c] = any[r * 3 + c]; + } + } +} + + +Matrix3::operator Any() const { + Any any(Any::ARRAY, "Matrix3"); + any.resize(9); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + any[r * 3 + c] = elt[r][c]; + } + } + + return any; +} + +const Matrix3& Matrix3::zero() { + static Matrix3 m(0, 0, 0, 0, 0, 0, 0, 0, 0); + return m; +} + +const Matrix3& Matrix3::identity() { + static Matrix3 m(1, 0, 0, 0, 1, 0, 0, 0, 1); + return m; +} + + +const float Matrix3::ms_fSvdEpsilon = 1e-04f; +const int Matrix3::ms_iSvdMaxIterations = 32; + +Matrix3::Matrix3(BinaryInput& b) { + deserialize(b); +} + +bool Matrix3::fuzzyEq(const Matrix3& b) const { + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + if (! G3D::fuzzyEq(elt[r][c], b[r][c])) { + return false; + } + } + } + return true; +} + + +bool Matrix3::isRightHanded() const{ + + const Vector3& X = column(0); + const Vector3& Y = column(1); + const Vector3& Z = column(2); + + const Vector3& W = X.cross(Y); + + return W.dot(Z) > 0.0f; +} + + +bool Matrix3::isOrthonormal() const { + const Vector3& X = column(0); + const Vector3& Y = column(1); + const Vector3& Z = column(2); + + return + (G3D::fuzzyEq(X.dot(Y), 0.0f) && + G3D::fuzzyEq(Y.dot(Z), 0.0f) && + G3D::fuzzyEq(X.dot(Z), 0.0f) && + G3D::fuzzyEq(X.squaredMagnitude(), 1.0f) && + G3D::fuzzyEq(Y.squaredMagnitude(), 1.0f) && + G3D::fuzzyEq(Z.squaredMagnitude(), 1.0f)); +} + +//---------------------------------------------------------------------------- +Matrix3::Matrix3(const Quat& _q) { + // Implementation from Watt and Watt, pg 362 + // See also http://www.flipcode.com/documents/matrfaq.html#Q54 + Quat q = _q; + q.unitize(); + float xx = 2.0f * q.x * q.x; + float xy = 2.0f * q.x * q.y; + float xz = 2.0f * q.x * q.z; + float xw = 2.0f * q.x * q.w; + + float yy = 2.0f * q.y * q.y; + float yz = 2.0f * q.y * q.z; + float yw = 2.0f * q.y * q.w; + + float zz = 2.0f * q.z * q.z; + float zw = 2.0f * q.z * q.w; + + set(1.0f - yy - zz, xy - zw, xz + yw, + xy + zw, 1.0f - xx - zz, yz - xw, + xz - yw, yz + xw, 1.0f - xx - yy); +} + +//---------------------------------------------------------------------------- + +Matrix3::Matrix3 (const float aafEntry[3][3]) { + memcpy(elt, aafEntry, 9*sizeof(float)); +} + +//---------------------------------------------------------------------------- +Matrix3::Matrix3 (const Matrix3& rkMatrix) { + memcpy(elt, rkMatrix.elt, 9*sizeof(float)); +} + +//---------------------------------------------------------------------------- +Matrix3::Matrix3( + float fEntry00, float fEntry01, float fEntry02, + float fEntry10, float fEntry11, float fEntry12, + float fEntry20, float fEntry21, float fEntry22) { + set(fEntry00, fEntry01, fEntry02, + fEntry10, fEntry11, fEntry12, + fEntry20, fEntry21, fEntry22); +} + +void Matrix3::set( + float fEntry00, float fEntry01, float fEntry02, + float fEntry10, float fEntry11, float fEntry12, + float fEntry20, float fEntry21, float fEntry22) { + + elt[0][0] = fEntry00; + elt[0][1] = fEntry01; + elt[0][2] = fEntry02; + elt[1][0] = fEntry10; + elt[1][1] = fEntry11; + elt[1][2] = fEntry12; + elt[2][0] = fEntry20; + elt[2][1] = fEntry21; + elt[2][2] = fEntry22; +} + + +void Matrix3::deserialize(BinaryInput& b) { + int r,c; + for (c = 0; c < 3; ++c) { + for (r = 0; r < 3; ++r) { + elt[r][c] = b.readFloat32(); + } + } +} + + +void Matrix3::serialize(BinaryOutput& b) const { + int r,c; + for (c = 0; c < 3; ++c) { + for (r = 0; r < 3; ++r) { + b.writeFloat32(elt[r][c]); + } + } +} + + +//---------------------------------------------------------------------------- +Vector3 Matrix3::column (int iCol) const { + assert((0 <= iCol) && (iCol < 3)); + return Vector3(elt[0][iCol], elt[1][iCol], + elt[2][iCol]); +} + + +const Vector3& Matrix3::row (int iRow) const { + assert((0 <= iRow) && (iRow < 3)); + return *reinterpret_cast<const Vector3*>(elt[iRow]); +} + + +void Matrix3::setColumn(int iCol, const Vector3 &vector) { + debugAssert((iCol >= 0) && (iCol < 3)); + elt[0][iCol] = vector.x; + elt[1][iCol] = vector.y; + elt[2][iCol] = vector.z; +} + + +void Matrix3::setRow(int iRow, const Vector3 &vector) { + debugAssert((iRow >= 0) && (iRow < 3)); + elt[iRow][0] = vector.x; + elt[iRow][1] = vector.y; + elt[iRow][2] = vector.z; +} + + +//---------------------------------------------------------------------------- +bool Matrix3::operator== (const Matrix3& rkMatrix) const { + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + if ( elt[iRow][iCol] != rkMatrix.elt[iRow][iCol] ) + return false; + } + } + + return true; +} + +//---------------------------------------------------------------------------- +bool Matrix3::operator!= (const Matrix3& rkMatrix) const { + return !operator==(rkMatrix); +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const { + Matrix3 kSum; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kSum.elt[iRow][iCol] = elt[iRow][iCol] + + rkMatrix.elt[iRow][iCol]; + } + } + + return kSum; +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const { + Matrix3 kDiff; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kDiff.elt[iRow][iCol] = elt[iRow][iCol] - + rkMatrix.elt[iRow][iCol]; + } + } + + return kDiff; +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const { + Matrix3 kProd; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kProd.elt[iRow][iCol] = + elt[iRow][0] * rkMatrix.elt[0][iCol] + + elt[iRow][1] * rkMatrix.elt[1][iCol] + + elt[iRow][2] * rkMatrix.elt[2][iCol]; + } + } + + return kProd; +} + +Matrix3& Matrix3::operator+= (const Matrix3& rkMatrix) { + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + elt[iRow][iCol] = elt[iRow][iCol] + rkMatrix.elt[iRow][iCol]; + } + } + + return *this; +} + +Matrix3& Matrix3::operator-= (const Matrix3& rkMatrix) { + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + elt[iRow][iCol] = elt[iRow][iCol] - rkMatrix.elt[iRow][iCol]; + } + } + + return *this; +} + +Matrix3& Matrix3::operator*= (const Matrix3& rkMatrix) { + Matrix3 mulMat; + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + mulMat.elt[iRow][iCol] = + elt[iRow][0] * rkMatrix.elt[0][iCol] + + elt[iRow][1] * rkMatrix.elt[1][iCol] + + elt[iRow][2] * rkMatrix.elt[2][iCol]; + } + } + + *this = mulMat; + return *this; +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::operator- () const { + Matrix3 kNeg; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kNeg[iRow][iCol] = -elt[iRow][iCol]; + } + } + + return kNeg; +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::operator* (float fScalar) const { + Matrix3 kProd; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kProd[iRow][iCol] = fScalar * elt[iRow][iCol]; + } + } + + return kProd; +} + +Matrix3& Matrix3::operator/= (float fScalar) { + return *this *= (1.0f / fScalar); +} + +Matrix3& Matrix3::operator*= (float fScalar) { + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + elt[iRow][iCol] *= fScalar; + } + } + + return *this; +} + +//---------------------------------------------------------------------------- +Matrix3 operator* (double fScalar, const Matrix3& rkMatrix) { + Matrix3 kProd; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kProd[iRow][iCol] = fScalar * rkMatrix.elt[iRow][iCol]; + } + } + + return kProd; +} + +Matrix3 operator* (float fScalar, const Matrix3& rkMatrix) { + return (double)fScalar * rkMatrix; +} + + +Matrix3 operator* (int fScalar, const Matrix3& rkMatrix) { + return (double)fScalar * rkMatrix; +} +//---------------------------------------------------------------------------- +Matrix3 Matrix3::transpose () const { + Matrix3 kTranspose; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + kTranspose[iRow][iCol] = elt[iCol][iRow]; + } + } + + return kTranspose; +} + +//---------------------------------------------------------------------------- +bool Matrix3::inverse (Matrix3& rkInverse, float fTolerance) const { + // Invert a 3x3 using cofactors. This is about 8 times faster than + // the Numerical Recipes code which uses Gaussian elimination. + + rkInverse[0][0] = elt[1][1] * elt[2][2] - + elt[1][2] * elt[2][1]; + rkInverse[0][1] = elt[0][2] * elt[2][1] - + elt[0][1] * elt[2][2]; + rkInverse[0][2] = elt[0][1] * elt[1][2] - + elt[0][2] * elt[1][1]; + rkInverse[1][0] = elt[1][2] * elt[2][0] - + elt[1][0] * elt[2][2]; + rkInverse[1][1] = elt[0][0] * elt[2][2] - + elt[0][2] * elt[2][0]; + rkInverse[1][2] = elt[0][2] * elt[1][0] - + elt[0][0] * elt[1][2]; + rkInverse[2][0] = elt[1][0] * elt[2][1] - + elt[1][1] * elt[2][0]; + rkInverse[2][1] = elt[0][1] * elt[2][0] - + elt[0][0] * elt[2][1]; + rkInverse[2][2] = elt[0][0] * elt[1][1] - + elt[0][1] * elt[1][0]; + + float fDet = + elt[0][0] * rkInverse[0][0] + + elt[0][1] * rkInverse[1][0] + + elt[0][2] * rkInverse[2][0]; + + if ( G3D::abs(fDet) <= fTolerance ) + return false; + + float fInvDet = 1.0 / fDet; + + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) + rkInverse[iRow][iCol] *= fInvDet; + } + + return true; +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::inverse (float fTolerance) const { + Matrix3 kInverse = Matrix3::zero(); + inverse(kInverse, fTolerance); + return kInverse; +} + +//---------------------------------------------------------------------------- +float Matrix3::determinant () const { + float fCofactor00 = elt[1][1] * elt[2][2] - + elt[1][2] * elt[2][1]; + float fCofactor10 = elt[1][2] * elt[2][0] - + elt[1][0] * elt[2][2]; + float fCofactor20 = elt[1][0] * elt[2][1] - + elt[1][1] * elt[2][0]; + + float fDet = + elt[0][0] * fCofactor00 + + elt[0][1] * fCofactor10 + + elt[0][2] * fCofactor20; + + return fDet; +} + +//---------------------------------------------------------------------------- +void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL, + Matrix3& kR) { + float afV[3], afW[3]; + float fLength, fSign, fT1, fInvT1, fT2; + bool bIdentity; + + // map first column to (*,0,0) + fLength = sqrt(kA[0][0] * kA[0][0] + kA[1][0] * kA[1][0] + + kA[2][0] * kA[2][0]); + + if ( fLength > 0.0 ) { + fSign = (kA[0][0] > 0.0 ? 1.0 : -1.0); + fT1 = kA[0][0] + fSign * fLength; + fInvT1 = 1.0 / fT1; + afV[1] = kA[1][0] * fInvT1; + afV[2] = kA[2][0] * fInvT1; + + fT2 = -2.0 / (1.0 + afV[1] * afV[1] + afV[2] * afV[2]); + afW[0] = fT2 * (kA[0][0] + kA[1][0] * afV[1] + kA[2][0] * afV[2]); + afW[1] = fT2 * (kA[0][1] + kA[1][1] * afV[1] + kA[2][1] * afV[2]); + afW[2] = fT2 * (kA[0][2] + kA[1][2] * afV[1] + kA[2][2] * afV[2]); + kA[0][0] += afW[0]; + kA[0][1] += afW[1]; + kA[0][2] += afW[2]; + kA[1][1] += afV[1] * afW[1]; + kA[1][2] += afV[1] * afW[2]; + kA[2][1] += afV[2] * afW[1]; + kA[2][2] += afV[2] * afW[2]; + + kL[0][0] = 1.0 + fT2; + kL[0][1] = kL[1][0] = fT2 * afV[1]; + kL[0][2] = kL[2][0] = fT2 * afV[2]; + kL[1][1] = 1.0 + fT2 * afV[1] * afV[1]; + kL[1][2] = kL[2][1] = fT2 * afV[1] * afV[2]; + kL[2][2] = 1.0 + fT2 * afV[2] * afV[2]; + bIdentity = false; + } else { + kL = Matrix3::identity(); + bIdentity = true; + } + + // map first row to (*,*,0) + fLength = sqrt(kA[0][1] * kA[0][1] + kA[0][2] * kA[0][2]); + + if ( fLength > 0.0 ) { + fSign = (kA[0][1] > 0.0 ? 1.0 : -1.0); + fT1 = kA[0][1] + fSign * fLength; + afV[2] = kA[0][2] / fT1; + + fT2 = -2.0 / (1.0 + afV[2] * afV[2]); + afW[0] = fT2 * (kA[0][1] + kA[0][2] * afV[2]); + afW[1] = fT2 * (kA[1][1] + kA[1][2] * afV[2]); + afW[2] = fT2 * (kA[2][1] + kA[2][2] * afV[2]); + kA[0][1] += afW[0]; + kA[1][1] += afW[1]; + kA[1][2] += afW[1] * afV[2]; + kA[2][1] += afW[2]; + kA[2][2] += afW[2] * afV[2]; + + kR[0][0] = 1.0; + kR[0][1] = kR[1][0] = 0.0; + kR[0][2] = kR[2][0] = 0.0; + kR[1][1] = 1.0 + fT2; + kR[1][2] = kR[2][1] = fT2 * afV[2]; + kR[2][2] = 1.0 + fT2 * afV[2] * afV[2]; + } else { + kR = Matrix3::identity(); + } + + // map second column to (*,*,0) + fLength = sqrt(kA[1][1] * kA[1][1] + kA[2][1] * kA[2][1]); + + if ( fLength > 0.0 ) { + fSign = (kA[1][1] > 0.0 ? 1.0 : -1.0); + fT1 = kA[1][1] + fSign * fLength; + afV[2] = kA[2][1] / fT1; + + fT2 = -2.0 / (1.0 + afV[2] * afV[2]); + afW[1] = fT2 * (kA[1][1] + kA[2][1] * afV[2]); + afW[2] = fT2 * (kA[1][2] + kA[2][2] * afV[2]); + kA[1][1] += afW[1]; + kA[1][2] += afW[2]; + kA[2][2] += afV[2] * afW[2]; + + float fA = 1.0 + fT2; + float fB = fT2 * afV[2]; + float fC = 1.0 + fB * afV[2]; + + if ( bIdentity ) { + kL[0][0] = 1.0; + kL[0][1] = kL[1][0] = 0.0; + kL[0][2] = kL[2][0] = 0.0; + kL[1][1] = fA; + kL[1][2] = kL[2][1] = fB; + kL[2][2] = fC; + } else { + for (int iRow = 0; iRow < 3; iRow++) { + float fTmp0 = kL[iRow][1]; + float fTmp1 = kL[iRow][2]; + kL[iRow][1] = fA * fTmp0 + fB * fTmp1; + kL[iRow][2] = fB * fTmp0 + fC * fTmp1; + } + } + } +} + +//---------------------------------------------------------------------------- +void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL, + Matrix3& kR) { + float fT11 = kA[0][1] * kA[0][1] + kA[1][1] * kA[1][1]; + float fT22 = kA[1][2] * kA[1][2] + kA[2][2] * kA[2][2]; + float fT12 = kA[1][1] * kA[1][2]; + float fTrace = fT11 + fT22; + float fDiff = fT11 - fT22; + float fDiscr = sqrt(fDiff * fDiff + 4.0 * fT12 * fT12); + float fRoot1 = 0.5 * (fTrace + fDiscr); + float fRoot2 = 0.5 * (fTrace - fDiscr); + + // adjust right + float fY = kA[0][0] - (G3D::abs(fRoot1 - fT22) <= + G3D::abs(fRoot2 - fT22) ? fRoot1 : fRoot2); + float fZ = kA[0][1]; + float fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ); + float fSin = fZ * fInvLength; + float fCos = -fY * fInvLength; + + float fTmp0 = kA[0][0]; + float fTmp1 = kA[0][1]; + kA[0][0] = fCos * fTmp0 - fSin * fTmp1; + kA[0][1] = fSin * fTmp0 + fCos * fTmp1; + kA[1][0] = -fSin * kA[1][1]; + kA[1][1] *= fCos; + + int iRow; + + for (iRow = 0; iRow < 3; iRow++) { + fTmp0 = kR[0][iRow]; + fTmp1 = kR[1][iRow]; + kR[0][iRow] = fCos * fTmp0 - fSin * fTmp1; + kR[1][iRow] = fSin * fTmp0 + fCos * fTmp1; + } + + // adjust left + fY = kA[0][0]; + + fZ = kA[1][0]; + + fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ); + + fSin = fZ * fInvLength; + + fCos = -fY * fInvLength; + + kA[0][0] = fCos * kA[0][0] - fSin * kA[1][0]; + + fTmp0 = kA[0][1]; + + fTmp1 = kA[1][1]; + + kA[0][1] = fCos * fTmp0 - fSin * fTmp1; + + kA[1][1] = fSin * fTmp0 + fCos * fTmp1; + + kA[0][2] = -fSin * kA[1][2]; + + kA[1][2] *= fCos; + + int iCol; + + for (iCol = 0; iCol < 3; iCol++) { + fTmp0 = kL[iCol][0]; + fTmp1 = kL[iCol][1]; + kL[iCol][0] = fCos * fTmp0 - fSin * fTmp1; + kL[iCol][1] = fSin * fTmp0 + fCos * fTmp1; + } + + // adjust right + fY = kA[0][1]; + + fZ = kA[0][2]; + + fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ); + + fSin = fZ * fInvLength; + + fCos = -fY * fInvLength; + + kA[0][1] = fCos * kA[0][1] - fSin * kA[0][2]; + + fTmp0 = kA[1][1]; + + fTmp1 = kA[1][2]; + + kA[1][1] = fCos * fTmp0 - fSin * fTmp1; + + kA[1][2] = fSin * fTmp0 + fCos * fTmp1; + + kA[2][1] = -fSin * kA[2][2]; + + kA[2][2] *= fCos; + + for (iRow = 0; iRow < 3; iRow++) { + fTmp0 = kR[1][iRow]; + fTmp1 = kR[2][iRow]; + kR[1][iRow] = fCos * fTmp0 - fSin * fTmp1; + kR[2][iRow] = fSin * fTmp0 + fCos * fTmp1; + } + + // adjust left + fY = kA[1][1]; + + fZ = kA[2][1]; + + fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ); + + fSin = fZ * fInvLength; + + fCos = -fY * fInvLength; + + kA[1][1] = fCos * kA[1][1] - fSin * kA[2][1]; + + fTmp0 = kA[1][2]; + + fTmp1 = kA[2][2]; + + kA[1][2] = fCos * fTmp0 - fSin * fTmp1; + + kA[2][2] = fSin * fTmp0 + fCos * fTmp1; + + for (iCol = 0; iCol < 3; iCol++) { + fTmp0 = kL[iCol][1]; + fTmp1 = kL[iCol][2]; + kL[iCol][1] = fCos * fTmp0 - fSin * fTmp1; + kL[iCol][2] = fSin * fTmp0 + fCos * fTmp1; + } +} + +//---------------------------------------------------------------------------- +void Matrix3::singularValueDecomposition (Matrix3& kL, Vector3& kS, + Matrix3& kR) const { + int iRow, iCol; + + Matrix3 kA = *this; + bidiagonalize(kA, kL, kR); + + for (int i = 0; i < ms_iSvdMaxIterations; i++) { + float fTmp, fTmp0, fTmp1; + float fSin0, fCos0, fTan0; + float fSin1, fCos1, fTan1; + + bool bTest1 = (G3D::abs(kA[0][1]) <= + ms_fSvdEpsilon * (G3D::abs(kA[0][0]) + G3D::abs(kA[1][1]))); + bool bTest2 = (G3D::abs(kA[1][2]) <= + ms_fSvdEpsilon * (G3D::abs(kA[1][1]) + G3D::abs(kA[2][2]))); + + if ( bTest1 ) { + if ( bTest2 ) { + kS[0] = kA[0][0]; + kS[1] = kA[1][1]; + kS[2] = kA[2][2]; + break; + } else { + // 2x2 closed form factorization + fTmp = (kA[1][1] * kA[1][1] - kA[2][2] * kA[2][2] + + kA[1][2] * kA[1][2]) / (kA[1][2] * kA[2][2]); + fTan0 = 0.5 * (fTmp + sqrt(fTmp * fTmp + 4.0)); + fCos0 = 1.0 / sqrt(1.0 + fTan0 * fTan0); + fSin0 = fTan0 * fCos0; + + for (iCol = 0; iCol < 3; iCol++) { + fTmp0 = kL[iCol][1]; + fTmp1 = kL[iCol][2]; + kL[iCol][1] = fCos0 * fTmp0 - fSin0 * fTmp1; + kL[iCol][2] = fSin0 * fTmp0 + fCos0 * fTmp1; + } + + fTan1 = (kA[1][2] - kA[2][2] * fTan0) / kA[1][1]; + fCos1 = 1.0 / sqrt(1.0 + fTan1 * fTan1); + fSin1 = -fTan1 * fCos1; + + for (iRow = 0; iRow < 3; iRow++) { + fTmp0 = kR[1][iRow]; + fTmp1 = kR[2][iRow]; + kR[1][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1; + kR[2][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1; + } + + kS[0] = kA[0][0]; + kS[1] = fCos0 * fCos1 * kA[1][1] - + fSin1 * (fCos0 * kA[1][2] - fSin0 * kA[2][2]); + kS[2] = fSin0 * fSin1 * kA[1][1] + + fCos1 * (fSin0 * kA[1][2] + fCos0 * kA[2][2]); + break; + } + } else { + if ( bTest2 ) { + // 2x2 closed form factorization + fTmp = (kA[0][0] * kA[0][0] + kA[1][1] * kA[1][1] - + kA[0][1] * kA[0][1]) / (kA[0][1] * kA[1][1]); + fTan0 = 0.5 * ( -fTmp + sqrt(fTmp * fTmp + 4.0)); + fCos0 = 1.0 / sqrt(1.0 + fTan0 * fTan0); + fSin0 = fTan0 * fCos0; + + for (iCol = 0; iCol < 3; iCol++) { + fTmp0 = kL[iCol][0]; + fTmp1 = kL[iCol][1]; + kL[iCol][0] = fCos0 * fTmp0 - fSin0 * fTmp1; + kL[iCol][1] = fSin0 * fTmp0 + fCos0 * fTmp1; + } + + fTan1 = (kA[0][1] - kA[1][1] * fTan0) / kA[0][0]; + fCos1 = 1.0 / sqrt(1.0 + fTan1 * fTan1); + fSin1 = -fTan1 * fCos1; + + for (iRow = 0; iRow < 3; iRow++) { + fTmp0 = kR[0][iRow]; + fTmp1 = kR[1][iRow]; + kR[0][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1; + kR[1][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1; + } + + kS[0] = fCos0 * fCos1 * kA[0][0] - + fSin1 * (fCos0 * kA[0][1] - fSin0 * kA[1][1]); + kS[1] = fSin0 * fSin1 * kA[0][0] + + fCos1 * (fSin0 * kA[0][1] + fCos0 * kA[1][1]); + kS[2] = kA[2][2]; + break; + } else { + golubKahanStep(kA, kL, kR); + } + } + } + + // positize diagonal + for (iRow = 0; iRow < 3; iRow++) { + if ( kS[iRow] < 0.0 ) { + kS[iRow] = -kS[iRow]; + + for (iCol = 0; iCol < 3; iCol++) + kR[iRow][iCol] = -kR[iRow][iCol]; + } + } +} + +//---------------------------------------------------------------------------- +void Matrix3::singularValueComposition (const Matrix3& kL, + const Vector3& kS, const Matrix3& kR) { + int iRow, iCol; + Matrix3 kTmp; + + // product S*R + for (iRow = 0; iRow < 3; iRow++) { + for (iCol = 0; iCol < 3; iCol++) + kTmp[iRow][iCol] = kS[iRow] * kR[iRow][iCol]; + } + + // product L*S*R + for (iRow = 0; iRow < 3; iRow++) { + for (iCol = 0; iCol < 3; iCol++) { + elt[iRow][iCol] = 0.0; + + for (int iMid = 0; iMid < 3; iMid++) + elt[iRow][iCol] += kL[iRow][iMid] * kTmp[iMid][iCol]; + } + } +} + +//---------------------------------------------------------------------------- +void Matrix3::orthonormalize () { + // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is + // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2], + // + // q0 = m0/|m0| + // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| + // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| + // + // where |V| indicates length of vector V and A*B indicates dot + // product of vectors A and B. + + // compute q0 + float fInvLength = 1.0 / sqrt(elt[0][0] * elt[0][0] + + elt[1][0] * elt[1][0] + + elt[2][0] * elt[2][0]); + + elt[0][0] *= fInvLength; + elt[1][0] *= fInvLength; + elt[2][0] *= fInvLength; + + // compute q1 + float fDot0 = + elt[0][0] * elt[0][1] + + elt[1][0] * elt[1][1] + + elt[2][0] * elt[2][1]; + + elt[0][1] -= fDot0 * elt[0][0]; + elt[1][1] -= fDot0 * elt[1][0]; + elt[2][1] -= fDot0 * elt[2][0]; + + fInvLength = 1.0 / sqrt(elt[0][1] * elt[0][1] + + elt[1][1] * elt[1][1] + + elt[2][1] * elt[2][1]); + + elt[0][1] *= fInvLength; + elt[1][1] *= fInvLength; + elt[2][1] *= fInvLength; + + // compute q2 + float fDot1 = + elt[0][1] * elt[0][2] + + elt[1][1] * elt[1][2] + + elt[2][1] * elt[2][2]; + + fDot0 = + elt[0][0] * elt[0][2] + + elt[1][0] * elt[1][2] + + elt[2][0] * elt[2][2]; + + elt[0][2] -= fDot0 * elt[0][0] + fDot1 * elt[0][1]; + elt[1][2] -= fDot0 * elt[1][0] + fDot1 * elt[1][1]; + elt[2][2] -= fDot0 * elt[2][0] + fDot1 * elt[2][1]; + + fInvLength = 1.0 / sqrt(elt[0][2] * elt[0][2] + + elt[1][2] * elt[1][2] + + elt[2][2] * elt[2][2]); + + elt[0][2] *= fInvLength; + elt[1][2] *= fInvLength; + elt[2][2] *= fInvLength; +} + +//---------------------------------------------------------------------------- +void Matrix3::qDUDecomposition (Matrix3& kQ, + Vector3& kD, Vector3& kU) const { + // Factor M = QR = QDU where Q is orthogonal, D is diagonal, + // and U is upper triangular with ones on its diagonal. Algorithm uses + // Gram-Schmidt orthogonalization (the QR algorithm). + // + // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then + // + // q0 = m0/|m0| + // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0| + // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1| + // + // where |V| indicates length of vector V and A*B indicates dot + // product of vectors A and B. The matrix R has entries + // + // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2 + // r10 = 0 r11 = q1*m1 r12 = q1*m2 + // r20 = 0 r21 = 0 r22 = q2*m2 + // + // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00, + // u02 = r02/r00, and u12 = r12/r11. + + // Q = rotation + // D = scaling + // U = shear + + // D stores the three diagonal entries r00, r11, r22 + // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12 + + // build orthogonal matrix Q + float fInvLength = 1.0 / sqrt(elt[0][0] * elt[0][0] + + elt[1][0] * elt[1][0] + + elt[2][0] * elt[2][0]); + kQ[0][0] = elt[0][0] * fInvLength; + kQ[1][0] = elt[1][0] * fInvLength; + kQ[2][0] = elt[2][0] * fInvLength; + + float fDot = kQ[0][0] * elt[0][1] + kQ[1][0] * elt[1][1] + + kQ[2][0] * elt[2][1]; + kQ[0][1] = elt[0][1] - fDot * kQ[0][0]; + kQ[1][1] = elt[1][1] - fDot * kQ[1][0]; + kQ[2][1] = elt[2][1] - fDot * kQ[2][0]; + fInvLength = 1.0 / sqrt(kQ[0][1] * kQ[0][1] + kQ[1][1] * kQ[1][1] + + kQ[2][1] * kQ[2][1]); + kQ[0][1] *= fInvLength; + kQ[1][1] *= fInvLength; + kQ[2][1] *= fInvLength; + + fDot = kQ[0][0] * elt[0][2] + kQ[1][0] * elt[1][2] + + kQ[2][0] * elt[2][2]; + kQ[0][2] = elt[0][2] - fDot * kQ[0][0]; + kQ[1][2] = elt[1][2] - fDot * kQ[1][0]; + kQ[2][2] = elt[2][2] - fDot * kQ[2][0]; + fDot = kQ[0][1] * elt[0][2] + kQ[1][1] * elt[1][2] + + kQ[2][1] * elt[2][2]; + kQ[0][2] -= fDot * kQ[0][1]; + kQ[1][2] -= fDot * kQ[1][1]; + kQ[2][2] -= fDot * kQ[2][1]; + fInvLength = 1.0 / sqrt(kQ[0][2] * kQ[0][2] + kQ[1][2] * kQ[1][2] + + kQ[2][2] * kQ[2][2]); + kQ[0][2] *= fInvLength; + kQ[1][2] *= fInvLength; + kQ[2][2] *= fInvLength; + + // guarantee that orthogonal matrix has determinant 1 (no reflections) + float fDet = kQ[0][0] * kQ[1][1] * kQ[2][2] + kQ[0][1] * kQ[1][2] * kQ[2][0] + + kQ[0][2] * kQ[1][0] * kQ[2][1] - kQ[0][2] * kQ[1][1] * kQ[2][0] - + kQ[0][1] * kQ[1][0] * kQ[2][2] - kQ[0][0] * kQ[1][2] * kQ[2][1]; + + if ( fDet < 0.0 ) { + for (int iRow = 0; iRow < 3; iRow++) + for (int iCol = 0; iCol < 3; iCol++) + kQ[iRow][iCol] = -kQ[iRow][iCol]; + } + + // build "right" matrix R + Matrix3 kR; + + kR[0][0] = kQ[0][0] * elt[0][0] + kQ[1][0] * elt[1][0] + + kQ[2][0] * elt[2][0]; + + kR[0][1] = kQ[0][0] * elt[0][1] + kQ[1][0] * elt[1][1] + + kQ[2][0] * elt[2][1]; + + kR[1][1] = kQ[0][1] * elt[0][1] + kQ[1][1] * elt[1][1] + + kQ[2][1] * elt[2][1]; + + kR[0][2] = kQ[0][0] * elt[0][2] + kQ[1][0] * elt[1][2] + + kQ[2][0] * elt[2][2]; + + kR[1][2] = kQ[0][1] * elt[0][2] + kQ[1][1] * elt[1][2] + + kQ[2][1] * elt[2][2]; + + kR[2][2] = kQ[0][2] * elt[0][2] + kQ[1][2] * elt[1][2] + + kQ[2][2] * elt[2][2]; + + // the scaling component + kD[0] = kR[0][0]; + + kD[1] = kR[1][1]; + + kD[2] = kR[2][2]; + + // the shear component + float fInvD0 = 1.0 / kD[0]; + + kU[0] = kR[0][1] * fInvD0; + + kU[1] = kR[0][2] * fInvD0; + + kU[2] = kR[1][2] / kD[1]; +} + +//---------------------------------------------------------------------------- +void Matrix3::polarDecomposition(Matrix3 &R, Matrix3 &S) const{ + /* + Polar decomposition of a matrix. Based on pseudocode from + Nicholas J Higham, "Computing the Polar Decomposition -- with + Applications Siam Journal of Science and Statistical Computing, Vol 7, No. 4, + October 1986. + + Decomposes A into R*S, where R is orthogonal and S is symmetric. + + Ken Shoemake's "Matrix animation and polar decomposition" + in Proceedings of the conference on Graphics interface '92 + seems to be better known in the world of graphics, but Higham's version + uses a scaling constant that can lead to faster convergence than + Shoemake's when the initial matrix is far from orthogonal. + */ + + Matrix3 X = *this; + Matrix3 tmp = X.inverse(); + Matrix3 Xit = tmp.transpose(); + int iter = 0; + + const int MAX_ITERS = 100; + + const double eps = 50 * std::numeric_limits<float>::epsilon(); + const float BigEps = 50 * eps; + + /* Higham suggests using OneNorm(Xit-X) < eps * OneNorm(X) + * as the convergence criterion, but OneNorm(X) should quickly + * settle down to something between 1 and 1.7, so just comparing + * with eps seems sufficient. + *--------------------------------------------------------------- */ + + double resid = X.diffOneNorm(Xit); + while (resid > eps && iter < MAX_ITERS) { + + tmp = X.inverse(); + Xit = tmp.transpose(); + + if (resid < BigEps) { + // close enough use simple iteration + X += Xit; + X *= 0.5f; + } + else { + // not close to convergence, compute acceleration factor + float gamma = sqrt( sqrt( + (Xit.l1Norm()* Xit.lInfNorm())/(X.l1Norm()*X.lInfNorm()) ) ); + + X *= 0.5f * gamma; + tmp = Xit; + tmp *= 0.5f / gamma; + X += tmp; + } + + resid = X.diffOneNorm(Xit); + iter++; + } + + R = X; + tmp = R.transpose(); + + S = tmp * (*this); + + // S := (S + S^t)/2 one more time to make sure it is symmetric + tmp = S.transpose(); + + S += tmp; + S *= 0.5f; + +#ifdef G3D_DEBUG + // Check iter limit + assert(iter < MAX_ITERS); + + // Check A = R*S + tmp = R*S; + resid = tmp.diffOneNorm(*this); + assert(resid < eps); + + // Check R is orthogonal + tmp = R*R.transpose(); + resid = tmp.diffOneNorm(Matrix3::identity()); + assert(resid < eps); + + // Check that S is symmetric + tmp = S.transpose(); + resid = tmp.diffOneNorm(S); + assert(resid < eps); +#endif +} + +//---------------------------------------------------------------------------- +float Matrix3::maxCubicRoot (float afCoeff[3]) { + // Spectral norm is for A^T*A, so characteristic polynomial + // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive float roots. + // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1]. + + // quick out for uniform scale (triple root) + const float fOneThird = 1.0f / 3.0f; + const float fEpsilon = 1e-06f; + float fDiscr = afCoeff[2] * afCoeff[2] - 3.0f * afCoeff[1]; + + if ( fDiscr <= fEpsilon ) + return -fOneThird*afCoeff[2]; + + // Compute an upper bound on roots of P(x). This assumes that A^T*A + // has been scaled by its largest entry. + float fX = 1.0f; + + float fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX)); + + if ( fPoly < 0.0f ) { + // uses a matrix norm to find an upper bound on maximum root + fX = G3D::abs(afCoeff[0]); + float fTmp = 1.0 + G3D::abs(afCoeff[1]); + + if ( fTmp > fX ) + fX = fTmp; + + fTmp = 1.0 + G3D::abs(afCoeff[2]); + + if ( fTmp > fX ) + fX = fTmp; + } + + // Newton's method to find root + float fTwoC2 = 2.0f * afCoeff[2]; + + for (int i = 0; i < 16; i++) { + fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX)); + + if ( G3D::abs(fPoly) <= fEpsilon ) + return fX; + + float fDeriv = afCoeff[1] + fX * (fTwoC2 + 3.0f * fX); + + fX -= fPoly / fDeriv; + } + + return fX; +} + +//---------------------------------------------------------------------------- +float Matrix3::spectralNorm () const { + Matrix3 kP; + int iRow, iCol; + float fPmax = 0.0; + + for (iRow = 0; iRow < 3; iRow++) { + for (iCol = 0; iCol < 3; iCol++) { + kP[iRow][iCol] = 0.0; + + for (int iMid = 0; iMid < 3; iMid++) { + kP[iRow][iCol] += + elt[iMid][iRow] * elt[iMid][iCol]; + } + + if ( kP[iRow][iCol] > fPmax ) + fPmax = kP[iRow][iCol]; + } + } + + float fInvPmax = 1.0 / fPmax; + + for (iRow = 0; iRow < 3; iRow++) { + for (iCol = 0; iCol < 3; iCol++) + kP[iRow][iCol] *= fInvPmax; + } + + float afCoeff[3]; + afCoeff[0] = -(kP[0][0] * (kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1]) + + kP[0][1] * (kP[2][0] * kP[1][2] - kP[1][0] * kP[2][2]) + + kP[0][2] * (kP[1][0] * kP[2][1] - kP[2][0] * kP[1][1])); + afCoeff[1] = kP[0][0] * kP[1][1] - kP[0][1] * kP[1][0] + + kP[0][0] * kP[2][2] - kP[0][2] * kP[2][0] + + kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1]; + afCoeff[2] = -(kP[0][0] + kP[1][1] + kP[2][2]); + + float fRoot = maxCubicRoot(afCoeff); + float fNorm = sqrt(fPmax * fRoot); + return fNorm; +} + +//---------------------------------------------------------------------------- +float Matrix3::squaredFrobeniusNorm() const { + float norm2 = 0; + const float* e = &elt[0][0]; + + for (int i = 0; i < 9; ++i){ + norm2 += (*e) * (*e); + } + + return norm2; +} + +//---------------------------------------------------------------------------- +float Matrix3::frobeniusNorm() const { + return sqrtf(squaredFrobeniusNorm()); +} + +//---------------------------------------------------------------------------- +float Matrix3::l1Norm() const { + // The one norm of a matrix is the max column sum in absolute value. + float oneNorm = 0; + for (int c = 0; c < 3; ++c) { + + float f = fabs(elt[0][c])+ fabs(elt[1][c]) + fabs(elt[2][c]); + + if (f > oneNorm) { + oneNorm = f; + } + } + return oneNorm; +} + +//---------------------------------------------------------------------------- +float Matrix3::lInfNorm() const { + // The infinity norm of a matrix is the max row sum in absolute value. + float infNorm = 0; + + for (int r = 0; r < 3; ++r) { + + float f = fabs(elt[r][0]) + fabs(elt[r][1])+ fabs(elt[r][2]); + + if (f > infNorm) { + infNorm = f; + } + } + return infNorm; +} + +//---------------------------------------------------------------------------- +float Matrix3::diffOneNorm(const Matrix3 &y) const{ + float oneNorm = 0; + + for (int c = 0; c < 3; ++c){ + + float f = fabs(elt[0][c] - y[0][c]) + fabs(elt[1][c] - y[1][c]) + + fabs(elt[2][c] - y[2][c]); + + if (f > oneNorm) { + oneNorm = f; + } + } + return oneNorm; +} + +//---------------------------------------------------------------------------- +void Matrix3::toAxisAngle (Vector3& rkAxis, float& rfRadians) const { + // + // Let (x,y,z) be the unit-length axis and let A be an angle of rotation. + // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 (Rodrigues' formula) where + // I is the identity and + // + // +- -+ + // P = | 0 -z +y | + // | +z 0 -x | + // | -y +x 0 | + // +- -+ + // + // If A > 0, R represents a counterclockwise rotation about the axis in + // the sense of looking from the tip of the axis vector towards the + // origin. Some algebra will show that + // + // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P + // + // In the event that A = pi, R-R^t = 0 which prevents us from extracting + // the axis through P. Instead note that R = I+2*P^2 when A = pi, so + // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and + // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi, + // it does not matter which sign you choose on the square roots. + + float fTrace = elt[0][0] + elt[1][1] + elt[2][2]; + float fCos = 0.5f * (fTrace - 1.0f); + rfRadians = G3D::aCos(fCos); // in [0,PI] + + if ( rfRadians > 0.0 ) { + if ( rfRadians < pi() ) { + rkAxis.x = elt[2][1] - elt[1][2]; + rkAxis.y = elt[0][2] - elt[2][0]; + rkAxis.z = elt[1][0] - elt[0][1]; + rkAxis.unitize(); + } else { + // angle is PI + float fHalfInverse; + + if ( elt[0][0] >= elt[1][1] ) { + // r00 >= r11 + if ( elt[0][0] >= elt[2][2] ) { + // r00 is maximum diagonal term + rkAxis.x = 0.5 * sqrt(elt[0][0] - + elt[1][1] - elt[2][2] + 1.0); + fHalfInverse = 0.5 / rkAxis.x; + rkAxis.y = fHalfInverse * elt[0][1]; + rkAxis.z = fHalfInverse * elt[0][2]; + } else { + // r22 is maximum diagonal term + rkAxis.z = 0.5 * sqrt(elt[2][2] - + elt[0][0] - elt[1][1] + 1.0); + fHalfInverse = 0.5 / rkAxis.z; + rkAxis.x = fHalfInverse * elt[0][2]; + rkAxis.y = fHalfInverse * elt[1][2]; + } + } else { + // r11 > r00 + if ( elt[1][1] >= elt[2][2] ) { + // r11 is maximum diagonal term + rkAxis.y = 0.5 * sqrt(elt[1][1] - + elt[0][0] - elt[2][2] + 1.0); + fHalfInverse = 0.5 / rkAxis.y; + rkAxis.x = fHalfInverse * elt[0][1]; + rkAxis.z = fHalfInverse * elt[1][2]; + } else { + // r22 is maximum diagonal term + rkAxis.z = 0.5 * sqrt(elt[2][2] - + elt[0][0] - elt[1][1] + 1.0); + fHalfInverse = 0.5 / rkAxis.z; + rkAxis.x = fHalfInverse * elt[0][2]; + rkAxis.y = fHalfInverse * elt[1][2]; + } + } + } + } else { + // The angle is 0 and the matrix is the identity. Any axis will + // work, so just use the x-axis. + rkAxis.x = 1.0; + rkAxis.y = 0.0; + rkAxis.z = 0.0; + } +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromAxisAngle (const Vector3& _axis, float fRadians) { + Vector3 axis = _axis.direction(); + + Matrix3 m; + float fCos = cos(fRadians); + float fSin = sin(fRadians); + float fOneMinusCos = 1.0 - fCos; + float fX2 = square(axis.x); + float fY2 = square(axis.y); + float fZ2 = square(axis.z); + float fXYM = axis.x * axis.y * fOneMinusCos; + float fXZM = axis.x * axis.z * fOneMinusCos; + float fYZM = axis.y * axis.z * fOneMinusCos; + float fXSin = axis.x * fSin; + float fYSin = axis.y * fSin; + float fZSin = axis.z * fSin; + + m.elt[0][0] = fX2 * fOneMinusCos + fCos; + m.elt[0][1] = fXYM - fZSin; + m.elt[0][2] = fXZM + fYSin; + + m.elt[1][0] = fXYM + fZSin; + m.elt[1][1] = fY2 * fOneMinusCos + fCos; + m.elt[1][2] = fYZM - fXSin; + + m.elt[2][0] = fXZM - fYSin; + m.elt[2][1] = fYZM + fXSin; + m.elt[2][2] = fZ2 * fOneMinusCos + fCos; + + return m; +} + +//---------------------------------------------------------------------------- +bool Matrix3::toEulerAnglesXYZ (float& rfXAngle, float& rfYAngle, + float& rfZAngle) const { + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + + if ( elt[0][2] < 1.0f ) { + if ( elt[0][2] > -1.0f ) { + rfXAngle = G3D::aTan2( -elt[1][2], elt[2][2]); + rfYAngle = (float) G3D::aSin(elt[0][2]); + rfZAngle = G3D::aTan2( -elt[0][1], elt[0][0]); + return true; + } else { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + rfXAngle = -G3D::aTan2(elt[1][0], elt[1][1]); + rfYAngle = -(float)halfPi(); + rfZAngle = 0.0f; + return false; + } + } else { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + rfXAngle = G3D::aTan2(elt[1][0], elt[1][1]); + rfYAngle = (float)halfPi(); + rfZAngle = 0.0f; + return false; + } +} + +//---------------------------------------------------------------------------- +bool Matrix3::toEulerAnglesXZY (float& rfXAngle, float& rfZAngle, + float& rfYAngle) const { + // rot = cy*cz -sz cz*sy + // sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz + // -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz + + if ( elt[0][1] < 1.0f ) { + if ( elt[0][1] > -1.0f ) { + rfXAngle = G3D::aTan2(elt[2][1], elt[1][1]); + rfZAngle = (float) asin( -elt[0][1]); + rfYAngle = G3D::aTan2(elt[0][2], elt[0][0]); + return true; + } else { + // WARNING. Not unique. XA - YA = atan2(r20,r22) + rfXAngle = G3D::aTan2(elt[2][0], elt[2][2]); + rfZAngle = (float)halfPi(); + rfYAngle = 0.0; + return false; + } + } else { + // WARNING. Not unique. XA + YA = atan2(-r20,r22) + rfXAngle = G3D::aTan2( -elt[2][0], elt[2][2]); + rfZAngle = -(float)halfPi(); + rfYAngle = 0.0f; + return false; + } +} + +//---------------------------------------------------------------------------- +bool Matrix3::toEulerAnglesYXZ (float& rfYAngle, float& rfXAngle, + float& rfZAngle) const { + // rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy + // cx*sz cx*cz -sx + // -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy + + if ( elt[1][2] < 1.0 ) { + if ( elt[1][2] > -1.0 ) { + rfYAngle = G3D::aTan2(elt[0][2], elt[2][2]); + rfXAngle = (float) asin( -elt[1][2]); + rfZAngle = G3D::aTan2(elt[1][0], elt[1][1]); + return true; + } else { + // WARNING. Not unique. YA - ZA = atan2(r01,r00) + rfYAngle = G3D::aTan2(elt[0][1], elt[0][0]); + rfXAngle = (float)halfPi(); + rfZAngle = 0.0; + return false; + } + } else { + // WARNING. Not unique. YA + ZA = atan2(-r01,r00) + rfYAngle = G3D::aTan2( -elt[0][1], elt[0][0]); + rfXAngle = -(float)halfPi(); + rfZAngle = 0.0f; + return false; + } +} + +//---------------------------------------------------------------------------- +bool Matrix3::toEulerAnglesYZX (float& rfYAngle, float& rfZAngle, + float& rfXAngle) const { + // rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz + // sz cx*cz -cz*sx + // -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz + + if ( elt[1][0] < 1.0 ) { + if ( elt[1][0] > -1.0 ) { + rfYAngle = G3D::aTan2( -elt[2][0], elt[0][0]); + rfZAngle = (float) asin(elt[1][0]); + rfXAngle = G3D::aTan2( -elt[1][2], elt[1][1]); + return true; + } else { + // WARNING. Not unique. YA - XA = -atan2(r21,r22); + rfYAngle = -G3D::aTan2(elt[2][1], elt[2][2]); + rfZAngle = -(float)halfPi(); + rfXAngle = 0.0; + return false; + } + } else { + // WARNING. Not unique. YA + XA = atan2(r21,r22) + rfYAngle = G3D::aTan2(elt[2][1], elt[2][2]); + rfZAngle = (float)halfPi(); + rfXAngle = 0.0f; + return false; + } +} + +//---------------------------------------------------------------------------- +bool Matrix3::toEulerAnglesZXY (float& rfZAngle, float& rfXAngle, + float& rfYAngle) const { + // rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz + // cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz + // -cx*sy sx cx*cy + + if ( elt[2][1] < 1.0 ) { + if ( elt[2][1] > -1.0 ) { + rfZAngle = G3D::aTan2( -elt[0][1], elt[1][1]); + rfXAngle = (float) asin(elt[2][1]); + rfYAngle = G3D::aTan2( -elt[2][0], elt[2][2]); + return true; + } else { + // WARNING. Not unique. ZA - YA = -atan(r02,r00) + rfZAngle = -G3D::aTan2(elt[0][2], elt[0][0]); + rfXAngle = -(float)halfPi(); + rfYAngle = 0.0f; + return false; + } + } else { + // WARNING. Not unique. ZA + YA = atan2(r02,r00) + rfZAngle = G3D::aTan2(elt[0][2], elt[0][0]); + rfXAngle = (float)halfPi(); + rfYAngle = 0.0f; + return false; + } +} + +//---------------------------------------------------------------------------- +bool Matrix3::toEulerAnglesZYX (float& rfZAngle, float& rfYAngle, + float& rfXAngle) const { + // rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz + // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz + // -sy cy*sx cx*cy + + if ( elt[2][0] < 1.0 ) { + if ( elt[2][0] > -1.0 ) { + rfZAngle = atan2f(elt[1][0], elt[0][0]); + rfYAngle = asinf(-(double)elt[2][1]); + rfXAngle = atan2f(elt[2][1], elt[2][2]); + return true; + } else { + // WARNING. Not unique. ZA - XA = -atan2(r01,r02) + rfZAngle = -G3D::aTan2(elt[0][1], elt[0][2]); + rfYAngle = (float)halfPi(); + rfXAngle = 0.0f; + return false; + } + } else { + // WARNING. Not unique. ZA + XA = atan2(-r01,-r02) + rfZAngle = G3D::aTan2( -elt[0][1], -elt[0][2]); + rfYAngle = -(float)halfPi(); + rfXAngle = 0.0f; + return false; + } +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromEulerAnglesXYZ (float fYAngle, float fPAngle, + float fRAngle) { + float fCos, fSin; + + fCos = cosf(fYAngle); + fSin = sinf(fYAngle); + Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0, fSin, fCos); + + fCos = cosf(fPAngle); + fSin = sinf(fPAngle); + Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos); + + fCos = cosf(fRAngle); + fSin = sinf(fRAngle); + Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f); + + return kXMat * (kYMat * kZMat); +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromEulerAnglesXZY (float fYAngle, float fPAngle, + float fRAngle) { + + float fCos, fSin; + + fCos = cosf(fYAngle); + fSin = sinf(fYAngle); + Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos); + + fCos = cosf(fPAngle); + fSin = sinf(fPAngle); + Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0); + + fCos = cosf(fRAngle); + fSin = sinf(fRAngle); + Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos); + + return kXMat * (kZMat * kYMat); +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromEulerAnglesYXZ( + float fYAngle, + float fPAngle, + float fRAngle) { + + float fCos, fSin; + + fCos = cos(fYAngle); + fSin = sin(fYAngle); + Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos); + + fCos = cos(fPAngle); + fSin = sin(fPAngle); + Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos); + + fCos = cos(fRAngle); + fSin = sin(fRAngle); + Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f); + + return kYMat * (kXMat * kZMat); +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromEulerAnglesYZX( + float fYAngle, + float fPAngle, + float fRAngle) { + + float fCos, fSin; + + fCos = cos(fYAngle); + fSin = sin(fYAngle); + Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos); + + fCos = cos(fPAngle); + fSin = sin(fPAngle); + Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f); + + fCos = cos(fRAngle); + fSin = sin(fRAngle); + Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos); + + return kYMat * (kZMat * kXMat); +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromEulerAnglesZXY (float fYAngle, float fPAngle, + float fRAngle) { + float fCos, fSin; + + fCos = cos(fYAngle); + fSin = sin(fYAngle); + Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0); + + fCos = cos(fPAngle); + fSin = sin(fPAngle); + Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos); + + fCos = cos(fRAngle); + fSin = sin(fRAngle); + Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos); + + return kZMat * (kXMat * kYMat); +} + +//---------------------------------------------------------------------------- +Matrix3 Matrix3::fromEulerAnglesZYX (float fYAngle, float fPAngle, + float fRAngle) { + float fCos, fSin; + + fCos = cos(fYAngle); + fSin = sin(fYAngle); + Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0); + + fCos = cos(fPAngle); + fSin = sin(fPAngle); + Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos); + + fCos = cos(fRAngle); + fSin = sin(fRAngle); + Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos); + + return kZMat * (kYMat * kXMat); +} + +//---------------------------------------------------------------------------- +void Matrix3::tridiagonal (float afDiag[3], float afSubDiag[3]) { + // Householder reduction T = Q^t M Q + // Input: + // mat, symmetric 3x3 matrix M + // Output: + // mat, orthogonal matrix Q + // diag, diagonal entries of T + // subd, subdiagonal entries of T (T is symmetric) + + float fA = elt[0][0]; + float fB = elt[0][1]; + float fC = elt[0][2]; + float fD = elt[1][1]; + float fE = elt[1][2]; + float fF = elt[2][2]; + + afDiag[0] = fA; + afSubDiag[2] = 0.0; + + if ( G3D::abs(fC) >= EPSILON ) { + float fLength = sqrt(fB * fB + fC * fC); + float fInvLength = 1.0 / fLength; + fB *= fInvLength; + fC *= fInvLength; + float fQ = 2.0 * fB * fE + fC * (fF - fD); + afDiag[1] = fD + fC * fQ; + afDiag[2] = fF - fC * fQ; + afSubDiag[0] = fLength; + afSubDiag[1] = fE - fB * fQ; + elt[0][0] = 1.0; + elt[0][1] = 0.0; + elt[0][2] = 0.0; + elt[1][0] = 0.0; + elt[1][1] = fB; + elt[1][2] = fC; + elt[2][0] = 0.0; + elt[2][1] = fC; + elt[2][2] = -fB; + } else { + afDiag[1] = fD; + afDiag[2] = fF; + afSubDiag[0] = fB; + afSubDiag[1] = fE; + elt[0][0] = 1.0; + elt[0][1] = 0.0; + elt[0][2] = 0.0; + elt[1][0] = 0.0; + elt[1][1] = 1.0; + elt[1][2] = 0.0; + elt[2][0] = 0.0; + elt[2][1] = 0.0; + elt[2][2] = 1.0; + } +} + +//---------------------------------------------------------------------------- +bool Matrix3::qLAlgorithm (float afDiag[3], float afSubDiag[3]) { + // QL iteration with implicit shifting to reduce matrix from tridiagonal + // to diagonal + + for (int i0 = 0; i0 < 3; i0++) { + const int iMaxIter = 32; + int iIter; + + for (iIter = 0; iIter < iMaxIter; iIter++) { + int i1; + + for (i1 = i0; i1 <= 1; i1++) { + float fSum = G3D::abs(afDiag[i1]) + + G3D::abs(afDiag[i1 + 1]); + + if ( G3D::abs(afSubDiag[i1]) + fSum == fSum ) + break; + } + + if ( i1 == i0 ) + break; + + float fTmp0 = (afDiag[i0 + 1] - afDiag[i0]) / (2.0 * afSubDiag[i0]); + + float fTmp1 = sqrt(fTmp0 * fTmp0 + 1.0); + + if ( fTmp0 < 0.0 ) + fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 - fTmp1); + else + fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 + fTmp1); + + float fSin = 1.0; + + float fCos = 1.0; + + float fTmp2 = 0.0; + + for (int i2 = i1 - 1; i2 >= i0; i2--) { + float fTmp3 = fSin * afSubDiag[i2]; + float fTmp4 = fCos * afSubDiag[i2]; + + if (G3D::abs(fTmp3) >= G3D::abs(fTmp0)) { + fCos = fTmp0 / fTmp3; + fTmp1 = sqrt(fCos * fCos + 1.0); + afSubDiag[i2 + 1] = fTmp3 * fTmp1; + fSin = 1.0 / fTmp1; + fCos *= fSin; + } else { + fSin = fTmp3 / fTmp0; + fTmp1 = sqrt(fSin * fSin + 1.0); + afSubDiag[i2 + 1] = fTmp0 * fTmp1; + fCos = 1.0 / fTmp1; + fSin *= fCos; + } + + fTmp0 = afDiag[i2 + 1] - fTmp2; + fTmp1 = (afDiag[i2] - fTmp0) * fSin + 2.0 * fTmp4 * fCos; + fTmp2 = fSin * fTmp1; + afDiag[i2 + 1] = fTmp0 + fTmp2; + fTmp0 = fCos * fTmp1 - fTmp4; + + for (int iRow = 0; iRow < 3; iRow++) { + fTmp3 = elt[iRow][i2 + 1]; + elt[iRow][i2 + 1] = fSin * elt[iRow][i2] + + fCos * fTmp3; + elt[iRow][i2] = fCos * elt[iRow][i2] - + fSin * fTmp3; + } + } + + afDiag[i0] -= fTmp2; + afSubDiag[i0] = fTmp0; + afSubDiag[i1] = 0.0; + } + + if ( iIter == iMaxIter ) { + // should not get here under normal circumstances + return false; + } + } + + return true; +} + +//---------------------------------------------------------------------------- +void Matrix3::eigenSolveSymmetric (float afEigenvalue[3], + Vector3 akEigenvector[3]) const { + Matrix3 kMatrix = *this; + float afSubDiag[3]; + kMatrix.tridiagonal(afEigenvalue, afSubDiag); + kMatrix.qLAlgorithm(afEigenvalue, afSubDiag); + + for (int i = 0; i < 3; i++) { + akEigenvector[i][0] = kMatrix[0][i]; + akEigenvector[i][1] = kMatrix[1][i]; + akEigenvector[i][2] = kMatrix[2][i]; + } + + // make eigenvectors form a right--handed system + Vector3 kCross = akEigenvector[1].cross(akEigenvector[2]); + + float fDet = akEigenvector[0].dot(kCross); + + if ( fDet < 0.0 ) { + akEigenvector[2][0] = - akEigenvector[2][0]; + akEigenvector[2][1] = - akEigenvector[2][1]; + akEigenvector[2][2] = - akEigenvector[2][2]; + } +} + +//---------------------------------------------------------------------------- +void Matrix3::tensorProduct (const Vector3& rkU, const Vector3& rkV, + Matrix3& rkProduct) { + for (int iRow = 0; iRow < 3; iRow++) { + for (int iCol = 0; iCol < 3; iCol++) { + rkProduct[iRow][iCol] = rkU[iRow] * rkV[iCol]; + } + } +} + +//---------------------------------------------------------------------------- + +// Runs in 52 cycles on AMD, 76 cycles on Intel Centrino +// +// The loop unrolling is necessary for performance. +// I was unable to improve performance further by flattening the matrices +// into float*'s instead of 2D arrays. +// +// -morgan +void Matrix3::_mul(const Matrix3& A, const Matrix3& B, Matrix3& out) { + const float* ARowPtr = A.elt[0]; + float* outRowPtr = out.elt[0]; + outRowPtr[0] = + ARowPtr[0] * B.elt[0][0] + + ARowPtr[1] * B.elt[1][0] + + ARowPtr[2] * B.elt[2][0]; + outRowPtr[1] = + ARowPtr[0] * B.elt[0][1] + + ARowPtr[1] * B.elt[1][1] + + ARowPtr[2] * B.elt[2][1]; + outRowPtr[2] = + ARowPtr[0] * B.elt[0][2] + + ARowPtr[1] * B.elt[1][2] + + ARowPtr[2] * B.elt[2][2]; + + ARowPtr = A.elt[1]; + outRowPtr = out.elt[1]; + + outRowPtr[0] = + ARowPtr[0] * B.elt[0][0] + + ARowPtr[1] * B.elt[1][0] + + ARowPtr[2] * B.elt[2][0]; + outRowPtr[1] = + ARowPtr[0] * B.elt[0][1] + + ARowPtr[1] * B.elt[1][1] + + ARowPtr[2] * B.elt[2][1]; + outRowPtr[2] = + ARowPtr[0] * B.elt[0][2] + + ARowPtr[1] * B.elt[1][2] + + ARowPtr[2] * B.elt[2][2]; + + ARowPtr = A.elt[2]; + outRowPtr = out.elt[2]; + + outRowPtr[0] = + ARowPtr[0] * B.elt[0][0] + + ARowPtr[1] * B.elt[1][0] + + ARowPtr[2] * B.elt[2][0]; + outRowPtr[1] = + ARowPtr[0] * B.elt[0][1] + + ARowPtr[1] * B.elt[1][1] + + ARowPtr[2] * B.elt[2][1]; + outRowPtr[2] = + ARowPtr[0] * B.elt[0][2] + + ARowPtr[1] * B.elt[1][2] + + ARowPtr[2] * B.elt[2][2]; +} + +//---------------------------------------------------------------------------- +void Matrix3::_transpose(const Matrix3& A, Matrix3& out) { + out[0][0] = A.elt[0][0]; + out[0][1] = A.elt[1][0]; + out[0][2] = A.elt[2][0]; + out[1][0] = A.elt[0][1]; + out[1][1] = A.elt[1][1]; + out[1][2] = A.elt[2][1]; + out[2][0] = A.elt[0][2]; + out[2][1] = A.elt[1][2]; + out[2][2] = A.elt[2][2]; +} + +//----------------------------------------------------------------------------- +std::string Matrix3::toString() const { + return G3D::format("[%g, %g, %g; %g, %g, %g; %g, %g, %g]", + elt[0][0], elt[0][1], elt[0][2], + elt[1][0], elt[1][1], elt[1][2], + elt[2][0], elt[2][1], elt[2][2]); +} + + + +} // namespace + diff --git a/externals/g3dlite/Matrix4.cpp b/externals/g3dlite/Matrix4.cpp new file mode 100644 index 00000000000..cd38a1a3602 --- /dev/null +++ b/externals/g3dlite/Matrix4.cpp @@ -0,0 +1,523 @@ +/** + @file Matrix4.cpp + + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2003-10-02 + @edited 2010-01-29 + */ + +#include "G3D/platform.h" +#include "G3D/Matrix4.h" +#include "G3D/Matrix3.h" +#include "G3D/Vector4.h" +#include "G3D/Vector3.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/CoordinateFrame.h" +#include "G3D/Rect2D.h" +#include "G3D/Any.h" +#include "G3D/stringutils.h" + +namespace G3D { + + +Matrix4::Matrix4(const Any& any) { + any.verifyName("Matrix4"); + any.verifyType(Any::ARRAY); + + const std::string& name = toLower(any.name()); + if (name == "matrix4") { + any.verifySize(16); + + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + elt[r][c] = any[r * 4 + c]; + } + } + } else if (name == "matrix4::scale") { + if (any.size() == 1) { + *this = scale(any[0].number()); + } else if (any.size() == 3) { + *this = scale(any[0], any[1], any[2]); + } else { + any.verify(false, "Matrix4::scale() takes either 1 or 3 arguments"); + } + } else { + any.verify(false, "Expected Matrix4 constructor"); + } +} + + +Matrix4::operator Any() const { + Any any(Any::ARRAY, "Matrix4"); + any.resize(16); + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + any[r * 4 + c] = elt[r][c]; + } + } + + return any; +} + +const Matrix4& Matrix4::identity() { + static Matrix4 m( + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1); + return m; +} + + +const Matrix4& Matrix4::zero() { + static Matrix4 m( + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0); + return m; +} + + +Matrix4::Matrix4(const class CoordinateFrame& cframe) { + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + elt[r][c] = cframe.rotation[r][c]; + } + elt[r][3] = cframe.translation[r]; + } + elt[3][0] = 0.0f; + elt[3][1] = 0.0f; + elt[3][2] = 0.0f; + elt[3][3] = 1.0f; +} + +Matrix4::Matrix4(const Matrix3& upper3x3, const Vector3& lastCol) { + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + elt[r][c] = upper3x3[r][c]; + } + elt[r][3] = lastCol[r]; + } + elt[3][0] = 0.0f; + elt[3][1] = 0.0f; + elt[3][2] = 0.0f; + elt[3][3] = 1.0f; +} + + +Matrix3 Matrix4::upper3x3() const { + return Matrix3(elt[0][0], elt[0][1], elt[0][2], + elt[1][0], elt[1][1], elt[1][2], + elt[2][0], elt[2][1], elt[2][2]); +} + + +Matrix4 Matrix4::orthogonalProjection( + const class Rect2D& rect, + float nearval, + float farval, + float upDirection) { + return Matrix4::orthogonalProjection(rect.x0(), rect.x1(), rect.y1(), rect.y0(), nearval, farval, upDirection); +} + + +Matrix4 Matrix4::orthogonalProjection( + float left, + float right, + float bottom, + float top, + float nearval, + float farval, + float upDirection) { + + // Adapted from Mesa. Note that Microsoft (http://msdn.microsoft.com/library/default.asp?url=/library/en-us/opengl/glfunc03_8qnj.asp) + // and Linux (http://www.xfree86.org/current/glOrtho.3.html) have different matrices shown in their documentation. + + float x, y, z; + float tx, ty, tz; + + x = 2.0f / (right-left); + y = 2.0f / (top-bottom); + z = -2.0f / (farval-nearval); + tx = -(right+left) / (right-left); + ty = -(top+bottom) / (top-bottom); + tz = -(farval+nearval) / (farval-nearval); + + y *= upDirection; + ty *= upDirection; + + return + Matrix4( x , 0.0f, 0.0f, tx, + 0.0f, y , 0.0f, ty, + 0.0f, 0.0f, z , tz, + 0.0f, 0.0f, 0.0f, 1.0f); +} + + +Matrix4 Matrix4::perspectiveProjection( + float left, + float right, + float bottom, + float top, + float nearval, + float farval, + float upDirection) { + + float x, y, a, b, c, d; + + x = (2.0f*nearval) / (right-left); + y = (2.0f*nearval) / (top-bottom); + a = (right+left) / (right-left); + b = (top+bottom) / (top-bottom); + + if (farval >= finf()) { + // Infinite view frustum + c = -1.0f; + d = -2.0f * nearval; + } else { + c = -(farval+nearval) / (farval-nearval); + d = -(2.0f*farval*nearval) / (farval-nearval); + } + + debugAssertM(abs(upDirection) == 1.0f, "upDirection must be -1 or +1"); + y *= upDirection; + b *= upDirection; + + return Matrix4( + x, 0, a, 0, + 0, y, b, 0, + 0, 0, c, d, + 0, 0, -1, 0); +} + + +void Matrix4::getPerspectiveProjectionParameters( + float& left, + float& right, + float& bottom, + float& top, + float& nearval, + float& farval, + float upDirection) const { + + debugAssertM(abs(upDirection) == 1.0f, "upDirection must be -1 or +1"); + + float x = elt[0][0]; + float y = elt[1][1] * upDirection; + float a = elt[0][2]; + float b = elt[1][2] * upDirection; + float c = elt[2][2]; + float d = elt[2][3]; + + // Verify that this really is a projection matrix + debugAssertM(elt[3][2] == -1, "Not a projection matrix"); + debugAssertM(elt[0][1] == 0, "Not a projection matrix"); + debugAssertM(elt[0][3] == 0, "Not a projection matrix"); + debugAssertM(elt[1][3] == 0, "Not a projection matrix"); + debugAssertM(elt[3][3] == 0, "Not a projection matrix"); + debugAssertM(elt[1][0] == 0, "Not a projection matrix"); + debugAssertM(elt[2][0] == 0, "Not a projection matrix"); + debugAssertM(elt[2][1] == 0, "Not a projection matrix"); + debugAssertM(elt[3][0] == 0, "Not a projection matrix"); + debugAssertM(elt[3][1] == 0, "Not a projection matrix"); + + if (c == -1) { + farval = finf(); + nearval = -d / 2.0f; + } else { + nearval = d * ((c - 1.0f) / (c + 1.0f) - 1.0f) / (-2.0f * (c - 1.0f) / (c + 1.0f)); + farval = nearval * ((c - 1.0f) / (c + 1.0f)); + } + + + left = (a - 1.0f) * nearval / x; + right = 2.0f * nearval / x + left; + + bottom = (b - 1.0f) * nearval / y; + top = 2.0f * nearval / y + bottom; +} + + +Matrix4::Matrix4( + float r1c1, float r1c2, float r1c3, float r1c4, + float r2c1, float r2c2, float r2c3, float r2c4, + float r3c1, float r3c2, float r3c3, float r3c4, + float r4c1, float r4c2, float r4c3, float r4c4) { + elt[0][0] = r1c1; elt[0][1] = r1c2; elt[0][2] = r1c3; elt[0][3] = r1c4; + elt[1][0] = r2c1; elt[1][1] = r2c2; elt[1][2] = r2c3; elt[1][3] = r2c4; + elt[2][0] = r3c1; elt[2][1] = r3c2; elt[2][2] = r3c3; elt[2][3] = r3c4; + elt[3][0] = r4c1; elt[3][1] = r4c2; elt[3][2] = r4c3; elt[3][3] = r4c4; +} + +/** + init should be <B>row major</B>. + */ +Matrix4::Matrix4(const float* init) { + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + elt[r][c] = init[r * 4 + c]; + } + } +} + + +Matrix4::Matrix4(const double* init) { + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + elt[r][c] = (float)init[r * 4 + c]; + } + } +} + + +Matrix4::Matrix4() { + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + elt[r][c] = 0; + } + } +} + + +void Matrix4::setRow(int r, const Vector4& v) { + for (int c = 0; c < 4; ++c) { + elt[r][c] = v[c]; + } +} + + +void Matrix4::setColumn(int c, const Vector4& v) { + for (int r = 0; r < 4; ++r) { + elt[r][c] = v[r]; + } +} + + +const Vector4& Matrix4::row(int r) const { + return reinterpret_cast<const Vector4*>(elt[r])[0]; +} + + +Vector4 Matrix4::column(int c) const { + Vector4 v; + for (int r = 0; r < 4; ++r) { + v[r] = elt[r][c]; + } + return v; +} + + +Matrix4 Matrix4::operator*(const Matrix4& other) const { + Matrix4 result; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + for (int i = 0; i < 4; ++i) { + result.elt[r][c] += elt[r][i] * other.elt[i][c]; + } + } + } + + return result; +} + + +Matrix4 Matrix4::operator*(const float s) const { + Matrix4 result; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + result.elt[r][c] = elt[r][c] * s; + } + } + + return result; +} + + +Vector3 Matrix4::homoMul(const class Vector3& v, float w) const { + Vector4 r = (*this) * Vector4(v, w); + return r.xyz() * (1.0f / r.w); +} + + +Vector4 Matrix4::operator*(const Vector4& vector) const { + Vector4 result(0,0,0,0); + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + result[r] += elt[r][c] * vector[c]; + } + } + + return result; +} + + +Matrix4 Matrix4::transpose() const { + Matrix4 result; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + result.elt[c][r] = elt[r][c]; + } + } + + return result; +} + + +bool Matrix4::operator!=(const Matrix4& other) const { + return ! (*this == other); +} + + +bool Matrix4::operator==(const Matrix4& other) const { + + // If the bit patterns are identical, they must be + // the same matrix. If not, they *might* still have + // equal elements due to floating point weirdness. + if (memcmp(this, &other, sizeof(Matrix4) == 0)) { + return true; + } + + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + if (elt[r][c] != other.elt[r][c]) { + return false; + } + } + } + + return true; +} + + +float Matrix4::determinant() const { + // Determinant is the dot product of the first row and the first row + // of cofactors (i.e. the first col of the adjoint matrix) + return cofactor().row(0).dot(row(0)); +} + + +Matrix4 Matrix4::adjoint() const { + return cofactor().transpose(); +} + + +Matrix4 Matrix4::inverse() const { + // Inverse = adjoint / determinant + + Matrix4 A = adjoint(); + + // Determinant is the dot product of the first row and the first row + // of cofactors (i.e. the first col of the adjoint matrix) + float det = A.column(0).dot(row(0)); + + return A * (1.0f / det); +} + + +Matrix4 Matrix4::cofactor() const { + Matrix4 out; + + // We'll use i to incrementally compute -1 ^ (r+c) + int i = 1; + + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + // Compute the determinant of the 3x3 submatrix + float det = subDeterminant(r, c); + out.elt[r][c] = i * det; + i = -i; + } + i = -i; + } + + return out; +} + + +float Matrix4::subDeterminant(int excludeRow, int excludeCol) const { + // Compute non-excluded row and column indices + int row[3]; + int col[3]; + + for (int i = 0; i < 3; ++i) { + row[i] = i; + col[i] = i; + + if (i >= excludeRow) { + ++row[i]; + } + if (i >= excludeCol) { + ++col[i]; + } + } + + // Compute the first row of cofactors + float cofactor00 = + elt[row[1]][col[1]] * elt[row[2]][col[2]] - + elt[row[1]][col[2]] * elt[row[2]][col[1]]; + + float cofactor10 = + elt[row[1]][col[2]] * elt[row[2]][col[0]] - + elt[row[1]][col[0]] * elt[row[2]][col[2]]; + + float cofactor20 = + elt[row[1]][col[0]] * elt[row[2]][col[1]] - + elt[row[1]][col[1]] * elt[row[2]][col[0]]; + + // Product of the first row and the cofactors along the first row + return + elt[row[0]][col[0]] * cofactor00 + + elt[row[0]][col[1]] * cofactor10 + + elt[row[0]][col[2]] * cofactor20; +} + + +CoordinateFrame Matrix4::approxCoordinateFrame() const { + CoordinateFrame cframe; + + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + cframe.rotation[r][c] = elt[r][c]; + } + cframe.translation[r] = elt[r][3]; + } + + // Ensure that the rotation matrix is orthonormal + cframe.rotation.orthonormalize(); + + return cframe; +} + + +void Matrix4::serialize(class BinaryOutput& b) const { + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + b.writeFloat32(elt[r][c]); + } + } +} + + +void Matrix4::deserialize(class BinaryInput& b) { + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + elt[r][c] = b.readFloat32(); + } + } +} + +std::string Matrix4::toString() const { + return G3D::format("[%g, %g, %g, %g; %g, %g, %g, %g; %g, %g, %g, %g; %g, %g, %g, %g]", + elt[0][0], elt[0][1], elt[0][2], elt[0][3], + elt[1][0], elt[1][1], elt[1][2], elt[1][3], + elt[2][0], elt[2][1], elt[2][2], elt[2][3], + elt[3][0], elt[3][1], elt[3][2], elt[3][3]); +} + +} // namespace + + diff --git a/externals/g3dlite/MemoryManager.cpp b/externals/g3dlite/MemoryManager.cpp new file mode 100644 index 00000000000..240188a1f0e --- /dev/null +++ b/externals/g3dlite/MemoryManager.cpp @@ -0,0 +1,91 @@ +/** + @file MemoryManager.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + @created 2009-04-20 + @edited 2009-05-29 + + Copyright 2000-2009, Morgan McGuire. + All rights reserved. + */ + +#include "G3D/MemoryManager.h" +#include "G3D/System.h" + +namespace G3D { + +MemoryManager::MemoryManager() {} + + +void* MemoryManager::alloc(size_t s) { + return System::malloc(s); +} + + +void MemoryManager::free(void* ptr) { + System::free(ptr); +} + + +bool MemoryManager::isThreadsafe() const { + return true; +} + + +MemoryManager::Ref MemoryManager::create() { + static MemoryManager::Ref m = new MemoryManager(); + return m; +} + + +/////////////////////////////////////////////////// + +AlignedMemoryManager::AlignedMemoryManager() {} + + +void* AlignedMemoryManager::alloc(size_t s) { + return System::alignedMalloc(s, 16); +} + + +void AlignedMemoryManager::free(void* ptr) { + System::alignedFree(ptr); +} + + +bool AlignedMemoryManager::isThreadsafe() const { + return true; +} + + +AlignedMemoryManager::Ref AlignedMemoryManager::create() { + static AlignedMemoryManager::Ref m = new AlignedMemoryManager(); + return m; +} + + +/////////////////////////////////////////////////// + +CRTMemoryManager::CRTMemoryManager() {} + + +void* CRTMemoryManager::alloc(size_t s) { + return ::malloc(s); +} + + +void CRTMemoryManager::free(void* ptr) { + return ::free(ptr); +} + + +bool CRTMemoryManager::isThreadsafe() const { + return true; +} + + +CRTMemoryManager::Ref CRTMemoryManager::create() { + static CRTMemoryManager::Ref m = new CRTMemoryManager(); + return m; +} +} diff --git a/externals/g3dlite/Plane.cpp b/externals/g3dlite/Plane.cpp new file mode 100644 index 00000000000..9b7991c0333 --- /dev/null +++ b/externals/g3dlite/Plane.cpp @@ -0,0 +1,149 @@ +/** + @file Plane.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2003-02-06 + @edited 2006-01-29 + */ + +#include "G3D/platform.h" +#include "G3D/Plane.h" +#include "G3D/BinaryOutput.h" +#include "G3D/BinaryInput.h" +#include "G3D/stringutils.h" + +namespace G3D { + +Plane::Plane(class BinaryInput& b) { + deserialize(b); +} + + +void Plane::serialize(class BinaryOutput& b) const { + _normal.serialize(b); + b.writeFloat64(_distance); +} + + +void Plane::deserialize(class BinaryInput& b) { + _normal.deserialize(b); + _distance = (float)b.readFloat64(); +} + + +Plane::Plane( + Vector4 point0, + Vector4 point1, + Vector4 point2) { + + debugAssertM( + point0.w != 0 || + point1.w != 0 || + point2.w != 0, + "At least one point must be finite."); + + // Rotate the points around so that the finite points come first. + + while ((point0.w == 0) && + ((point1.w == 0) || (point2.w != 0))) { + Vector4 temp = point0; + point0 = point1; + point1 = point2; + point2 = temp; + } + + Vector3 dir1; + Vector3 dir2; + + if (point1.w == 0) { + // 1 finite, 2 infinite points; the plane must contain + // the direction of the two direcitons + dir1 = point1.xyz(); + dir2 = point2.xyz(); + } else if (point2.w != 0) { + // 3 finite points, the plane must contain the directions + // betwseen the points. + dir1 = point1.xyz() - point0.xyz(); + dir2 = point2.xyz() - point0.xyz(); + } else { + // 2 finite, 1 infinite point; the plane must contain + // the direction between the first two points and the + // direction of the third point. + dir1 = point1.xyz() - point0.xyz(); + dir2 = point2.xyz(); + } + + _normal = dir1.cross(dir2).direction(); + _distance = _normal.dot(point0.xyz()); +} + + +Plane::Plane( + const Vector3& point0, + const Vector3& point1, + const Vector3& point2) { + + _normal = (point1 - point0).cross(point2 - point0).direction(); + _distance = _normal.dot(point0); +} + + +Plane::Plane( + const Vector3& __normal, + const Vector3& point) { + + _normal = __normal.direction(); + _distance = _normal.dot(point); +} + + +Plane Plane::fromEquation(float a, float b, float c, float d) { + Vector3 n(a, b, c); + float magnitude = n.magnitude(); + d /= magnitude; + n /= magnitude; + return Plane(n, -d); +} + + +void Plane::flip() { + _normal = -_normal; + _distance = -_distance; +} + + +void Plane::getEquation(Vector3& n, float& d) const { + double _d; + getEquation(n, _d); + d = (float)_d; +} + +void Plane::getEquation(Vector3& n, double& d) const { + n = _normal; + d = -_distance; +} + + +void Plane::getEquation(float& a, float& b, float& c, float& d) const { + double _a, _b, _c, _d; + getEquation(_a, _b, _c, _d); + a = (float)_a; + b = (float)_b; + c = (float)_c; + d = (float)_d; +} + +void Plane::getEquation(double& a, double& b, double& c, double& d) const { + a = _normal.x; + b = _normal.y; + c = _normal.z; + d = -_distance; +} + + +std::string Plane::toString() const { + return format("Plane(%g, %g, %g, %g)", _normal.x, _normal.y, _normal.z, _distance); +} + +} diff --git a/externals/g3dlite/Quat.cpp b/externals/g3dlite/Quat.cpp new file mode 100644 index 00000000000..225c5b51acc --- /dev/null +++ b/externals/g3dlite/Quat.cpp @@ -0,0 +1,583 @@ +/** + @file Quat.cpp + + Quaternion implementation based on Watt & Watt page 363 + + @author Morgan McGuire, graphics3d.com + + @created 2002-01-23 + @edited 2006-01-31 + */ + +#include "G3D/Quat.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" + +namespace G3D { + +Quat Quat::fromAxisAngleRotation( + const Vector3& axis, + float angle) { + + Quat q; + + q.w = cos(angle / 2.0f); + q.imag() = axis.direction() * sin(angle / 2.0f); + + return q; +} + + +Quat::Quat( + const Matrix3& rot) { + + static const int plus1mod3[] = {1, 2, 0}; + + // Find the index of the largest diagonal component + // These ? operations hopefully compile to conditional + // move instructions instead of branches. + int i = (rot[1][1] > rot[0][0]) ? 1 : 0; + i = (rot[2][2] > rot[i][i]) ? 2 : i; + + // Find the indices of the other elements + int j = plus1mod3[i]; + int k = plus1mod3[j]; + + // Index the elements of the vector part of the quaternion as a float* + float* v = (float*)(this); + + // If we attempted to pre-normalize and trusted the matrix to be + // perfectly orthonormal, the result would be: + // + // double c = sqrt((rot[i][i] - (rot[j][j] + rot[k][k])) + 1.0) + // v[i] = -c * 0.5 + // v[j] = -(rot[i][j] + rot[j][i]) * 0.5 / c + // v[k] = -(rot[i][k] + rot[k][i]) * 0.5 / c + // w = (rot[j][k] - rot[k][j]) * 0.5 / c + // + // Since we're going to pay the sqrt anyway, we perform a post normalization, which also + // fixes any poorly normalized input. Multiply all elements by 2*c in the above, giving: + + // nc2 = -c^2 + double nc2 = ((rot[j][j] + rot[k][k]) - rot[i][i]) - 1.0; + v[i] = nc2; + w = (rot[j][k] - rot[k][j]); + v[j] = -(rot[i][j] + rot[j][i]); + v[k] = -(rot[i][k] + rot[k][i]); + + // We now have the correct result with the wrong magnitude, so normalize it: + float s = sqrt(x*x + y*y + z*z + w*w); + if (s > 0.00001f) { + s = 1.0f / s; + x *= s; + y *= s; + z *= s; + w *= s; + } else { + // The quaternion is nearly zero. Make it 0 0 0 1 + x = 0.0f; + y = 0.0f; + z = 0.0f; + w = 1.0f; + } +} + + +void Quat::toAxisAngleRotation( + Vector3& axis, + double& angle) const { + + // Decompose the quaternion into an angle and an axis. + + axis = Vector3(x, y, z); + angle = 2 * acos(w); + + float len = sqrt(1.0f - w * w); + + if (fuzzyGt(abs(len), 0.0f)) { + axis /= len; + } + + // Reduce the range of the angle. + + if (angle < 0) { + angle = -angle; + axis = -axis; + } + + while (angle > twoPi()) { + angle -= twoPi(); + } + + if (abs(angle) > pi()) { + angle -= twoPi(); + } + + // Make the angle positive. + + if (angle < 0.0f) { + angle = -angle; + axis = -axis; + } +} + + +Matrix3 Quat::toRotationMatrix() const { + Matrix3 out = Matrix3::zero(); + + toRotationMatrix(out); + + return out; +} + + +void Quat::toRotationMatrix( + Matrix3& rot) const { + + rot = Matrix3(*this); +} + + +Quat Quat::slerp( + const Quat& _quat1, + float alpha, + float threshold) const { + + // From: Game Physics -- David Eberly pg 538-540 + // Modified to include lerp for small angles, which + // is a common practice. + + // See also: + // http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/index.html + + const Quat& quat0 = *this; + Quat quat1 = _quat1; + + // angle between quaternion rotations + float phi; + float cosphi = quat0.dot(quat1); + + + if (cosphi < 0) { + // Change the sign and fix the dot product; we need to + // loop the other way to get the shortest path + quat1 = -quat1; + cosphi = -cosphi; + } + + // Using G3D::aCos will clamp the angle to 0 and pi + phi = static_cast<float>(G3D::aCos(cosphi)); + + if (phi >= threshold) { + // For large angles, slerp + float scale0, scale1; + + scale0 = sin((1.0f - alpha) * phi); + scale1 = sin(alpha * phi); + + return ( (quat0 * scale0) + (quat1 * scale1) ) / sin(phi); + } else { + // For small angles, linear interpolate + return quat0.nlerp(quat1, alpha); + } +} + + +Quat Quat::nlerp( + const Quat& quat1, + float alpha) const { + + Quat result = (*this) * (1.0f - alpha) + quat1 * alpha; + return result / result.magnitude(); +} + + +Quat Quat::operator*(const Quat& other) const { + + // Following Watt & Watt, page 360 + const Vector3& v1 = imag(); + const Vector3& v2 = other.imag(); + float s1 = w; + float s2 = other.w; + + return Quat(s1*v2 + s2*v1 + v1.cross(v2), s1*s2 - v1.dot(v2)); +} + + +// From "Uniform Random Rotations", Ken Shoemake, Graphics Gems III. +Quat Quat::unitRandom() { + float x0 = uniformRandom(); + float r1 = sqrtf(1 - x0), + r2 = sqrtf(x0); + float t1 = (float)G3D::twoPi() * uniformRandom(); + float t2 = (float)G3D::twoPi() * uniformRandom(); + float c1 = cosf(t1), + s1 = sinf(t1); + float c2 = cosf(t2), + s2 = sinf(t2); + return Quat(s1 * r1, c1 * r1, s2 * r2, c2 * r2); +} + + +void Quat::deserialize(class BinaryInput& b) { + x = b.readFloat32(); + y = b.readFloat32(); + z = b.readFloat32(); + w = b.readFloat32(); +} + + +void Quat::serialize(class BinaryOutput& b) const { + b.writeFloat32(x); + b.writeFloat32(y); + b.writeFloat32(z); + b.writeFloat32(w); +} + + +// 2-char swizzles + +Vector2 Quat::xx() const { return Vector2 (x, x); } +Vector2 Quat::yx() const { return Vector2 (y, x); } +Vector2 Quat::zx() const { return Vector2 (z, x); } +Vector2 Quat::wx() const { return Vector2 (w, x); } +Vector2 Quat::xy() const { return Vector2 (x, y); } +Vector2 Quat::yy() const { return Vector2 (y, y); } +Vector2 Quat::zy() const { return Vector2 (z, y); } +Vector2 Quat::wy() const { return Vector2 (w, y); } +Vector2 Quat::xz() const { return Vector2 (x, z); } +Vector2 Quat::yz() const { return Vector2 (y, z); } +Vector2 Quat::zz() const { return Vector2 (z, z); } +Vector2 Quat::wz() const { return Vector2 (w, z); } +Vector2 Quat::xw() const { return Vector2 (x, w); } +Vector2 Quat::yw() const { return Vector2 (y, w); } +Vector2 Quat::zw() const { return Vector2 (z, w); } +Vector2 Quat::ww() const { return Vector2 (w, w); } + +// 3-char swizzles + +Vector3 Quat::xxx() const { return Vector3 (x, x, x); } +Vector3 Quat::yxx() const { return Vector3 (y, x, x); } +Vector3 Quat::zxx() const { return Vector3 (z, x, x); } +Vector3 Quat::wxx() const { return Vector3 (w, x, x); } +Vector3 Quat::xyx() const { return Vector3 (x, y, x); } +Vector3 Quat::yyx() const { return Vector3 (y, y, x); } +Vector3 Quat::zyx() const { return Vector3 (z, y, x); } +Vector3 Quat::wyx() const { return Vector3 (w, y, x); } +Vector3 Quat::xzx() const { return Vector3 (x, z, x); } +Vector3 Quat::yzx() const { return Vector3 (y, z, x); } +Vector3 Quat::zzx() const { return Vector3 (z, z, x); } +Vector3 Quat::wzx() const { return Vector3 (w, z, x); } +Vector3 Quat::xwx() const { return Vector3 (x, w, x); } +Vector3 Quat::ywx() const { return Vector3 (y, w, x); } +Vector3 Quat::zwx() const { return Vector3 (z, w, x); } +Vector3 Quat::wwx() const { return Vector3 (w, w, x); } +Vector3 Quat::xxy() const { return Vector3 (x, x, y); } +Vector3 Quat::yxy() const { return Vector3 (y, x, y); } +Vector3 Quat::zxy() const { return Vector3 (z, x, y); } +Vector3 Quat::wxy() const { return Vector3 (w, x, y); } +Vector3 Quat::xyy() const { return Vector3 (x, y, y); } +Vector3 Quat::yyy() const { return Vector3 (y, y, y); } +Vector3 Quat::zyy() const { return Vector3 (z, y, y); } +Vector3 Quat::wyy() const { return Vector3 (w, y, y); } +Vector3 Quat::xzy() const { return Vector3 (x, z, y); } +Vector3 Quat::yzy() const { return Vector3 (y, z, y); } +Vector3 Quat::zzy() const { return Vector3 (z, z, y); } +Vector3 Quat::wzy() const { return Vector3 (w, z, y); } +Vector3 Quat::xwy() const { return Vector3 (x, w, y); } +Vector3 Quat::ywy() const { return Vector3 (y, w, y); } +Vector3 Quat::zwy() const { return Vector3 (z, w, y); } +Vector3 Quat::wwy() const { return Vector3 (w, w, y); } +Vector3 Quat::xxz() const { return Vector3 (x, x, z); } +Vector3 Quat::yxz() const { return Vector3 (y, x, z); } +Vector3 Quat::zxz() const { return Vector3 (z, x, z); } +Vector3 Quat::wxz() const { return Vector3 (w, x, z); } +Vector3 Quat::xyz() const { return Vector3 (x, y, z); } +Vector3 Quat::yyz() const { return Vector3 (y, y, z); } +Vector3 Quat::zyz() const { return Vector3 (z, y, z); } +Vector3 Quat::wyz() const { return Vector3 (w, y, z); } +Vector3 Quat::xzz() const { return Vector3 (x, z, z); } +Vector3 Quat::yzz() const { return Vector3 (y, z, z); } +Vector3 Quat::zzz() const { return Vector3 (z, z, z); } +Vector3 Quat::wzz() const { return Vector3 (w, z, z); } +Vector3 Quat::xwz() const { return Vector3 (x, w, z); } +Vector3 Quat::ywz() const { return Vector3 (y, w, z); } +Vector3 Quat::zwz() const { return Vector3 (z, w, z); } +Vector3 Quat::wwz() const { return Vector3 (w, w, z); } +Vector3 Quat::xxw() const { return Vector3 (x, x, w); } +Vector3 Quat::yxw() const { return Vector3 (y, x, w); } +Vector3 Quat::zxw() const { return Vector3 (z, x, w); } +Vector3 Quat::wxw() const { return Vector3 (w, x, w); } +Vector3 Quat::xyw() const { return Vector3 (x, y, w); } +Vector3 Quat::yyw() const { return Vector3 (y, y, w); } +Vector3 Quat::zyw() const { return Vector3 (z, y, w); } +Vector3 Quat::wyw() const { return Vector3 (w, y, w); } +Vector3 Quat::xzw() const { return Vector3 (x, z, w); } +Vector3 Quat::yzw() const { return Vector3 (y, z, w); } +Vector3 Quat::zzw() const { return Vector3 (z, z, w); } +Vector3 Quat::wzw() const { return Vector3 (w, z, w); } +Vector3 Quat::xww() const { return Vector3 (x, w, w); } +Vector3 Quat::yww() const { return Vector3 (y, w, w); } +Vector3 Quat::zww() const { return Vector3 (z, w, w); } +Vector3 Quat::www() const { return Vector3 (w, w, w); } + +// 4-char swizzles + +Vector4 Quat::xxxx() const { return Vector4 (x, x, x, x); } +Vector4 Quat::yxxx() const { return Vector4 (y, x, x, x); } +Vector4 Quat::zxxx() const { return Vector4 (z, x, x, x); } +Vector4 Quat::wxxx() const { return Vector4 (w, x, x, x); } +Vector4 Quat::xyxx() const { return Vector4 (x, y, x, x); } +Vector4 Quat::yyxx() const { return Vector4 (y, y, x, x); } +Vector4 Quat::zyxx() const { return Vector4 (z, y, x, x); } +Vector4 Quat::wyxx() const { return Vector4 (w, y, x, x); } +Vector4 Quat::xzxx() const { return Vector4 (x, z, x, x); } +Vector4 Quat::yzxx() const { return Vector4 (y, z, x, x); } +Vector4 Quat::zzxx() const { return Vector4 (z, z, x, x); } +Vector4 Quat::wzxx() const { return Vector4 (w, z, x, x); } +Vector4 Quat::xwxx() const { return Vector4 (x, w, x, x); } +Vector4 Quat::ywxx() const { return Vector4 (y, w, x, x); } +Vector4 Quat::zwxx() const { return Vector4 (z, w, x, x); } +Vector4 Quat::wwxx() const { return Vector4 (w, w, x, x); } +Vector4 Quat::xxyx() const { return Vector4 (x, x, y, x); } +Vector4 Quat::yxyx() const { return Vector4 (y, x, y, x); } +Vector4 Quat::zxyx() const { return Vector4 (z, x, y, x); } +Vector4 Quat::wxyx() const { return Vector4 (w, x, y, x); } +Vector4 Quat::xyyx() const { return Vector4 (x, y, y, x); } +Vector4 Quat::yyyx() const { return Vector4 (y, y, y, x); } +Vector4 Quat::zyyx() const { return Vector4 (z, y, y, x); } +Vector4 Quat::wyyx() const { return Vector4 (w, y, y, x); } +Vector4 Quat::xzyx() const { return Vector4 (x, z, y, x); } +Vector4 Quat::yzyx() const { return Vector4 (y, z, y, x); } +Vector4 Quat::zzyx() const { return Vector4 (z, z, y, x); } +Vector4 Quat::wzyx() const { return Vector4 (w, z, y, x); } +Vector4 Quat::xwyx() const { return Vector4 (x, w, y, x); } +Vector4 Quat::ywyx() const { return Vector4 (y, w, y, x); } +Vector4 Quat::zwyx() const { return Vector4 (z, w, y, x); } +Vector4 Quat::wwyx() const { return Vector4 (w, w, y, x); } +Vector4 Quat::xxzx() const { return Vector4 (x, x, z, x); } +Vector4 Quat::yxzx() const { return Vector4 (y, x, z, x); } +Vector4 Quat::zxzx() const { return Vector4 (z, x, z, x); } +Vector4 Quat::wxzx() const { return Vector4 (w, x, z, x); } +Vector4 Quat::xyzx() const { return Vector4 (x, y, z, x); } +Vector4 Quat::yyzx() const { return Vector4 (y, y, z, x); } +Vector4 Quat::zyzx() const { return Vector4 (z, y, z, x); } +Vector4 Quat::wyzx() const { return Vector4 (w, y, z, x); } +Vector4 Quat::xzzx() const { return Vector4 (x, z, z, x); } +Vector4 Quat::yzzx() const { return Vector4 (y, z, z, x); } +Vector4 Quat::zzzx() const { return Vector4 (z, z, z, x); } +Vector4 Quat::wzzx() const { return Vector4 (w, z, z, x); } +Vector4 Quat::xwzx() const { return Vector4 (x, w, z, x); } +Vector4 Quat::ywzx() const { return Vector4 (y, w, z, x); } +Vector4 Quat::zwzx() const { return Vector4 (z, w, z, x); } +Vector4 Quat::wwzx() const { return Vector4 (w, w, z, x); } +Vector4 Quat::xxwx() const { return Vector4 (x, x, w, x); } +Vector4 Quat::yxwx() const { return Vector4 (y, x, w, x); } +Vector4 Quat::zxwx() const { return Vector4 (z, x, w, x); } +Vector4 Quat::wxwx() const { return Vector4 (w, x, w, x); } +Vector4 Quat::xywx() const { return Vector4 (x, y, w, x); } +Vector4 Quat::yywx() const { return Vector4 (y, y, w, x); } +Vector4 Quat::zywx() const { return Vector4 (z, y, w, x); } +Vector4 Quat::wywx() const { return Vector4 (w, y, w, x); } +Vector4 Quat::xzwx() const { return Vector4 (x, z, w, x); } +Vector4 Quat::yzwx() const { return Vector4 (y, z, w, x); } +Vector4 Quat::zzwx() const { return Vector4 (z, z, w, x); } +Vector4 Quat::wzwx() const { return Vector4 (w, z, w, x); } +Vector4 Quat::xwwx() const { return Vector4 (x, w, w, x); } +Vector4 Quat::ywwx() const { return Vector4 (y, w, w, x); } +Vector4 Quat::zwwx() const { return Vector4 (z, w, w, x); } +Vector4 Quat::wwwx() const { return Vector4 (w, w, w, x); } +Vector4 Quat::xxxy() const { return Vector4 (x, x, x, y); } +Vector4 Quat::yxxy() const { return Vector4 (y, x, x, y); } +Vector4 Quat::zxxy() const { return Vector4 (z, x, x, y); } +Vector4 Quat::wxxy() const { return Vector4 (w, x, x, y); } +Vector4 Quat::xyxy() const { return Vector4 (x, y, x, y); } +Vector4 Quat::yyxy() const { return Vector4 (y, y, x, y); } +Vector4 Quat::zyxy() const { return Vector4 (z, y, x, y); } +Vector4 Quat::wyxy() const { return Vector4 (w, y, x, y); } +Vector4 Quat::xzxy() const { return Vector4 (x, z, x, y); } +Vector4 Quat::yzxy() const { return Vector4 (y, z, x, y); } +Vector4 Quat::zzxy() const { return Vector4 (z, z, x, y); } +Vector4 Quat::wzxy() const { return Vector4 (w, z, x, y); } +Vector4 Quat::xwxy() const { return Vector4 (x, w, x, y); } +Vector4 Quat::ywxy() const { return Vector4 (y, w, x, y); } +Vector4 Quat::zwxy() const { return Vector4 (z, w, x, y); } +Vector4 Quat::wwxy() const { return Vector4 (w, w, x, y); } +Vector4 Quat::xxyy() const { return Vector4 (x, x, y, y); } +Vector4 Quat::yxyy() const { return Vector4 (y, x, y, y); } +Vector4 Quat::zxyy() const { return Vector4 (z, x, y, y); } +Vector4 Quat::wxyy() const { return Vector4 (w, x, y, y); } +Vector4 Quat::xyyy() const { return Vector4 (x, y, y, y); } +Vector4 Quat::yyyy() const { return Vector4 (y, y, y, y); } +Vector4 Quat::zyyy() const { return Vector4 (z, y, y, y); } +Vector4 Quat::wyyy() const { return Vector4 (w, y, y, y); } +Vector4 Quat::xzyy() const { return Vector4 (x, z, y, y); } +Vector4 Quat::yzyy() const { return Vector4 (y, z, y, y); } +Vector4 Quat::zzyy() const { return Vector4 (z, z, y, y); } +Vector4 Quat::wzyy() const { return Vector4 (w, z, y, y); } +Vector4 Quat::xwyy() const { return Vector4 (x, w, y, y); } +Vector4 Quat::ywyy() const { return Vector4 (y, w, y, y); } +Vector4 Quat::zwyy() const { return Vector4 (z, w, y, y); } +Vector4 Quat::wwyy() const { return Vector4 (w, w, y, y); } +Vector4 Quat::xxzy() const { return Vector4 (x, x, z, y); } +Vector4 Quat::yxzy() const { return Vector4 (y, x, z, y); } +Vector4 Quat::zxzy() const { return Vector4 (z, x, z, y); } +Vector4 Quat::wxzy() const { return Vector4 (w, x, z, y); } +Vector4 Quat::xyzy() const { return Vector4 (x, y, z, y); } +Vector4 Quat::yyzy() const { return Vector4 (y, y, z, y); } +Vector4 Quat::zyzy() const { return Vector4 (z, y, z, y); } +Vector4 Quat::wyzy() const { return Vector4 (w, y, z, y); } +Vector4 Quat::xzzy() const { return Vector4 (x, z, z, y); } +Vector4 Quat::yzzy() const { return Vector4 (y, z, z, y); } +Vector4 Quat::zzzy() const { return Vector4 (z, z, z, y); } +Vector4 Quat::wzzy() const { return Vector4 (w, z, z, y); } +Vector4 Quat::xwzy() const { return Vector4 (x, w, z, y); } +Vector4 Quat::ywzy() const { return Vector4 (y, w, z, y); } +Vector4 Quat::zwzy() const { return Vector4 (z, w, z, y); } +Vector4 Quat::wwzy() const { return Vector4 (w, w, z, y); } +Vector4 Quat::xxwy() const { return Vector4 (x, x, w, y); } +Vector4 Quat::yxwy() const { return Vector4 (y, x, w, y); } +Vector4 Quat::zxwy() const { return Vector4 (z, x, w, y); } +Vector4 Quat::wxwy() const { return Vector4 (w, x, w, y); } +Vector4 Quat::xywy() const { return Vector4 (x, y, w, y); } +Vector4 Quat::yywy() const { return Vector4 (y, y, w, y); } +Vector4 Quat::zywy() const { return Vector4 (z, y, w, y); } +Vector4 Quat::wywy() const { return Vector4 (w, y, w, y); } +Vector4 Quat::xzwy() const { return Vector4 (x, z, w, y); } +Vector4 Quat::yzwy() const { return Vector4 (y, z, w, y); } +Vector4 Quat::zzwy() const { return Vector4 (z, z, w, y); } +Vector4 Quat::wzwy() const { return Vector4 (w, z, w, y); } +Vector4 Quat::xwwy() const { return Vector4 (x, w, w, y); } +Vector4 Quat::ywwy() const { return Vector4 (y, w, w, y); } +Vector4 Quat::zwwy() const { return Vector4 (z, w, w, y); } +Vector4 Quat::wwwy() const { return Vector4 (w, w, w, y); } +Vector4 Quat::xxxz() const { return Vector4 (x, x, x, z); } +Vector4 Quat::yxxz() const { return Vector4 (y, x, x, z); } +Vector4 Quat::zxxz() const { return Vector4 (z, x, x, z); } +Vector4 Quat::wxxz() const { return Vector4 (w, x, x, z); } +Vector4 Quat::xyxz() const { return Vector4 (x, y, x, z); } +Vector4 Quat::yyxz() const { return Vector4 (y, y, x, z); } +Vector4 Quat::zyxz() const { return Vector4 (z, y, x, z); } +Vector4 Quat::wyxz() const { return Vector4 (w, y, x, z); } +Vector4 Quat::xzxz() const { return Vector4 (x, z, x, z); } +Vector4 Quat::yzxz() const { return Vector4 (y, z, x, z); } +Vector4 Quat::zzxz() const { return Vector4 (z, z, x, z); } +Vector4 Quat::wzxz() const { return Vector4 (w, z, x, z); } +Vector4 Quat::xwxz() const { return Vector4 (x, w, x, z); } +Vector4 Quat::ywxz() const { return Vector4 (y, w, x, z); } +Vector4 Quat::zwxz() const { return Vector4 (z, w, x, z); } +Vector4 Quat::wwxz() const { return Vector4 (w, w, x, z); } +Vector4 Quat::xxyz() const { return Vector4 (x, x, y, z); } +Vector4 Quat::yxyz() const { return Vector4 (y, x, y, z); } +Vector4 Quat::zxyz() const { return Vector4 (z, x, y, z); } +Vector4 Quat::wxyz() const { return Vector4 (w, x, y, z); } +Vector4 Quat::xyyz() const { return Vector4 (x, y, y, z); } +Vector4 Quat::yyyz() const { return Vector4 (y, y, y, z); } +Vector4 Quat::zyyz() const { return Vector4 (z, y, y, z); } +Vector4 Quat::wyyz() const { return Vector4 (w, y, y, z); } +Vector4 Quat::xzyz() const { return Vector4 (x, z, y, z); } +Vector4 Quat::yzyz() const { return Vector4 (y, z, y, z); } +Vector4 Quat::zzyz() const { return Vector4 (z, z, y, z); } +Vector4 Quat::wzyz() const { return Vector4 (w, z, y, z); } +Vector4 Quat::xwyz() const { return Vector4 (x, w, y, z); } +Vector4 Quat::ywyz() const { return Vector4 (y, w, y, z); } +Vector4 Quat::zwyz() const { return Vector4 (z, w, y, z); } +Vector4 Quat::wwyz() const { return Vector4 (w, w, y, z); } +Vector4 Quat::xxzz() const { return Vector4 (x, x, z, z); } +Vector4 Quat::yxzz() const { return Vector4 (y, x, z, z); } +Vector4 Quat::zxzz() const { return Vector4 (z, x, z, z); } +Vector4 Quat::wxzz() const { return Vector4 (w, x, z, z); } +Vector4 Quat::xyzz() const { return Vector4 (x, y, z, z); } +Vector4 Quat::yyzz() const { return Vector4 (y, y, z, z); } +Vector4 Quat::zyzz() const { return Vector4 (z, y, z, z); } +Vector4 Quat::wyzz() const { return Vector4 (w, y, z, z); } +Vector4 Quat::xzzz() const { return Vector4 (x, z, z, z); } +Vector4 Quat::yzzz() const { return Vector4 (y, z, z, z); } +Vector4 Quat::zzzz() const { return Vector4 (z, z, z, z); } +Vector4 Quat::wzzz() const { return Vector4 (w, z, z, z); } +Vector4 Quat::xwzz() const { return Vector4 (x, w, z, z); } +Vector4 Quat::ywzz() const { return Vector4 (y, w, z, z); } +Vector4 Quat::zwzz() const { return Vector4 (z, w, z, z); } +Vector4 Quat::wwzz() const { return Vector4 (w, w, z, z); } +Vector4 Quat::xxwz() const { return Vector4 (x, x, w, z); } +Vector4 Quat::yxwz() const { return Vector4 (y, x, w, z); } +Vector4 Quat::zxwz() const { return Vector4 (z, x, w, z); } +Vector4 Quat::wxwz() const { return Vector4 (w, x, w, z); } +Vector4 Quat::xywz() const { return Vector4 (x, y, w, z); } +Vector4 Quat::yywz() const { return Vector4 (y, y, w, z); } +Vector4 Quat::zywz() const { return Vector4 (z, y, w, z); } +Vector4 Quat::wywz() const { return Vector4 (w, y, w, z); } +Vector4 Quat::xzwz() const { return Vector4 (x, z, w, z); } +Vector4 Quat::yzwz() const { return Vector4 (y, z, w, z); } +Vector4 Quat::zzwz() const { return Vector4 (z, z, w, z); } +Vector4 Quat::wzwz() const { return Vector4 (w, z, w, z); } +Vector4 Quat::xwwz() const { return Vector4 (x, w, w, z); } +Vector4 Quat::ywwz() const { return Vector4 (y, w, w, z); } +Vector4 Quat::zwwz() const { return Vector4 (z, w, w, z); } +Vector4 Quat::wwwz() const { return Vector4 (w, w, w, z); } +Vector4 Quat::xxxw() const { return Vector4 (x, x, x, w); } +Vector4 Quat::yxxw() const { return Vector4 (y, x, x, w); } +Vector4 Quat::zxxw() const { return Vector4 (z, x, x, w); } +Vector4 Quat::wxxw() const { return Vector4 (w, x, x, w); } +Vector4 Quat::xyxw() const { return Vector4 (x, y, x, w); } +Vector4 Quat::yyxw() const { return Vector4 (y, y, x, w); } +Vector4 Quat::zyxw() const { return Vector4 (z, y, x, w); } +Vector4 Quat::wyxw() const { return Vector4 (w, y, x, w); } +Vector4 Quat::xzxw() const { return Vector4 (x, z, x, w); } +Vector4 Quat::yzxw() const { return Vector4 (y, z, x, w); } +Vector4 Quat::zzxw() const { return Vector4 (z, z, x, w); } +Vector4 Quat::wzxw() const { return Vector4 (w, z, x, w); } +Vector4 Quat::xwxw() const { return Vector4 (x, w, x, w); } +Vector4 Quat::ywxw() const { return Vector4 (y, w, x, w); } +Vector4 Quat::zwxw() const { return Vector4 (z, w, x, w); } +Vector4 Quat::wwxw() const { return Vector4 (w, w, x, w); } +Vector4 Quat::xxyw() const { return Vector4 (x, x, y, w); } +Vector4 Quat::yxyw() const { return Vector4 (y, x, y, w); } +Vector4 Quat::zxyw() const { return Vector4 (z, x, y, w); } +Vector4 Quat::wxyw() const { return Vector4 (w, x, y, w); } +Vector4 Quat::xyyw() const { return Vector4 (x, y, y, w); } +Vector4 Quat::yyyw() const { return Vector4 (y, y, y, w); } +Vector4 Quat::zyyw() const { return Vector4 (z, y, y, w); } +Vector4 Quat::wyyw() const { return Vector4 (w, y, y, w); } +Vector4 Quat::xzyw() const { return Vector4 (x, z, y, w); } +Vector4 Quat::yzyw() const { return Vector4 (y, z, y, w); } +Vector4 Quat::zzyw() const { return Vector4 (z, z, y, w); } +Vector4 Quat::wzyw() const { return Vector4 (w, z, y, w); } +Vector4 Quat::xwyw() const { return Vector4 (x, w, y, w); } +Vector4 Quat::ywyw() const { return Vector4 (y, w, y, w); } +Vector4 Quat::zwyw() const { return Vector4 (z, w, y, w); } +Vector4 Quat::wwyw() const { return Vector4 (w, w, y, w); } +Vector4 Quat::xxzw() const { return Vector4 (x, x, z, w); } +Vector4 Quat::yxzw() const { return Vector4 (y, x, z, w); } +Vector4 Quat::zxzw() const { return Vector4 (z, x, z, w); } +Vector4 Quat::wxzw() const { return Vector4 (w, x, z, w); } +Vector4 Quat::xyzw() const { return Vector4 (x, y, z, w); } +Vector4 Quat::yyzw() const { return Vector4 (y, y, z, w); } +Vector4 Quat::zyzw() const { return Vector4 (z, y, z, w); } +Vector4 Quat::wyzw() const { return Vector4 (w, y, z, w); } +Vector4 Quat::xzzw() const { return Vector4 (x, z, z, w); } +Vector4 Quat::yzzw() const { return Vector4 (y, z, z, w); } +Vector4 Quat::zzzw() const { return Vector4 (z, z, z, w); } +Vector4 Quat::wzzw() const { return Vector4 (w, z, z, w); } +Vector4 Quat::xwzw() const { return Vector4 (x, w, z, w); } +Vector4 Quat::ywzw() const { return Vector4 (y, w, z, w); } +Vector4 Quat::zwzw() const { return Vector4 (z, w, z, w); } +Vector4 Quat::wwzw() const { return Vector4 (w, w, z, w); } +Vector4 Quat::xxww() const { return Vector4 (x, x, w, w); } +Vector4 Quat::yxww() const { return Vector4 (y, x, w, w); } +Vector4 Quat::zxww() const { return Vector4 (z, x, w, w); } +Vector4 Quat::wxww() const { return Vector4 (w, x, w, w); } +Vector4 Quat::xyww() const { return Vector4 (x, y, w, w); } +Vector4 Quat::yyww() const { return Vector4 (y, y, w, w); } +Vector4 Quat::zyww() const { return Vector4 (z, y, w, w); } +Vector4 Quat::wyww() const { return Vector4 (w, y, w, w); } +Vector4 Quat::xzww() const { return Vector4 (x, z, w, w); } +Vector4 Quat::yzww() const { return Vector4 (y, z, w, w); } +Vector4 Quat::zzww() const { return Vector4 (z, z, w, w); } +Vector4 Quat::wzww() const { return Vector4 (w, z, w, w); } +Vector4 Quat::xwww() const { return Vector4 (x, w, w, w); } +Vector4 Quat::ywww() const { return Vector4 (y, w, w, w); } +Vector4 Quat::zwww() const { return Vector4 (z, w, w, w); } +Vector4 Quat::wwww() const { return Vector4 (w, w, w, w); } +} + diff --git a/externals/g3dlite/Random.cpp b/externals/g3dlite/Random.cpp new file mode 100644 index 00000000000..2dda744a1ac --- /dev/null +++ b/externals/g3dlite/Random.cpp @@ -0,0 +1,212 @@ +/** + @file Random.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2009-01-02 + @edited 2009-03-29 + + Copyright 2000-2009, Morgan McGuire. + All rights reserved. + */ +#include "G3D/Random.h" + +namespace G3D { + +Random& Random::common() { + static Random r; + return r; +} + +Random::Random(void* x) : state(NULL), m_threadsafe(false) { + (void)x; +} + + +Random::Random(uint32 seed, bool threadsafe) : m_threadsafe(threadsafe) { + const uint32 X = 1812433253UL; + + state = new uint32[N]; + state[0] = seed; + for (index = 1; index < (int)N; ++index) { + state[index] = X * (state[index - 1] ^ (state[index - 1] >> 30)) + index; + } +} + + +Random::~Random() { + delete[] state; + state = NULL; +} + + +uint32 Random::bits() { + // See http://en.wikipedia.org/wiki/Mersenne_twister + + // Make a local copy of the index variable to ensure that it + // is not out of bounds + int localIndex = index; + + // Automatically checks for index < 0 if corrupted + // by unsynchronized threads. + if ((unsigned int)localIndex >= (unsigned int)N) { + generate(); + localIndex = 0; + } + // Increment the global index. It may go out of bounds on + // multiple threads, but the above check ensures that the + // array index actually used never goes out of bounds. + // It doesn't matter if we grab the same array index twice + // on two threads, since the distribution of random numbers + // will still be uniform. + ++index; + // Return the next random in the sequence + uint32 r = state[localIndex]; + + // Temper the result + r ^= r >> U; + r ^= (r << S) & B; + r ^= (r << T) & C; + r ^= r >> L; + + return r; +} + + +/** Generate the next N ints, and store them for readback later */ +void Random::generate() { + // Lower R bits + static const uint32 LOWER_MASK = (1LU << R) - 1; + + // Upper (32 - R) bits + static const uint32 UPPER_MASK = 0xFFFFFFFF << R; + static const uint32 mag01[2] = {0UL, (uint32)A}; + + if (m_threadsafe) { + bool contention = ! lock.lock(); + if (contention) { + // Another thread just generated a set of numbers; no need for + // this thread to do it too + lock.unlock(); + return; + } + } + + // First N - M + for (unsigned int i = 0; i < N - M; ++i) { + uint32 x = (state[i] & UPPER_MASK) | (state[i + 1] & LOWER_MASK); + state[i] = state[i + M] ^ (x >> 1) ^ mag01[x & 1]; + } + + // Rest + for (unsigned int i = N - M + 1; i < N - 1; ++i) { + uint32 x = (state[i] & UPPER_MASK) | (state[i + 1] & LOWER_MASK); + state[i] = state[i + (M - N)] ^ (x >> 1) ^ mag01[x & 1]; + } + + uint32 y = (state[N - 1] & UPPER_MASK) | (state[0] & LOWER_MASK); + state[N - 1] = state[M - 1] ^ (y >> 1) ^ mag01[y & 1]; + index = 0; + + if (m_threadsafe) { + lock.unlock(); + } +} + + +int Random::integer(int low, int high) { + int r = iFloor(low + (high - low + 1) * (double)bits() / 0xFFFFFFFFUL); + + // There is a *very small* chance of generating + // a number larger than high. + if (r > high) { + return high; + } else { + return r; + } +} + + +float Random::gaussian(float mean, float stdev) { + + // Using Box-Mueller method from http://www.taygeta.com/random/gaussian.html + // Modified to specify standard deviation and mean of distribution + float w, x1, x2; + + // Loop until w is less than 1 so that log(w) is negative + do { + x1 = uniform(-1.0, 1.0); + x2 = uniform(-1.0, 1.0); + + w = float(square(x1) + square(x2)); + } while (w > 1.0f); + + // Transform to gassian distribution + // Multiply by sigma (stdev ^ 2) and add mean. + return x2 * (float)square(stdev) * sqrtf((-2.0f * logf(w) ) / w) + mean; +} + + +void Random::cosHemi(float& x, float& y, float& z) { + const float e1 = uniform(); + const float e2 = uniform(); + + // Jensen's method + const float sin_theta = sqrtf(1.0f - e1); + const float cos_theta = sqrtf(e1); + const float phi = 6.28318531f * e2; + + x = cos(phi) * sin_theta; + y = sin(phi) * sin_theta; + z = cos_theta; + + // We could also use Malley's method (pbrt p.657), since they are the same cost: + // + // r = sqrt(e1); + // t = 2*pi*e2; + // x = cos(t)*r; + // y = sin(t)*r; + // z = sqrt(1.0 - x*x + y*y); +} + + +void Random::cosPowHemi(const float k, float& x, float& y, float& z) { + const float e1 = uniform(); + const float e2 = uniform(); + + const float cos_theta = pow(e1, 1.0f / (k + 1.0f)); + const float sin_theta = sqrtf(1.0f - square(cos_theta)); + const float phi = 6.28318531f * e2; + + x = cos(phi) * sin_theta; + y = sin(phi) * sin_theta; + z = cos_theta; +} + + +void Random::hemi(float& x, float& y, float& z) { + sphere(x, y, z); + z = fabsf(z); +} + + +void Random::sphere(float& x, float& y, float& z) { + // Squared magnitude + float m2; + + // Rejection sample + do { + x = uniform() * 2.0f - 1.0f, + y = uniform() * 2.0f - 1.0f, + z = uniform() * 2.0f - 1.0f; + m2 = x*x + y*y + z*z; + } while (m2 >= 1.0f); + + // Divide by magnitude to produce a unit vector + float s = rsqrt(m2); + x *= s; + y *= s; + z *= s; +} + +} // G3D diff --git a/externals/g3dlite/Ray.cpp b/externals/g3dlite/Ray.cpp new file mode 100644 index 00000000000..0436ef0b323 --- /dev/null +++ b/externals/g3dlite/Ray.cpp @@ -0,0 +1,218 @@ +/** + @file Ray.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2002-07-12 + @edited 2004-03-19 + */ + +#include "G3D/platform.h" +#include "G3D/Ray.h" +#include "G3D/Plane.h" +#include "G3D/Sphere.h" +#include "G3D/CollisionDetection.h" + +namespace G3D { + +void Ray::set(const Vector3& origin, const Vector3& direction) { + m_origin = origin; + m_direction = direction; + debugAssert(direction.isUnit()); + + m_invDirection = Vector3::one() / direction; + + // ray slope + ibyj = m_direction.x * m_invDirection.y; + jbyi = m_direction.y * m_invDirection.x; + jbyk = m_direction.y * m_invDirection.z; + kbyj = m_direction.z * m_invDirection.y; + ibyk = m_direction.x * m_invDirection.z; + kbyi = m_direction.z * m_invDirection.x; + + // precomputed terms + c_xy = m_origin.y - jbyi * m_origin.x; + c_xz = m_origin.z - kbyi * m_origin.x; + c_yx = m_origin.x - ibyj * m_origin.y; + c_yz = m_origin.z - kbyj * m_origin.y; + c_zx = m_origin.x - ibyk * m_origin.z; + c_zy = m_origin.y - jbyk * m_origin.z; + + //ray slope classification + if (m_direction.x < 0) { + if (m_direction.y < 0) { + if (m_direction.z < 0) { + classification = MMM; + } else if (m_direction.z > 0) { + classification = MMP; + } else { //(m_direction.z >= 0) + classification = MMO; + } + } else { //(m_direction.y >= 0) + if (m_direction.z < 0) { + if (m_direction.y == 0) { + classification = MOM; + } else { + classification = MPM; + } + } else { //(m_direction.z >= 0) + if ((m_direction.y == 0) && (m_direction.z == 0)) { + classification = MOO; + } else if (m_direction.z == 0) { + classification = MPO; + } else if (m_direction.y == 0) { + classification = MOP; + } else { + classification = MPP; + } + } + } + } else { //(m_direction.x >= 0) + if (m_direction.y < 0) { + if (m_direction.z < 0) { + if (m_direction.x == 0) { + classification = OMM; + } else { + classification = PMM; + } + } else { //(m_direction.z >= 0) + if ((m_direction.x == 0) && (m_direction.z == 0)) { + classification = OMO; + } else if (m_direction.z == 0) { + classification = PMO; + } else if (m_direction.x == 0) { + classification = OMP; + } else { + classification = PMP; + } + } + } else { //(m_direction.y >= 0) + if (m_direction.z < 0) { + if ((m_direction.x == 0) && (m_direction.y == 0)) { + classification = OOM; + } else if (m_direction.x == 0) { + classification = OPM; + } else if (m_direction.y == 0) { + classification = POM; + } else { + classification = PPM; + } + } else { //(m_direction.z > 0) + if (m_direction.x == 0) { + if (m_direction.y == 0) { + classification = OOP; + } else if (m_direction.z == 0) { + classification = OPO; + } else { + classification = OPP; + } + } else { + if ((m_direction.y == 0) && (m_direction.z == 0)) { + classification = POO; + } else if (m_direction.y == 0) { + classification = POP; + } else if (m_direction.z == 0) { + classification = PPO; + } else { + classification = PPP; + } + } + } + } + } +} + +Ray::Ray(class BinaryInput& b) { + deserialize(b); +} + + +void Ray::serialize(class BinaryOutput& b) const { + m_origin.serialize(b); + m_direction.serialize(b); +} + + +void Ray::deserialize(class BinaryInput& b) { + m_origin.deserialize(b); + m_direction.deserialize(b); + set(m_origin, m_direction); +} + + +Ray Ray::refract( + const Vector3& newOrigin, + const Vector3& normal, + float iInside, + float iOutside) const { + + Vector3 D = m_direction.refractionDirection(normal, iInside, iOutside); + return Ray(newOrigin + (m_direction + normal * (float)sign(m_direction.dot(normal))) * 0.001f, D); +} + + +Ray Ray::reflect( + const Vector3& newOrigin, + const Vector3& normal) const { + + Vector3 D = m_direction.reflectionDirection(normal); + return Ray(newOrigin + (D + normal) * 0.001f, D); +} + + +Vector3 Ray::intersection(const Plane& plane) const { + float d; + Vector3 normal = plane.normal(); + plane.getEquation(normal, d); + float rate = m_direction.dot(normal); + + if (rate >= 0.0f) { + return Vector3::inf(); + } else { + float t = -(d + m_origin.dot(normal)) / rate; + return m_origin + m_direction * t; + } +} + + +float Ray::intersectionTime(const class Sphere& sphere, bool solid) const { + Vector3 dummy; + return CollisionDetection::collisionTimeForMovingPointFixedSphere( + m_origin, m_direction, sphere, dummy, dummy, solid); +} + + +float Ray::intersectionTime(const class Plane& plane) const { + Vector3 dummy; + return CollisionDetection::collisionTimeForMovingPointFixedPlane( + m_origin, m_direction, plane, dummy); +} + + +float Ray::intersectionTime(const class Box& box) const { + Vector3 dummy; + float time = CollisionDetection::collisionTimeForMovingPointFixedBox( + m_origin, m_direction, box, dummy); + + if ((time == finf()) && (box.contains(m_origin))) { + return 0.0f; + } else { + return time; + } +} + + +float Ray::intersectionTime(const class AABox& box) const { + Vector3 dummy; + bool inside; + float time = CollisionDetection::collisionTimeForMovingPointFixedAABox( + m_origin, m_direction, box, dummy, inside); + + if ((time == finf()) && inside) { + return 0.0f; + } else { + return time; + } +} + +} diff --git a/externals/g3dlite/ReferenceCount.cpp b/externals/g3dlite/ReferenceCount.cpp new file mode 100644 index 00000000000..2e1f117e0d9 --- /dev/null +++ b/externals/g3dlite/ReferenceCount.cpp @@ -0,0 +1,61 @@ +/** + @file ReferenceCount.cpp + + Reference Counting Garbage Collector for C++ + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + @cite Adapted and extended from Justin Miller's "RGC" class that appeared in BYTE magazine. + @cite See also http://www.jelovic.com/articles/cpp_without_memory_errors_slides.htm + + @created 2001-10-23 + @edited 2009-04-25 +*/ +#include "G3D/platform.h" +#include "G3D/ReferenceCount.h" + +namespace G3D { + +ReferenceCountedObject::ReferenceCountedObject() : + ReferenceCountedObject_refCount(0), + ReferenceCountedObject_weakPointer(0) { + + debugAssertM(isValidHeapPointer(this), + "Reference counted objects must be allocated on the heap."); +} + +void ReferenceCountedObject::ReferenceCountedObject_zeroWeakPointers() { + // Tell all of my weak pointers that I'm gone. + + _WeakPtrLinkedList* node = ReferenceCountedObject_weakPointer; + + while (node != NULL) { + // Notify the weak pointer that it is going away + node->weakPtr->objectCollected(); + + // Free the node and advance + _WeakPtrLinkedList* tmp = node; + node = node->next; + delete tmp; + } +} + +ReferenceCountedObject::~ReferenceCountedObject() {} + + +ReferenceCountedObject::ReferenceCountedObject(const ReferenceCountedObject& notUsed) : + ReferenceCountedObject_refCount(0), + ReferenceCountedObject_weakPointer(0) { + (void)notUsed; + debugAssertM(G3D::isValidHeapPointer(this), + "Reference counted objects must be allocated on the heap."); +} + +ReferenceCountedObject& ReferenceCountedObject::operator=(const ReferenceCountedObject& other) { + (void)other; + // Nothing changes when I am assigned; the reference count on + // both objects is the same (although my super-class probably + // changes). + return *this; +} + +} // G3D diff --git a/externals/g3dlite/Sphere.cpp b/externals/g3dlite/Sphere.cpp new file mode 100644 index 00000000000..4ed0811cb29 --- /dev/null +++ b/externals/g3dlite/Sphere.cpp @@ -0,0 +1,223 @@ +/** + @file Sphere.cpp + + Sphere class + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2001-04-17 + @edited 2009-01-20 + */ + +#include "G3D/platform.h" +#include "G3D/Sphere.h" +#include "G3D/stringutils.h" +#include "G3D/BinaryOutput.h" +#include "G3D/BinaryInput.h" +#include "G3D/AABox.h" +#include "G3D/Plane.h" + +namespace G3D { + +int32 Sphere::dummy; + +Sphere::Sphere(class BinaryInput& b) { + deserialize(b); +} + + +void Sphere::serialize(class BinaryOutput& b) const { + center.serialize(b); + b.writeFloat64(radius); +} + + +void Sphere::deserialize(class BinaryInput& b) { + center.deserialize(b); + radius = (float)b.readFloat64(); +} + + +std::string Sphere::toString() const { + return format("Sphere(<%g, %g, %g>, %g)", + center.x, center.y, center.z, radius); +} + + +bool Sphere::contains(const Vector3& point) const { + float distance = (center - point).squaredMagnitude(); + return distance <= square(radius); +} + + +bool Sphere::contains(const Sphere& other) const { + float distance = (center - other.center).squaredMagnitude(); + return (radius >= other.radius) && (distance <= square(radius - other.radius)); +} + + +bool Sphere::intersects(const Sphere& other) const { + return (other.center - center).length() <= (radius + other.radius); +} + + +void Sphere::merge(const Sphere& other) { + if (other.contains(*this)) { + *this = other; + } else if (! contains(other)) { + // The farthest distance is along the axis between the centers, which + // must not be colocated since neither contains the other. + Vector3 toMe = center - other.center; + // Get a point on the axis from each + toMe = toMe.direction(); + const Vector3& A = center + toMe * radius; + const Vector3& B = other.center - toMe * other.radius; + + // Now just bound the A->B segment + center = (A + B) * 0.5f; + radius = (A - B).length(); + } + // (if this contains other, we're done) +} + + +bool Sphere::culledBy( + const Array<Plane>& plane, + int& cullingPlaneIndex, + const uint32 inMask, + uint32& outMask) const { + + return culledBy(plane.getCArray(), plane.size(), cullingPlaneIndex, inMask, outMask); +} + + +bool Sphere::culledBy( + const Array<Plane>& plane, + int& cullingPlaneIndex, + const uint32 inMask) const { + + return culledBy(plane.getCArray(), plane.size(), cullingPlaneIndex, inMask); +} + + +bool Sphere::culledBy( + const class Plane* plane, + int numPlanes, + int& cullingPlane, + const uint32 _inMask, + uint32& childMask) const { + + if (radius == finf()) { + // No plane can cull the infinite box + return false; + } + + uint32 inMask = _inMask; + assert(numPlanes < 31); + + childMask = 0; + + // See if there is one plane for which all of the + // vertices are in the negative half space. + for (int p = 0; p < numPlanes; p++) { + + // Only test planes that are not masked + if ((inMask & 1) != 0) { + + bool culledLow = ! plane[p].halfSpaceContainsFinite(center + plane[p].normal() * radius); + bool culledHigh = ! plane[p].halfSpaceContainsFinite(center - plane[p].normal() * radius); + + if (culledLow) { + // Plane p culled the sphere + cullingPlane = p; + + // The caller should not recurse into the children, + // since the parent is culled. If they do recurse, + // make them only test against this one plane, which + // will immediately cull the volume. + childMask = 1 << p; + return true; + + } else if (culledHigh) { + // The bounding volume straddled the plane; we have + // to keep testing against this plane + childMask |= (1 << p); + } + } + + // Move on to the next bit. + inMask = inMask >> 1; + } + + // None of the planes could cull this box + cullingPlane = -1; + return false; +} + + +bool Sphere::culledBy( + const class Plane* plane, + int numPlanes, + int& cullingPlane, + const uint32 _inMask) const { + + uint32 inMask = _inMask; + assert(numPlanes < 31); + + // See if there is one plane for which all of the + // vertices are in the negative half space. + for (int p = 0; p < numPlanes; p++) { + + // Only test planes that are not masked + if ((inMask & 1) != 0) { + bool culled = ! plane[p].halfSpaceContains(center + plane[p].normal() * radius); + if (culled) { + // Plane p culled the sphere + cullingPlane = p; + return true; + } + } + + // Move on to the next bit. + inMask = inMask >> 1; + } + + // None of the planes could cull this box + cullingPlane = -1; + return false; +} + + +Vector3 Sphere::randomSurfacePoint() const { + return Vector3::random() * radius + center; +} + + +Vector3 Sphere::randomInteriorPoint() const { + Vector3 result; + do { + result = Vector3(uniformRandom(-1, 1), + uniformRandom(-1, 1), + uniformRandom(-1, 1)); + } while (result.squaredMagnitude() >= 1.0f); + + return result * radius + center; +} + + +float Sphere::volume() const { + return (float)pi() * (4.0f / 3.0f) * powf((float)radius, 3.0f); +} + + +float Sphere::area() const { + return (float)pi() * 4.0f * powf((float)radius, 2.0f); +} + + +void Sphere::getBounds(AABox& out) const { + Vector3 extent(radius, radius, radius); + out = AABox(center - extent, center + extent); +} + +} // namespace diff --git a/externals/g3dlite/System.cpp b/externals/g3dlite/System.cpp new file mode 100644 index 00000000000..e03c4e8c6fa --- /dev/null +++ b/externals/g3dlite/System.cpp @@ -0,0 +1,1746 @@ +/** + @file System.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + Note: every routine must call init() first. + + There are two kinds of detection used in this file. At compile + time, the _MSC_VER #define is used to determine whether x86 assembly + can be used at all. At runtime, processor detection is used to + determine if we can safely call the routines that use that assembly. + + @created 2003-01-25 + @edited 2010-01-03 + */ + +#include "G3D/platform.h" +#include "G3D/System.h" +#include "G3D/debug.h" +#include "G3D/fileutils.h" +#include "G3D/TextOutput.h" +#include "G3D/G3DGameUnits.h" +#include "G3D/Crypto.h" +#include "G3D/prompt.h" +#include "G3D/stringutils.h" +#include "G3D/Log.h" +#include "G3D/Table.h" +#include "G3D/GMutex.h" +#include "G3D/units.h" +#include <time.h> + +#include <cstring> +#include <cstdio> + +// Uncomment the following line to turn off G3D::System memory +// allocation and use the operating system's malloc. +//#define NO_BUFFERPOOL + +#if defined(__i386__) || defined(__x86_64__) || defined(G3D_WIN32) +# define G3D_NOT_OSX_PPC +#endif + +#include <cstdlib> + +#ifdef G3D_WIN32 + +# include <conio.h> +# include <sys/timeb.h> +# include "G3D/RegistryUtil.h" + +#elif defined(G3D_LINUX) + +# include <stdlib.h> +# include <stdio.h> +# include <errno.h> +# include <sys/types.h> +# include <sys/select.h> +# include <termios.h> +# include <unistd.h> +# include <sys/ioctl.h> +# include <sys/time.h> +# include <pthread.h> + +#elif defined(G3D_OSX) + + #include <stdlib.h> + #include <stdio.h> + #include <errno.h> + #include <sys/types.h> + #include <sys/sysctl.h> + #include <sys/select.h> + #include <sys/time.h> + #include <termios.h> + #include <unistd.h> + #include <pthread.h> + #include <mach-o/arch.h> + + #include <sstream> + #include <CoreServices/CoreServices.h> +#endif + +// SIMM include +#ifdef __SSE__ +#include <xmmintrin.h> +#endif + +namespace G3D { + + +/** Checks if the CPUID command is available on the processor (called from init) */ +static bool checkForCPUID(); + +/** Called from init */ +static void getG3DVersion(std::string& s); + +/** Called from init */ +static G3DEndian checkEndian(); + + +System& System::instance() { + static System thesystem; + return thesystem; +} + + +System::System() : + m_initialized(false), + m_cpuSpeed(0), + m_hasCPUID(false), + m_hasRDTSC(false), + m_hasMMX(false), + m_hasSSE(false), + m_hasSSE2(false), + m_hasSSE3(false), + m_has3DNOW(false), + m_has3DNOW2(false), + m_hasAMDMMX(false), + m_cpuVendor("Uninitialized"), + m_numCores(1), + m_machineEndian(G3D_LITTLE_ENDIAN), + m_cpuArch("Uninitialized"), + m_operatingSystem("Uninitialized"), + m_version("Uninitialized"), + m_outOfMemoryCallback(NULL), + m_realWorldGetTickTime0(0), + m_highestCPUIDFunction(0) { + + init(); +} + + +void System::init() { + // NOTE: Cannot use most G3D data structures or utility functions + // in here because they are not initialized. + + if (m_initialized) { + return; + } else { + m_initialized = true; + } + + getG3DVersion(m_version); + + m_machineEndian = checkEndian(); + + m_hasCPUID = checkForCPUID(); + // Process the CPUID information + if (m_hasCPUID) { + // We read the standard CPUID level 0x00000000 which should + // be available on every x86 processor. This fills out + // a string with the processor vendor tag. + unsigned int eaxreg = 0, ebxreg = 0, ecxreg = 0, edxreg = 0; + + cpuid(CPUID_VENDOR_ID, eaxreg, ebxreg, ecxreg, edxreg); + + { + char c[100]; + // Then we connect the single register values to the vendor string + *((unsigned int*) c) = ebxreg; + *((unsigned int*) (c + 4)) = edxreg; + *((unsigned int*) (c + 8)) = ecxreg; + c[12] = '\0'; + m_cpuVendor = c; + } + + switch (ebxreg) { + case 0x756E6547: // GenuineIntel + m_cpuArch = "Intel Processor"; + break; + + case 0x68747541: // AuthenticAMD + m_cpuArch = "AMD Processor"; + break; + + case 0x69727943: // CyrixInstead + m_cpuArch = "Cyrix Processor"; + break; + + default: + m_cpuArch = "Unknown Processor Vendor"; + break; + } + + + unsigned int highestFunction = eaxreg; + if (highestFunction >= CPUID_NUM_CORES) { + cpuid(CPUID_NUM_CORES, eaxreg, ebxreg, ecxreg, edxreg); + // Number of cores is in (eax>>26) + 1 + m_numCores = (eaxreg >> 26) + 1; + } + + cpuid(CPUID_GET_HIGHEST_FUNCTION, m_highestCPUIDFunction, ebxreg, ecxreg, edxreg); + } + + + // Get the operating system name (also happens to read some other information) +# ifdef G3D_WIN32 + // Note that this overrides some of the values computed above + bool success = RegistryUtil::readInt32 + ("HKEY_LOCAL_MACHINE\\HARDWARE\\DESCRIPTION\\System\\CentralProcessor\\0", + "~MHz", m_cpuSpeed); + + SYSTEM_INFO systemInfo; + GetSystemInfo(&systemInfo); + const char* arch = NULL; + switch (systemInfo.wProcessorArchitecture) { + case PROCESSOR_ARCHITECTURE_INTEL: + arch = "Intel"; + break; + + case PROCESSOR_ARCHITECTURE_MIPS: + arch = "MIPS"; + break; + + case PROCESSOR_ARCHITECTURE_ALPHA: + arch = "Alpha"; + break; + + case PROCESSOR_ARCHITECTURE_PPC: + arch = "Power PC"; + break; + + default: + arch = "Unknown"; + } + + m_numCores = systemInfo.dwNumberOfProcessors; + uint32 maxAddr = (uint32)systemInfo.lpMaximumApplicationAddress; + { + char c[1024]; + sprintf(c, "%d x %d-bit %s processor", + systemInfo.dwNumberOfProcessors, + (int)(::log((double)maxAddr) / ::log(2.0) + 2.0), + arch); + m_cpuArch = c; + } + + OSVERSIONINFO osVersionInfo; + osVersionInfo.dwOSVersionInfoSize = sizeof(OSVERSIONINFO); + success = GetVersionEx(&osVersionInfo) != 0; + + if (success) { + char c[1000]; + sprintf(c, "Windows %d.%d build %d Platform %d %s", + osVersionInfo.dwMajorVersion, + osVersionInfo.dwMinorVersion, + osVersionInfo.dwBuildNumber, + osVersionInfo.dwPlatformId, + osVersionInfo.szCSDVersion); + m_operatingSystem = c; + } else { + m_operatingSystem = "Windows"; + } + +# elif defined(G3D_LINUX) || defined(G3D_FREEBSD) + + { + // Find the operating system using the 'uname' command + FILE* f = popen("uname -a", "r"); + + int len = 100; + char* r = (char*)::malloc(len * sizeof(char)); + fgets(r, len, f); + // Remove trailing newline + if (r[strlen(r) - 1] == '\n') { + r[strlen(r) - 1] = '\0'; + } + fclose(f); + + m_operatingSystem = r; + ::free(r); + } + +# elif defined(G3D_OSX) + + // Operating System: + SInt32 macVersion; + Gestalt(gestaltSystemVersion, &macVersion); + + int major = (macVersion >> 8) & 0xFF; + int minor = (macVersion >> 4) & 0xF; + int revision = macVersion & 0xF; + + { + char c[1000]; + sprintf(c, "OS X %x.%x.%x", major, minor, revision); + m_operatingSystem = c; + } + + // Clock Cycle Timing Information: + Gestalt('pclk', &m_OSXCPUSpeed); + m_cpuSpeed = iRound((double)m_OSXCPUSpeed / (1024 * 1024)); + m_secondsPerNS = 1.0 / 1.0e9; + + // System Architecture: + const NXArchInfo* pInfo = NXGetLocalArchInfo(); + + if (pInfo) { + m_cpuArch = pInfo->description; + + switch (pInfo->cputype) { + case CPU_TYPE_POWERPC: + switch(pInfo->cpusubtype){ + case CPU_SUBTYPE_POWERPC_750: + case CPU_SUBTYPE_POWERPC_7400: + case CPU_SUBTYPE_POWERPC_7450: + m_cpuVendor = "Motorola"; + break; + case CPU_SUBTYPE_POWERPC_970: + m_cpuVendor = "IBM"; + break; + } + break; + + case CPU_TYPE_I386: + m_cpuVendor = "Intel"; + break; + } + } +# endif + + initTime(); + + getStandardProcessorExtensions(); +} + + +void getG3DVersion(std::string& s) { + char cstr[100]; + if ((G3D_VER % 100) != 0) { + sprintf(cstr, "G3D %d.%02d beta %d", + G3D_VER / 10000, + (G3D_VER / 100) % 100, + G3D_VER % 100); + } else { + sprintf(cstr, "G3D %d.%02d", + G3D_VER / 10000, + (G3D_VER / 100) % 100); + } + s = cstr; +} + +#if 0 // TODO: delete +struct Directory { + std::string path; + Array<std::string> contents; +}; + +static bool maybeAddDirectory(const std::string& newPath, Array<Directory>& directoryArray, bool recurse = true) { + if (fileExists(newPath)) { + Directory& d = directoryArray.next(); + d.path = newPath; + getFiles(pathConcat(newPath, "*"), d.contents); + Array<std::string> dirs; + getDirs(pathConcat(newPath, "*"), dirs); + d.contents.append(dirs); + + if (recurse) { + // Look for subdirectories + static const std::string subdirs[] = + {"font", "gui", "SuperShader", "cubemap", "icon", "material", "image", "md2", "md3", "ifs", "3ds", "sky", ""}; + + for (int j = 0; j < dirs.size(); ++j) { + for (int i = 0; ! subdirs[i].empty(); ++i) { + if (dirs[j] == subdirs[i]) { + maybeAddDirectory(pathConcat(newPath, dirs[j]), directoryArray, false); + } + } + } + } + return true; + } else { + return false; + } +} +#endif + +std::string System::findDataFile +(const std::string& full, + bool errorIfNotFound) { + + // Places where specific files were most recently found. This is + // used to cache seeking of common files. + static Table<std::string, std::string> lastFound; + + // First check if the file exists as requested. This will go + // through the FileSystemCache, so most calls do not touch disk. + if (fileExists(full)) { + return full; + } + + // Now check where we previously found this file. + std::string* last = lastFound.getPointer(full); + if (last != NULL) { + if (fileExists(*last)) { + // Even if cwd has changed the file is still present. + // We won't notice if it has been deleted, however. + return *last; + } else { + // Remove this from the cache it is invalid + lastFound.remove(full); + } + } + + // Places to look + static Array<std::string> directoryArray; + + if (directoryArray.size() == 0) { + // Initialize the directory array + RealTime t0 = System::time(); + + Array<std::string> baseDirArray; + + std::string initialAppDataDir(instance().m_appDataDir); + + baseDirArray.append(""); + if (! initialAppDataDir.empty()) { + baseDirArray.append(initialAppDataDir); + } + + const char* g3dPath = getenv("G3DDATA"); + + if (g3dPath && (initialAppDataDir != g3dPath)) { + baseDirArray.append(g3dPath); + } + + static const std::string subdirs[] = + {"font", "gui", "SuperShader", "cubemap", "icon", "material", "image", "md2", "md3", "ifs", "3ds", "sky", ""}; + for (int j = 0; j < baseDirArray.size(); ++j) { + std::string d = baseDirArray[j]; + if (fileExists(d)) { + directoryArray.append(d); + for (int i = 0; ! subdirs[i].empty(); ++i) { + const std::string& p = pathConcat(d, subdirs[i]); + if (fileExists(p)) { + directoryArray.append(p); + } + } + } + } + + logLazyPrintf("Initializing System::findDataFile took %fs\n", System::time() - t0); + } + + for (int i = 0; i < directoryArray.size(); ++i) { + const std::string& p = pathConcat(directoryArray[i], full); + if (fileExists(p)) { + lastFound.set(full, p); + return p; + } + } + + if (errorIfNotFound) { + // Generate an error message + std::string locations; + for (int i = 0; i < directoryArray.size(); ++i) { + locations += pathConcat(directoryArray[i], full) + "\n"; + } + alwaysAssertM(false, "Could not find '" + full + "' in:\n" + locations); + } + + // Not found + return ""; +} + + +void System::setAppDataDir(const std::string& path) { + instance().m_appDataDir = path; +} + + +std::string demoFindData(bool errorIfNotFound) { + static const char* g3dPath = getenv("G3DDATA"); + if (g3dPath) { + return g3dPath; +# ifdef G3D_WIN32 + } else if (fileExists("../data")) { + // G3D install on Windows + return "../data"; + } else if (fileExists("../data-files")) { + // G3D source on Windows + return "../data-files"; +# else + } else if (fileExists("../../../../data")) { + // G3D install on Unix + return "../../../../data"; + } else if (fileExists("../../../../data-files")) { + // G3D source on Unix + return "../../../../data-files"; +# endif + } else { + return ""; + } +} + + +const std::string& System::build() { + const static std::string b = +# ifdef _DEBUG + "Debug"; +# else + "Release"; +# endif + + return b; +} + + +static G3DEndian checkEndian() { + int32 a = 1; + if (*(uint8*)&a == 1) { + return G3D_LITTLE_ENDIAN; + } else { + return G3D_BIG_ENDIAN; + } +} + + +static bool checkForCPUID() { + // all known supported architectures have cpuid + // add cases for incompatible architectures if they are added + // e.g., if we ever support __powerpc__ being defined again + + return true; +} + + +void System::getStandardProcessorExtensions() { +#if ! defined(G3D_OSX) || defined(G3D_OSX_INTEL) + if (! m_hasCPUID) { + return; + } + + uint32 eaxreg = 0, ebxreg = 0, ecxreg = 0, features = 0; + + cpuid(CPUID_PROCESSOR_FEATURES, eaxreg, ebxreg, ecxreg, features); + +# define checkBit(var, bit) ((var & (1 << bit)) ? true : false) + + m_hasRDTSC = checkBit(features, 4); + m_hasMMX = checkBit(features, 23); + m_hasSSE = checkBit(features, 25); + m_hasSSE2 = checkBit(features, 26); + // Bit 28 is HTT; not checked by G3D + + m_hasSSE3 = checkBit(ecxreg, 0); + + if (m_highestCPUIDFunction >= CPUID_EXTENDED_FEATURES) { + cpuid(CPUID_EXTENDED_FEATURES, eaxreg, ebxreg, ecxreg, features); + m_hasAMDMMX = checkBit(features, 22); // Only on AMD + m_has3DNOW = checkBit(features, 31); // Only on AMD + m_has3DNOW2 = checkBit(features, 30); // Only on AMD + } else { + m_hasAMDMMX = false; + m_has3DNOW = false; + m_has3DNOW2 = false; + } + +# undef checkBit +#endif +} + +#if defined(G3D_WIN32) && !defined(G3D_64BIT) + #pragma message("Port System::memcpy SIMD to all platforms") +/** Michael Herf's fast memcpy */ +void memcpyMMX(void* dst, const void* src, int nbytes) { + int remainingBytes = nbytes; + + if (nbytes > 64) { + _asm { + mov esi, src + mov edi, dst + mov ecx, nbytes + shr ecx, 6 // 64 bytes per iteration + + loop1: + movq mm1, 0[ESI] // Read in source data + movq mm2, 8[ESI] + movq mm3, 16[ESI] + movq mm4, 24[ESI] + movq mm5, 32[ESI] + movq mm6, 40[ESI] + movq mm7, 48[ESI] + movq mm0, 56[ESI] + + movntq 0[EDI], mm1 // Non-temporal stores + movntq 8[EDI], mm2 + movntq 16[EDI], mm3 + movntq 24[EDI], mm4 + movntq 32[EDI], mm5 + movntq 40[EDI], mm6 + movntq 48[EDI], mm7 + movntq 56[EDI], mm0 + + add esi, 64 + add edi, 64 + dec ecx + jnz loop1 + + emms + } + remainingBytes -= ((nbytes >> 6) << 6); + } + + if (remainingBytes > 0) { + // Memcpy the rest + memcpy((uint8*)dst + (nbytes - remainingBytes), + (const uint8*)src + (nbytes - remainingBytes), remainingBytes); + } +} +#endif + +void System::memcpy(void* dst, const void* src, size_t numBytes) { +#if defined(G3D_WIN32) && !defined(G3D_64BIT) + memcpyMMX(dst, src, numBytes); +#else + ::memcpy(dst, src, numBytes); +#endif +} + + +/** Michael Herf's fastest memset. n32 must be filled with the same + character repeated. */ +#if defined(G3D_WIN32) && !defined(G3D_64BIT) + #pragma message("Port System::memfill SIMD to all platforms") + +// On x86 processors, use MMX +void memfill(void *dst, int n32, unsigned long i) { + + int originalSize = i; + int bytesRemaining = i; + + if (i > 16) { + + bytesRemaining = i % 16; + i -= bytesRemaining; + __asm { + movq mm0, n32 + punpckldq mm0, mm0 + mov edi, dst + + loopwrite: + + movntq 0[edi], mm0 + movntq 8[edi], mm0 + + add edi, 16 + sub i, 16 + jg loopwrite + + emms + } + } + + if (bytesRemaining > 0) { + ::memset((uint8*)dst + (originalSize - bytesRemaining), n32, bytesRemaining); + } +} +#endif + + +void System::memset(void* dst, uint8 value, size_t numBytes) { +#if defined(G3D_WIN32) && !defined(G3D_64BIT) + uint32 v = value; + v = v + (v << 8) + (v << 16) + (v << 24); + G3D::memfill(dst, v, numBytes); +#else + ::memset(dst, value, numBytes); +#endif +} + + +/** Removes the 'd' that icompile / Morgan's VC convention appends. */ +static std::string computeAppName(const std::string& start) { + if (start.size() < 2) { + return start; + } + + if (start[start.size() - 1] == 'd') { + // Maybe remove the 'd'; see if ../ or ../../ has the same name + char tmp[1024]; + getcwd(tmp, sizeof(tmp)); + std::string drive, base, ext; + Array<std::string> path; + parseFilename(tmp, drive, path, base, ext); + + std::string shortName = start.substr(0, start.size() - 1); + + if ((path.size() > 1) && (toLower(path.last()) == toLower(shortName))) { + return shortName; + } + + if ((path.size() > 2) && (toLower(path[path.size() - 2]) == toLower(shortName))) { + return shortName; + } + } + + return start; +} + + +std::string& System::appName() { + static std::string n = computeAppName(filenameBase(currentProgramFilename())); + return n; +} + + +std::string System::currentProgramFilename() { + char filename[2048]; + +# ifdef G3D_WIN32 + { + GetModuleFileNameA(NULL, filename, sizeof(filename)); + } +# elif defined(G3D_OSX) + { + // Run the 'ps' program to extract the program name + // from the process ID. + int pid; + FILE* fd; + char cmd[80]; + pid = getpid(); + sprintf(cmd, "ps -p %d -o comm=\"\"", pid); + + fd = popen(cmd, "r"); + int s = fread(filename, 1, sizeof(filename), fd); + // filename will contain a newline. Overwrite it: + filename[s - 1] = '\0'; + } +# else + { + int ret = readlink("/proc/self/exe", filename, sizeof(filename)); + + // In case of an error, leave the handling up to the caller + if (ret == -1) { + return ""; + } + + debugAssert((int)sizeof(filename) > ret); + + // Ensure proper NULL termination + filename[ret] = 0; + } + #endif + + return filename; +} + + +void System::sleep(RealTime t) { + + // Overhead of calling this function, measured from a previous run. + static const RealTime OVERHEAD = 0.00006f; + + RealTime now = time(); + RealTime wakeupTime = now + t - OVERHEAD; + + RealTime remainingTime = wakeupTime - now; + RealTime sleepTime = 0; + + // On Windows, a "time slice" is measured in quanta of 3-5 ms (http://support.microsoft.com/kb/259025) + // Sleep(0) yields the remainder of the time slice, which could be a long time. + // A 1 ms minimum time experimentally kept the "Empty GApp" at nearly no CPU load at 100 fps, + // yet nailed the frame timing perfectly. + static RealTime minRealSleepTime = 3 * units::milliseconds(); + + while (remainingTime > 0) { + + if (remainingTime > minRealSleepTime * 2.5) { + // Safe to use Sleep with a time... sleep for half the remaining time + sleepTime = max(remainingTime * 0.5, 0.0005); + } else if (remainingTime > minRealSleepTime) { + // Safe to use Sleep with a zero time; + // causes the program to yield only + // the current time slice, and then return. + sleepTime = 0; + } else { + // Not safe to use Sleep; busy wait + sleepTime = -1; + } + + if (sleepTime >= 0) { + #ifdef G3D_WIN32 + // Translate to milliseconds + Sleep((int)(sleepTime * 1e3)); + #else + // Translate to microseconds + usleep((int)(sleepTime * 1e6)); + #endif + } + + now = time(); + remainingTime = wakeupTime - now; + } +} + + +void System::consoleClearScreen() { +# ifdef G3D_WIN32 + system("cls"); +# else + system("clear"); +# endif +} + + +bool System::consoleKeyPressed() { + #ifdef G3D_WIN32 + + return _kbhit() != 0; + + #else + + static const int STDIN = 0; + static bool initialized = false; + + if (! initialized) { + // Use termios to turn off line buffering + termios term; + tcgetattr(STDIN, &term); + term.c_lflag &= ~ICANON; + tcsetattr(STDIN, TCSANOW, &term); + setbuf(stdin, NULL); + initialized = true; + } + + #ifdef G3D_LINUX + + int bytesWaiting; + ioctl(STDIN, FIONREAD, &bytesWaiting); + return bytesWaiting; + + #else + + timeval timeout; + fd_set rdset; + + FD_ZERO(&rdset); + FD_SET(STDIN, &rdset); + timeout.tv_sec = 0; + timeout.tv_usec = 0; + + return select(STDIN + 1, &rdset, NULL, NULL, &timeout); + #endif + #endif +} + + +int System::consoleReadKey() { +# ifdef G3D_WIN32 + return _getch(); +# else + char c; + read(0, &c, 1); + return c; +# endif +} + + +void System::initTime() { + #ifdef G3D_WIN32 + if (QueryPerformanceFrequency(&m_counterFrequency)) { + QueryPerformanceCounter(&m_start); + } + + struct _timeb t; + _ftime(&t); + + m_realWorldGetTickTime0 = (RealTime)t.time - t.timezone * G3D::MINUTE + (t.dstflag ? G3D::HOUR : 0); + + #else + gettimeofday(&m_start, NULL); + // "sse" = "seconds since epoch". The time + // function returns the seconds since the epoch + // GMT (perhaps more correctly called UTC). + time_t gmt = ::time(NULL); + + // No call to free or delete is needed, but subsequent + // calls to asctime, ctime, mktime, etc. might overwrite + // local_time_vals. + tm* localTimeVals = localtime(&gmt); + + time_t local = gmt; + + if (localTimeVals) { + // tm_gmtoff is already corrected for daylight savings. + local = local + localTimeVals->tm_gmtoff; + } + + m_realWorldGetTickTime0 = local; + #endif +} + + +RealTime System::time() { +# ifdef G3D_WIN32 + LARGE_INTEGER now; + QueryPerformanceCounter(&now); + + return ((RealTime)(now.QuadPart - instance().m_start.QuadPart) / + instance().m_counterFrequency.QuadPart) + instance().m_realWorldGetTickTime0; +# else + // Linux resolution defaults to 100Hz. + // There is no need to do a separate RDTSC call as gettimeofday + // actually uses RDTSC when on systems that support it, otherwise + // it uses the system clock. + struct timeval now; + gettimeofday(&now, NULL); + + return (now.tv_sec - instance().m_start.tv_sec) + + (now.tv_usec - instance().m_start.tv_usec) / 1e6 + + instance().m_realWorldGetTickTime0; +# endif +} + + +//////////////////////////////////////////////////////////////// + +#define REALPTR_TO_USERPTR(x) ((uint8*)(x) + sizeof (void *)) +#define USERPTR_TO_REALPTR(x) ((uint8*)(x) - sizeof (void *)) +#define REALBLOCK_SIZE(x) ((x) + sizeof (void *)) + +class BufferPool { +public: + + /** Only store buffers up to these sizes (in bytes) in each pool-> + Different pools have different management strategies. + + A large block is preallocated for tiny buffers; they are used with + tremendous frequency. Other buffers are allocated as demanded. + Tiny buffers are 128 bytes long because that seems to align well with + cache sizes on many machines. + */ + enum {tinyBufferSize = 128, smallBufferSize = 1024, medBufferSize = 4096}; + + /** + Most buffers we're allowed to store. + 250000 * 128 = 32 MB (preallocated) + 10000 * 1024 = 10 MB (allocated on demand) + 1024 * 4096 = 4 MB (allocated on demand) + */ + enum {maxTinyBuffers = 250000, maxSmallBuffers = 10000, maxMedBuffers = 1024}; + +private: + + class MemBlock { + public: + void* ptr; + size_t bytes; + + inline MemBlock() : ptr(NULL), bytes(0) {} + inline MemBlock(void* p, size_t b) : ptr(p), bytes(b) {} + }; + + MemBlock smallPool[maxSmallBuffers]; + int smallPoolSize; + + MemBlock medPool[maxMedBuffers]; + int medPoolSize; + + /** The tiny pool is a single block of storage into which all tiny + objects are allocated. This provides better locality for + small objects and avoids the search time, since all tiny + blocks are exactly the same size. */ + void* tinyPool[maxTinyBuffers]; + int tinyPoolSize; + + /** Pointer to the data in the tiny pool */ + void* tinyHeap; + + Spinlock m_lock; + + void lock() { + m_lock.lock(); + } + + void unlock() { + m_lock.unlock(); + } + +#if 0 //-----------------------------------------------old mutex +# ifdef G3D_WIN32 + CRITICAL_SECTION mutex; +# else + pthread_mutex_t mutex; +# endif + + /** Provide synchronization between threads */ + void lock() { +# ifdef G3D_WIN32 + EnterCriticalSection(&mutex); +# else + pthread_mutex_lock(&mutex); +# endif + } + + void unlock() { +# ifdef G3D_WIN32 + LeaveCriticalSection(&mutex); +# else + pthread_mutex_unlock(&mutex); +# endif + } +#endif //-------------------------------------------old mutex + + /** + Malloc out of the tiny heap. Returns NULL if allocation failed. + */ + inline void* tinyMalloc(size_t bytes) { + // Note that we ignore the actual byte size + // and create a constant size block. + (void)bytes; + assert(tinyBufferSize >= bytes); + + void* ptr = NULL; + + if (tinyPoolSize > 0) { + --tinyPoolSize; + + // Return the old last pointer from the freelist + ptr = tinyPool[tinyPoolSize]; + +# ifdef G3D_DEBUG + if (tinyPoolSize > 0) { + assert(tinyPool[tinyPoolSize - 1] != ptr); + // "System::malloc heap corruption detected: " + // "the last two pointers on the freelist are identical (during tinyMalloc)."); + } +# endif + + // NULL out the entry to help detect corruption + tinyPool[tinyPoolSize] = NULL; + } + + return ptr; + } + + /** Returns true if this is a pointer into the tiny heap. */ + bool inTinyHeap(void* ptr) { + return + (ptr >= tinyHeap) && + (ptr < (uint8*)tinyHeap + maxTinyBuffers * tinyBufferSize); + } + + void tinyFree(void* ptr) { + assert(ptr); + assert(tinyPoolSize < maxTinyBuffers); + // "Tried to free a tiny pool buffer when the tiny pool freelist is full."); + +# ifdef G3D_DEBUG + if (tinyPoolSize > 0) { + void* prevOnHeap = tinyPool[tinyPoolSize - 1]; + assert(prevOnHeap != ptr); +// "System::malloc heap corruption detected: " +// "the last two pointers on the freelist are identical (during tinyFree)."); + } +# endif + + assert(tinyPool[tinyPoolSize] == NULL); + + // Put the pointer back into the free list + tinyPool[tinyPoolSize] = ptr; + ++tinyPoolSize; + + } + + void flushPool(MemBlock* pool, int& poolSize) { + for (int i = 0; i < poolSize; ++i) { + ::free(pool[i].ptr); + pool[i].ptr = NULL; + pool[i].bytes = 0; + } + poolSize = 0; + } + + + /** Allocate out of a specific pool-> Return NULL if no suitable + memory was found. + + */ + void* malloc(MemBlock* pool, int& poolSize, size_t bytes) { + + // OPT: find the smallest block that satisfies the request. + + // See if there's something we can use in the buffer pool-> + // Search backwards since usually we'll re-use the last one. + for (int i = (int)poolSize - 1; i >= 0; --i) { + if (pool[i].bytes >= bytes) { + // We found a suitable entry in the pool-> + + // No need to offset the pointer; it is already offset + void* ptr = pool[i].ptr; + + // Remove this element from the pool + --poolSize; + pool[i] = pool[poolSize]; + + return ptr; + } + } + + return NULL; + } + +public: + + /** Count of memory allocations that have occurred. */ + int totalMallocs; + int mallocsFromTinyPool; + int mallocsFromSmallPool; + int mallocsFromMedPool; + + /** Amount of memory currently allocated (according to the application). + This does not count the memory still remaining in the buffer pool, + but does count extra memory required for rounding off to the size + of a buffer. + Primarily useful for detecting leaks.*/ + // TODO: make me an atomic int! + volatile int bytesAllocated; + + BufferPool() { + totalMallocs = 0; + + mallocsFromTinyPool = 0; + mallocsFromSmallPool = 0; + mallocsFromMedPool = 0; + + bytesAllocated = true; + + tinyPoolSize = 0; + tinyHeap = NULL; + + smallPoolSize = 0; + + medPoolSize = 0; + + + // Initialize the tiny heap as a bunch of pointers into one + // pre-allocated buffer. + tinyHeap = ::malloc(maxTinyBuffers * tinyBufferSize); + for (int i = 0; i < maxTinyBuffers; ++i) { + tinyPool[i] = (uint8*)tinyHeap + (tinyBufferSize * i); + } + tinyPoolSize = maxTinyBuffers; + +#if 0 ///---------------------------------- old mutex +# ifdef G3D_WIN32 + InitializeCriticalSection(&mutex); +# else + pthread_mutex_init(&mutex, NULL); +# endif +#endif ///---------------------------------- old mutex + } + + + ~BufferPool() { + ::free(tinyHeap); +#if 0 //-------------------------------- old mutex +# ifdef G3D_WIN32 + DeleteCriticalSection(&mutex); +# else + // No destruction on pthreads +# endif +#endif //--------------------------------old mutex + } + + + void* realloc(void* ptr, size_t bytes) { + if (ptr == NULL) { + return malloc(bytes); + } + + if (inTinyHeap(ptr)) { + if (bytes <= tinyBufferSize) { + // The old pointer actually had enough space. + return ptr; + } else { + // Free the old pointer and malloc + + void* newPtr = malloc(bytes); + System::memcpy(newPtr, ptr, tinyBufferSize); + tinyFree(ptr); + return newPtr; + + } + } else { + // In one of our heaps. + + // See how big the block really was + size_t realSize = *(uint32*)USERPTR_TO_REALPTR(ptr); + if (bytes <= realSize) { + // The old block was big enough. + return ptr; + } + + // Need to reallocate + void* newPtr = malloc(bytes); + System::memcpy(newPtr, ptr, realSize); + free(ptr); + return newPtr; + } + } + + + void* malloc(size_t bytes) { + lock(); + ++totalMallocs; + + if (bytes <= tinyBufferSize) { + + void* ptr = tinyMalloc(bytes); + + if (ptr) { + ++mallocsFromTinyPool; + unlock(); + return ptr; + } + + } + + // Failure to allocate a tiny buffer is allowed to flow + // through to a small buffer + if (bytes <= smallBufferSize) { + + void* ptr = malloc(smallPool, smallPoolSize, bytes); + + if (ptr) { + ++mallocsFromSmallPool; + unlock(); + return ptr; + } + + } else if (bytes <= medBufferSize) { + // Note that a small allocation failure does *not* fall + // through into a medium allocation because that would + // waste the medium buffer's resources. + + void* ptr = malloc(medPool, medPoolSize, bytes); + + if (ptr) { + ++mallocsFromMedPool; + unlock(); + debugAssertM(ptr != NULL, "BufferPool::malloc returned NULL"); + return ptr; + } + } + + bytesAllocated += REALBLOCK_SIZE(bytes); + unlock(); + + // Heap allocate + + // Allocate 4 extra bytes for our size header (unfortunate, + // since malloc already added its own header). + void* ptr = ::malloc(REALBLOCK_SIZE(bytes)); + + if (ptr == NULL) { + // Flush memory pools to try and recover space + flushPool(smallPool, smallPoolSize); + flushPool(medPool, medPoolSize); + ptr = ::malloc(REALBLOCK_SIZE(bytes)); + } + + if (ptr == NULL) { + if ((System::outOfMemoryCallback() != NULL) && + (System::outOfMemoryCallback()(REALBLOCK_SIZE(bytes), true) == true)) { + // Re-attempt the malloc + ptr = ::malloc(REALBLOCK_SIZE(bytes)); + } + } + + if (ptr == NULL) { + if (System::outOfMemoryCallback() != NULL) { + // Notify the application + System::outOfMemoryCallback()(REALBLOCK_SIZE(bytes), false); + } +# ifdef G3D_DEBUG + debugPrintf("::malloc(%d) returned NULL\n", (int)REALBLOCK_SIZE(bytes)); +# endif + debugAssertM(ptr != NULL, + "::malloc returned NULL. Either the " + "operating system is out of memory or the " + "heap is corrupt."); + return NULL; + } + + *(uint32*)ptr = bytes; + + return REALPTR_TO_USERPTR(ptr); + } + + + void free(void* ptr) { + if (ptr == NULL) { + // Free does nothing on null pointers + return; + } + + assert(isValidPointer(ptr)); + + if (inTinyHeap(ptr)) { + lock(); + tinyFree(ptr); + unlock(); + return; + } + + uint32 bytes = *(uint32*)USERPTR_TO_REALPTR(ptr); + + lock(); + if (bytes <= smallBufferSize) { + if (smallPoolSize < maxSmallBuffers) { + smallPool[smallPoolSize] = MemBlock(ptr, bytes); + ++smallPoolSize; + unlock(); + return; + } + } else if (bytes <= medBufferSize) { + if (medPoolSize < maxMedBuffers) { + medPool[medPoolSize] = MemBlock(ptr, bytes); + ++medPoolSize; + unlock(); + return; + } + } + bytesAllocated -= REALBLOCK_SIZE(bytes); + unlock(); + + // Free; the buffer pools are full or this is too big to store. + ::free(USERPTR_TO_REALPTR(ptr)); + } + + std::string performance() const { + if (totalMallocs > 0) { + int pooled = mallocsFromTinyPool + + mallocsFromSmallPool + + mallocsFromMedPool; + + int total = totalMallocs; + + return format("malloc performance: %5.1f%% <= %db, %5.1f%% <= %db, " + "%5.1f%% <= %db, %5.1f%% > %db", + 100.0 * mallocsFromTinyPool / total, + BufferPool::tinyBufferSize, + 100.0 * mallocsFromSmallPool / total, + BufferPool::smallBufferSize, + 100.0 * mallocsFromMedPool / total, + BufferPool::medBufferSize, + 100.0 * (1.0 - (double)pooled / total), + BufferPool::medBufferSize); + } else { + return "No System::malloc calls made yet."; + } + } + + std::string status() const { + return format("preallocated shared buffers: %5d/%d x %db", + maxTinyBuffers - tinyPoolSize, maxTinyBuffers, tinyBufferSize); + } +}; + +// Dynamically allocated because we need to ensure that +// the buffer pool is still around when the last global variable +// is deallocated. +static BufferPool* bufferpool = NULL; + +std::string System::mallocPerformance() { +#ifndef NO_BUFFERPOOL + return bufferpool->performance(); +#else + return "NO_BUFFERPOOL"; +#endif +} + +std::string System::mallocStatus() { +#ifndef NO_BUFFERPOOL + return bufferpool->status(); +#else + return "NO_BUFFERPOOL"; +#endif +} + + +void System::resetMallocPerformanceCounters() { +#ifndef NO_BUFFERPOOL + bufferpool->totalMallocs = 0; + bufferpool->mallocsFromMedPool = 0; + bufferpool->mallocsFromSmallPool = 0; + bufferpool->mallocsFromTinyPool = 0; +#endif +} + + +#ifndef NO_BUFFERPOOL +inline void initMem() { + // Putting the test here ensures that the system is always + // initialized, even when globals are being allocated. + static bool initialized = false; + if (! initialized) { + bufferpool = new BufferPool(); + initialized = true; + } +} +#endif + + +void* System::malloc(size_t bytes) { +#ifndef NO_BUFFERPOOL + initMem(); + return bufferpool->malloc(bytes); +#else + return ::malloc(bytes); +#endif +} + +void* System::calloc(size_t n, size_t x) { +#ifndef NO_BUFFERPOOL + void* b = System::malloc(n * x); + debugAssertM(b != NULL, "System::malloc returned NULL"); + debugAssertM(isValidHeapPointer(b), "System::malloc returned an invalid pointer"); + System::memset(b, 0, n * x); + return b; +#else + return ::calloc(n, x); +#endif +} + + +void* System::realloc(void* block, size_t bytes) { +#ifndef NO_BUFFERPOOL + initMem(); + return bufferpool->realloc(block, bytes); +#else + return ::realloc(block, bytes); +#endif +} + + +void System::free(void* p) { +#ifndef NO_BUFFERPOOL + bufferpool->free(p); +#else + return ::free(p); +#endif +} + + +void* System::alignedMalloc(size_t bytes, size_t alignment) { + + alwaysAssertM(isPow2(alignment), "alignment must be a power of 2"); + + // We must align to at least a word boundary. + alignment = iMax(alignment, sizeof(void *)); + + // Pad the allocation size with the alignment size and the + // size of the redirect pointer. + size_t totalBytes = bytes + alignment + sizeof(void*); + + size_t truePtr = (size_t)System::malloc(totalBytes); + + if (truePtr == 0) { + // malloc returned NULL + return NULL; + } + + debugAssert(isValidHeapPointer((void*)truePtr)); + #ifdef G3D_WIN32 + // The blocks we return will not be valid Win32 debug heap + // pointers because they are offset + // debugAssert(_CrtIsValidPointer((void*)truePtr, totalBytes, TRUE) ); + #endif + + // The return pointer will be the next aligned location (we must at least + // leave space for the redirect pointer, however). + size_t alignedPtr = truePtr + sizeof(void*); + + // 2^n - 1 has the form 1111... in binary. + uint32 bitMask = (alignment - 1); + + // Advance forward until we reach an aligned location. + while ((alignedPtr & bitMask) != 0) { + alignedPtr += sizeof(void*); + } + + debugAssert(alignedPtr - truePtr + bytes <= totalBytes); + + // Immediately before the aligned location, write the true array location + // so that we can free it correctly. + size_t* redirectPtr = (size_t *)(alignedPtr - sizeof(void *)); + redirectPtr[0] = truePtr; + + debugAssert(isValidHeapPointer((void*)truePtr)); + + #ifdef G3D_WIN32 + debugAssert( _CrtIsValidPointer((void*)alignedPtr, bytes, TRUE) ); + #endif + return (void *)alignedPtr; +} + + +void System::alignedFree(void* _ptr) { + if (_ptr == NULL) { + return; + } + + size_t alignedPtr = (size_t)_ptr; + + // Back up one word from the pointer the user passed in. + // We now have a pointer to a pointer to the true start + // of the memory block. + size_t* redirectPtr = (size_t*)(alignedPtr - sizeof(void *)); + + // Dereference that pointer so that ptr = true start + void* truePtr = (void*)redirectPtr[0]; + + debugAssert(isValidHeapPointer((void*)truePtr)); + System::free(truePtr); +} + + +void System::setEnv(const std::string& name, const std::string& value) { + std::string cmd = name + "=" + value; +# ifdef G3D_WIN32 + _putenv(cmd.c_str()); +# else + // Many linux implementations of putenv expect char* + putenv(const_cast<char*>(cmd.c_str())); +# endif +} + + +const char* System::getEnv(const std::string& name) { + return getenv(name.c_str()); +} + + +static void var(TextOutput& t, const std::string& name, const std::string& val) { + t.writeSymbols(name,"="); + t.writeString(val); + t.writeNewline(); +} + + +static void var(TextOutput& t, const std::string& name, const bool val) { + t.writeSymbols(name, "=", val ? "Yes" : "No"); + t.writeNewline(); +} + + +static void var(TextOutput& t, const std::string& name, const int val) { + t.writeSymbols(name,"="); + t.writeNumber(val); + t.writeNewline(); +} + + +void System::describeSystem( + std::string& s) { + + TextOutput t; + describeSystem(t); + t.commitString(s); +} + +void System::describeSystem( + TextOutput& t) { + + t.writeSymbols("App", "{"); + t.writeNewline(); + t.pushIndent(); + { + var(t, "Name", System::currentProgramFilename()); + char cwd[1024]; + getcwd(cwd, 1024); + var(t, "cwd", std::string(cwd)); + } + t.popIndent(); + t.writeSymbols("}"); + t.writeNewline(); + t.writeNewline(); + + t.writeSymbols("OS", "{"); + t.writeNewline(); + t.pushIndent(); + { + var(t, "Name", System::operatingSystem()); + } + t.popIndent(); + t.writeSymbols("}"); + t.writeNewline(); + t.writeNewline(); + + t.writeSymbols("CPU", "{"); + t.writeNewline(); + t.pushIndent(); + { + var(t, "Vendor", System::cpuVendor()); + var(t, "Architecture", System::cpuArchitecture()); + var(t, "hasCPUID", System::hasCPUID()); + var(t, "hasMMX", System::hasMMX()); + var(t, "hasSSE", System::hasSSE()); + var(t, "hasSSE2", System::hasSSE2()); + var(t, "hasSSE3", System::hasSSE3()); + var(t, "has3DNow", System::has3DNow()); + var(t, "hasRDTSC", System::hasRDTSC()); + var(t, "numCores", System::numCores()); + } + t.popIndent(); + t.writeSymbols("}"); + t.writeNewline(); + t.writeNewline(); + + t.writeSymbols("G3D", "{"); + t.writeNewline(); + t.pushIndent(); + { + var(t, "Link version", G3D_VER); + var(t, "Compile version", System::version()); + } + t.popIndent(); + t.writeSymbols("}"); + t.writeNewline(); + t.writeNewline(); +} + + +void System::setClipboardText(const std::string& s) { +# ifdef G3D_WIN32 + if (OpenClipboard(NULL)) { + HGLOBAL hMem = GlobalAlloc(GHND | GMEM_DDESHARE, s.size() + 1); + if (hMem) { + char *pMem = (char*)GlobalLock(hMem); + strcpy(pMem, s.c_str()); + GlobalUnlock(hMem); + + EmptyClipboard(); + SetClipboardData(CF_TEXT, hMem); + } + + CloseClipboard(); + GlobalFree(hMem); + } +# endif +} + + +std::string System::getClipboardText() { + std::string s; + +# ifdef G3D_WIN32 + if (OpenClipboard(NULL)) { + HANDLE h = GetClipboardData(CF_TEXT); + + if (h) { + char* temp = (char*)GlobalLock(h); + if (temp) { + s = temp; + } + temp = NULL; + GlobalUnlock(h); + } + CloseClipboard(); + } +# endif + return s; +} + + +std::string System::currentDateString() { + time_t t1; + ::time(&t1); + tm* t = localtime(&t1); + return format("%d-%02d-%02d", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday); +} + +#ifdef _MSC_VER + +// VC on Intel +void System::cpuid(CPUIDFunction func, uint32& areg, uint32& breg, uint32& creg, uint32& dreg) { +#if !defined(G3D_64BIT) + // Can't copy from assembler direct to a function argument (which is on the stack) in VC. + uint32 a,b,c,d; + + // Intel assembler syntax + __asm { + mov eax, func // eax <- func + mov ecx, 0 + cpuid + mov a, eax + mov b, ebx + mov c, ecx + mov d, edx + } + areg = a; + breg = b; + creg = c; + dreg = d; +#else + int CPUInfo[4]; + __cpuid(CPUInfo, func); + memcpy(&areg, &CPUInfo[0], 4); + memcpy(&breg, &CPUInfo[1], 4); + memcpy(&creg, &CPUInfo[2], 4); + memcpy(&dreg, &CPUInfo[3], 4); +#endif +} + +#elif defined(G3D_OSX) && ! defined(G3D_OSX_INTEL) + +// non-intel OS X; no CPUID +void System::cpuid(CPUIDFunction func, uint32& eax, uint32& ebx, uint32& ecx, uint32& edx) { + eax = 0; + ebx = 0; + ecx = 0; + edx = 0; +} + +#else + +// See http://sam.zoy.org/blog/2007-04-13-shlib-with-non-pic-code-have-inline-assembly-and-pic-mix-well +// for a discussion of why the second version saves ebx; it allows 32-bit code to compile with the -fPIC option. +// On 64-bit x86, PIC code has a dedicated rip register for PIC so there is no ebx conflict. +void System::cpuid(CPUIDFunction func, uint32& eax, uint32& ebx, uint32& ecx, uint32& edx) { +#if ! defined(__PIC__) || defined(__x86_64__) + // AT&T assembler syntax + asm volatile( + "movl $0, %%ecx \n\n" /* Wipe ecx */ + "cpuid \n\t" + : "=a"(eax), "=b"(ebx), "=c"(ecx), "=d"(edx) + : "a"(func)); +#else + // AT&T assembler syntax + asm volatile( + "pushl %%ebx \n\t" /* save ebx */ + "movl $0, %%ecx \n\n" /* Wipe ecx */ + "cpuid \n\t" + "movl %%ebx, %1 \n\t" /* save what cpuid just put in %ebx */ + "popl %%ebx \n\t" /* restore the old ebx */ + : "=a"(eax), "=r"(ebx), "=c"(ecx), "=d"(edx) + : "a"(func)); +#endif +} + +#endif + +} // namespace diff --git a/externals/g3dlite/TextInput.cpp b/externals/g3dlite/TextInput.cpp new file mode 100644 index 00000000000..7276d8c66b2 --- /dev/null +++ b/externals/g3dlite/TextInput.cpp @@ -0,0 +1,1136 @@ +/** + @file TextInput.cpp + + @author Morgan McGuire, graphics3d.com + + @cite Based on a lexer written by Aaron Orenstein. + + @created 2001-11-27 + @edited 2008-07-14 + */ + +#include "G3D/fileutils.h" +#include "G3D/TextInput.h" +#include "G3D/BinaryInput.h" +#include "G3D/stringutils.h" + +#ifdef _MSC_VER +# pragma warning (push) +// conversion from 'int' to 'char', possible loss of data (TODO: fix underlying problems) +# pragma warning (disable: 4244) +#endif + +namespace G3D { + +Token TextInput::readSignificant() { + Token t; + do { + t = read(); + } while ((t.type() == Token::COMMENT) || (t.type() == Token::NEWLINE)); + return t; +} + + +double Token::number() const { + if (_type == NUMBER) { + std::string s = toLower(_string); + if (s == "-1.#ind00") { + return nan(); + } + + if (s == "1.#inf00") { + return inf(); + } + + if (s == "-1.#inf00") { + return -inf(); + } + + double n; + if ((_string.length() > 2) && + (_string[0] == '0') && + (_string[1] == 'x')) { + // Hex + uint32 i; + sscanf(_string.c_str(), "%x", &i); + n = i; + } else { + sscanf(_string.c_str(), "%lg", &n); + } + return n; + } else { + return 0.0; + } +} + + +TextInput::Settings::Settings () : + cppBlockComments(true), + cppLineComments(true), + otherLineComments(true), + escapeSequencesInStrings(true), + otherCommentCharacter('\0'), + otherCommentCharacter2('\0'), + generateCommentTokens(false), + generateNewlineTokens(false), + signedNumbers(true), + singleQuotedStrings(true), + singleQuoteCharacter('\''), + sourceFileName(), + startingLineNumberOffset(0), + msvcSpecials(true), + proofSymbols(false), + caseSensitive(true) +{ + trueSymbols.insert("true"); + falseSymbols.insert("false"); +} + + +Token TextInput::peek() { + if (stack.size() == 0) { + Token t = nextToken(); + push(t); + } + + return stack.front(); +} + + +int TextInput::peekLineNumber() { + return peek().line(); +} + + +int TextInput::peekCharacterNumber() { + return peek().character(); +} + + +Token TextInput::read() { + if (stack.size() > 0) { + Token t = stack.front(); + stack.pop_front(); + return t; + } else { + return nextToken(); + } +} + +static void toUpper(Set<std::string>& set) { + Array<std::string> symbols; + set.getMembers(symbols); + set.clear(); + for (int i = 0; i < symbols.size(); ++i) { + set.insert(toUpper(symbols[i])); + } +} + +void TextInput::init() { + currentCharOffset = 0; + charNumber = 1; + lineNumber = 1 + options.startingLineNumberOffset; + + if (! options.caseSensitive) { + // Convert true and false symbols to all uppercase for fast comparisons + toUpper(options.trueSymbols); + toUpper(options.falseSymbols); + } +} + + +void TextInput::push(const Token& t) { + stack.push_front(t); +} + + +bool TextInput::hasMore() { + return (peek()._type != Token::END); +} + + +int TextInput::eatInputChar() { + // Don't go off the end + if (currentCharOffset >= buffer.length()) { + return EOF; + } + + unsigned char c = buffer[currentCharOffset]; + ++currentCharOffset; + + // update lineNumber and charNumber to reflect the location of the *next* + // character which will be read. + + // increment line number for \r, \n and \r\n which matches Token::NEWLINE parsing + if (c == '\r') { + ++lineNumber; + charNumber = 1; + + // check for \r\n + if (currentCharOffset < buffer.length()) { + unsigned char c2 = buffer[currentCharOffset]; + if (c2 == '\n') { + c = c2; + ++currentCharOffset; + } + } + } else if (c == '\n') { + ++lineNumber; + charNumber = 1; + } else { + ++charNumber; + } + + return c; +} + +int TextInput::peekInputChar(int distance) { + // Don't go off the end + if ((currentCharOffset + distance) >= buffer.length()) { + return EOF; + } + + unsigned char c = buffer[currentCharOffset + distance]; + return c; +} + + +Token TextInput::nextToken() { + Token t; + + t._line = lineNumber; + t._character = charNumber; + t._type = Token::END; + t._extendedType = Token::END_TYPE; + + int c = peekInputChar(); + if (c == EOF) { + return t; + } + + // loop through white space, newlines and comments + // found before other tokens + bool whitespaceDone = false; + while (! whitespaceDone) { + whitespaceDone = true; + + // generate newlines tokens for '\n' and '\r' and '\r\n' + if (options.generateNewlineTokens && isNewline(c)) { + t._type = Token::NEWLINE; + t._extendedType = Token::NEWLINE_TYPE; + t._string = c; + + int c2 = peekInputChar(1); + if (c == '\r' && c2 == '\n') { + t._string += c2; + } + + eatInputChar(); + return t; + } else { + // Consume whitespace + while (isWhiteSpace(c)) { + c = eatAndPeekInputChar(); + } + } + + // update line and character number to include discarded whitespace + t._line = lineNumber; + t._character = charNumber; + + int c2 = peekInputChar(1); + + // parse comments and generate tokens if enabled + std::string commentString; + + // check for line comments first + bool isLineComment = false; + if (options.cppLineComments && (c == '/' && c2 == '/')) { + // set start of line comment and eat markers + isLineComment = true; + eatInputChar(); + eatInputChar(); + } else if ( options.otherCommentCharacter && + (options.otherCommentCharacter != '\0' && c == options.otherCommentCharacter) ) { + // set start of line comment and eat markers + isLineComment = true; + eatInputChar(); + } else if ( options.otherCommentCharacter && + (options.otherCommentCharacter2 != '\0' && c == options.otherCommentCharacter2) ) { + // set start of line comment and eat markers + isLineComment = true; + eatInputChar(); + } + + if (isLineComment) { + + // consume line comment to newline or EOF + c = peekInputChar(); + while (! isNewline(c) && c != EOF) { + // build comment string for token + commentString += c; + + c = eatAndPeekInputChar(); + } + + if (options.generateCommentTokens) { + t._type = Token::COMMENT; + t._extendedType = Token::LINE_COMMENT_TYPE; + t._string = commentString; + return t; + } else { + // There is whitespace after the comment (in particular, the + // newline that terminates the comment). There might also be + // whitespace at the start of the next line. + whitespaceDone = false; + } + + } else if (options.cppBlockComments && (c == '/' && c2 == '*')) { + // consume block comment to end-marker or EOF + + // consume both start-comment chars, can't let the trailing one + // help close the comment. + eatInputChar(); + eatInputChar(); + + c = peekInputChar(); + c2 = peekInputChar(1); + while (! ((c == '*') && (c2 == '/')) && (c != EOF)) { + commentString += c; + + eatInputChar(); + c = c2; + c2 = peekInputChar(1); + } + eatInputChar(); // eat closing '*' + eatInputChar(); // eat closing '/' + + c = peekInputChar(); + + if (options.generateCommentTokens) { + t._type = Token::COMMENT; + t._extendedType = Token::BLOCK_COMMENT_TYPE; + t._string = commentString; + return t; + } else { + // There is whitespace after the comment (in particular, the + // newline that terminates the comment). There might also be + // whitespace at the start of the next line. + whitespaceDone = false; + } + } + + } // while (! whitespaceDone) + + t._line = lineNumber; + t._character = charNumber; + + // handle EOF + if (c == EOF) { + return t; + } + + // Extended ASCII parses as itself, except for EOF + if (c > 127 && c < 255) { + t._type = Token::SYMBOL; + t._extendedType = Token::SYMBOL_TYPE; + t._string = c; + c = eatAndPeekInputChar(); + } + + + // Perform appropriate setup for a symbol (including setting up the token + // string to start with c), eat the input character, and overwrite + // 'c' with the peeked next input character. +#define SETUP_SYMBOL(c) \ + { \ + t._type = Token::SYMBOL; \ + t._extendedType = Token::SYMBOL_TYPE; \ + t._string = c; \ + c = eatAndPeekInputChar(); \ + } + + switch (c) { + + case '@': // Simple symbols -> just themselves. + case '(': + case ')': + case ',': + case ';': + case '{': + case '}': + case '[': + case ']': + case '#': + case '$': + case '?': + case '%': + SETUP_SYMBOL(c); + return t; + + case '-': // negative number, -, --, -=, or -> + SETUP_SYMBOL(c); + + switch (c) { + case '>': // -> + case '-': // -- + case '=': // -= + t._string += c; + eatInputChar(); + return t; + } + + if (options.signedNumbers + && (isDigit(c) || (c == '.' && isDigit(peekInputChar(1))))) { + + // Negative number. 'c' is still the first digit, and is + // the next input char. + + goto numLabel; + } + + // plain - + return t; + + case '+': // positive number, +, ++, or += + SETUP_SYMBOL(c); + + switch (c) { + case '+': // ++ + case '=': // += + t._string += c; + eatInputChar(); + return t; + } + + if (options.signedNumbers + && (isDigit(c) || (c == '.' && isDigit(peekInputChar(1))))) { + + // Positive number. 'c' is still the first digit, and is + // the next input char. + + goto numLabel; + } + + return t; + + case ':': // : or :: or ::> or ::= or := or :> + SETUP_SYMBOL(c); + + if (c == ':') { + t._string += c; + eatInputChar(); + + if (options.proofSymbols) { + c = peekInputChar(0); + + if ((c == '>') || (c == '=')) { + t._string += c; + eatInputChar(); + } + } + } + else if (options.proofSymbols && (c == '=' || c == '>')) { + t._string += c; + eatInputChar(); + } + return t; + + case '=': // = or == or => + SETUP_SYMBOL(c); + + if (c == '=') { + t._string += c; + eatInputChar(); + return t; + } else if (options.proofSymbols && (c == '>')) { + t._string += c; + eatInputChar(); + return t; + } + return t; + + case '*': // * or *= + case '/': // / or /= + case '!': // ! or != + case '~': // ~ or ~= + case '^': // ^ or ^= + SETUP_SYMBOL(c); + + if (c == '=') { + t._string += c; + eatInputChar(); + return t; + } + return t; + + case '>': // >, >>,or >= + case '<': // <<, <<, or <= or <- or <: + case '|': // ||, ||, or |= or |- + case '&': // &, &&, or &= + { + int orig_c = c; + SETUP_SYMBOL(c); + + if ((c == '=') || (orig_c == c)) { + t._string += c; + eatInputChar(); + return t; + } else if (options.proofSymbols) { + if ((orig_c == '<') && (c == '-')) { + t._string += c; + eatInputChar(); + } else if ((orig_c == '|') && (c == '-')) { + t._string += c; + eatInputChar(); + } else if ((orig_c == '<') && (c == ':')) { + t._string += c; + + c = eatAndPeekInputChar(); + + if (c == ':') { + t._string += c; + eatInputChar(); + } + } + } + } + return t; + + case '\\': // backslash or escaped comment char. + SETUP_SYMBOL(c); + + if ((options.otherCommentCharacter != '\0' + && c == options.otherCommentCharacter) + || (options.otherCommentCharacter2 != '\0' + && c == options.otherCommentCharacter2)) { + + // escaped comment character. Return the raw comment + // char (no backslash). + + t._string = c; + eatInputChar(); + return t; + } + return t; + + case '.': // number, ., .., or ... + if (isDigit(peekInputChar(1))) { + // We're parsing a float that began without a leading zero + goto numLabel; + } + + SETUP_SYMBOL(c); + + if (c == '.') { // .. or ... + t._string += c; + c = eatAndPeekInputChar(); + + if (c == '.') { // ... + t._string += c; + eatInputChar(); + } + return t; + } + + return t; + + } // switch (c) + +#undef SETUP_SYMBOL + +numLabel: + if (isDigit(c) || (c == '.')) { + + // A number. Note-- single dots have been + // parsed already, so a . indicates a number + // less than 1 in floating point form. + + // [0-9]*(\.[0-9][f]) or [0-9]+ or 0x[0-9,A-F]+ + + if (t._string != "-") { + // If we picked up a leading "-" sign above, keep it, + // otherwise drop the string parsed thus far + t._string = ""; + } + t._type = Token::NUMBER; + if (c == '.') { + t._extendedType = Token::FLOATING_POINT_TYPE; + } else { + t._extendedType = Token::INTEGER_TYPE; + } + + if ((c == '0') && (peekInputChar(1) == 'x')) { + // Hex number + t._string += "0x"; + + // skip the 0x + eatInputChar(); + eatInputChar(); + + c = peekInputChar(); + while (isDigit(c) || ((c >= 'A') && (c <= 'F')) || ((c >= 'a') && (c <= 'f'))) { + t._string += c; + c = eatAndPeekInputChar(); + } + + } else { + // Non-hex number + + // Read the part before the decimal. + while (isDigit(c)) { + t._string += c; + c = eatAndPeekInputChar(); + } + + // True if we are reading a floating-point special type + bool isSpecial = false; + + // Read the decimal, if one exists + if (c == '.') { + t._extendedType = Token::FLOATING_POINT_TYPE; + + // The '.' character was a decimal point, not the start of a + // method or range operator + t._string += c; + c = eatAndPeekInputChar(); + + // Floating point specials (msvc format only) + if (options.msvcSpecials && (c == '#')) { + isSpecial = true; + // We are reading a floating point special value + // of the form -1.#IND00, -1.#INF00, or 1.#INF00 + c = eatAndPeekInputChar(); + char test = c; + if (! options.caseSensitive) { + test = toupper(c); + } + if (test != 'I') { + throw BadMSVCSpecial + ( + "Incorrect floating-point special (inf or nan) " + "format.", + t.line(), charNumber); + } + c = eatAndPeekInputChar(); + test = c; + if (! options.caseSensitive) { + test = toupper(c); + } + if (test != 'N') { + throw BadMSVCSpecial + ( + "Incorrect floating-point special (inf or nan) " + "format.", + t.line(), charNumber); + } + t._string += "#IN"; + c = eatAndPeekInputChar(); + test = c; + if (! options.caseSensitive) { + test = toupper(c); + } + if ((test != 'F') && (test != 'D')) { + throw BadMSVCSpecial + ( + "Incorrect floating-point special (inf or nan) " + "format.", + t.line(), charNumber); + } + t._string += c; + for (int j = 0; j < 2; ++j) { + c = eatAndPeekInputChar(); + if (c != '0') { + throw BadMSVCSpecial + ( + "Incorrect floating-point special (inf or" + "nan) format.", + t.line(), charNumber); + } + t._string += (char)c; + } + + } else { + + // Read the part after the decimal + while (isDigit((char)c)) { + t._string += (char)c; + c = eatAndPeekInputChar(); + } + } + } + + if (! isSpecial && ((c == 'e') || (c == 'E'))) { + // Read exponent + t._extendedType = Token::FLOATING_POINT_TYPE; + t._string += c; + + c = eatAndPeekInputChar(); + if ((c == '-') || (c == '+')) { + t._string += c; + c = eatAndPeekInputChar(); + } + + while (isDigit(c)) { + t._string += c; + c = eatAndPeekInputChar(); + } + } + + if (! isSpecial && (t._extendedType == Token::FLOATING_POINT_TYPE) && (c == 'f')) { + // Trailing f on a float + t._string += c; + c = eatAndPeekInputChar(); + } + } + return t; + + } else if (isLetter(c) || (c == '_')) { + // Identifier or keyword + // [A-Za-z_][A-Za-z_0-9]* + + t._type = Token::SYMBOL; + t._extendedType = Token::SYMBOL_TYPE; + t._string = ""; + do { + t._string += c; + c = eatAndPeekInputChar(); + } while (isLetter(c) || isDigit(c) || (c == '_')); + + // See if this symbol is actually a boolean + if ((options.trueSymbols.size() > 0) || (options.falseSymbols.size() > 0)) { + std::string str = t._string; + if (! options.caseSensitive) { + str = toUpper(str); + } + if (options.trueSymbols.contains(str)) { + t._type = Token::BOOLEAN; + t._extendedType = Token::BOOLEAN_TYPE; + t._bool = true; + } else if (options.falseSymbols.contains(str)) { + t._type = Token::BOOLEAN; + t._extendedType = Token::BOOLEAN_TYPE; + t._bool = false; + } + } + + return t; + + } else if (c == '\"') { + + // Discard the double-quote. + eatInputChar(); + + // Double quoted string + parseQuotedString('\"', t); + return t; + + } else if (c == options.singleQuoteCharacter) { + + // Discard the single-quote. + eatInputChar(); + + if (options.singleQuotedStrings) { + // Single quoted string + parseQuotedString(options.singleQuoteCharacter, t); + } else { + t._string = c; + t._type = Token::SYMBOL; + t._extendedType = Token::SYMBOL_TYPE; + } + return t; + + } // end of special case tokens + + if (c == EOF) { + t._type = Token::END; + t._extendedType = Token::END_TYPE; + t._string = ""; + return t; + } + + // Some unknown token + debugAssertM(false, + format("Unrecognized token type beginning with character '%c' (ASCII %d)", + c, c)); + return t; +} + + +void TextInput::parseQuotedString(unsigned char delimiter, Token& t) { + + t._type = Token::STRING; + + if (delimiter == options.singleQuoteCharacter) { + t._extendedType = Token::SINGLE_QUOTED_TYPE; + } else { + t._extendedType = Token::DOUBLE_QUOTED_TYPE; + } + + while (true) { + // We're definitely going to consume the next input char, so we get + // it right now. This makes the condition handling below a bit easier. + int c = eatInputChar(); + + if (c == EOF) { + // END inside a quoted string. (We finish the string.) + break; + } + + if (options.escapeSequencesInStrings && (c == '\\')) { + // An escaped character. We're definitely going to consume it, + // so we get it (and consume it) now. + + c = eatInputChar(); + + switch (c) { + case 'r': + t._string += '\r'; + break; + case 'n': + t._string += '\n'; + break; + case 't': + t._string += '\t'; + break; + case '0': + t._string += '\0'; + break; + + case '\\': + case '\"': + t._string += (char)c; + break; + + default: + if (c == options.singleQuoteCharacter) { + t._string += (char)c; + break; + } + + if (((c == options.otherCommentCharacter) && + (options.otherCommentCharacter != '\0')) || + ((c == options.otherCommentCharacter2) && + (options.otherCommentCharacter2 != '\0'))) { + t._string += c; + } + // otherwise, some illegal escape sequence; skip it. + break; + + } // switch + + } else if (c == delimiter) { + // End of the string. Already consumed the character. + break; + } else { + // All other chars, go on to the string. Already consumed the + // character. + t._string += (char)c; + } + + } +} + +bool TextInput::readBoolean() { + Token t(read()); + + if (t._type == Token::BOOLEAN) { + return t.boolean(); + } + + // Push initial token back, and throw an error. We intentionally + // indicate that the wrong type is the type of the initial token. + // Logically, the number started there. + push(t); + throw WrongTokenType(options.sourceFileName, t.line(), t.character(), + Token::BOOLEAN, t._type); +} + +double TextInput::readNumber() { + Token t(read()); + + if (t._type == Token::NUMBER) { + return t.number(); + } + + // Even if signedNumbers is disabled, readNumber attempts to + // read a signed number, so we handle that case here. + if (! options.signedNumbers + && (t._type == Token::SYMBOL) + && ((t._string == "-") + || (t._string == "+"))) { + + Token t2(read()); + + if ((t2._type == Token::NUMBER) + && (t2._character == t._character + 1)) { + + if (t._string == "-") { + return -t2.number(); + } else { + return t2.number(); + } + } + + // push back the second token. + push(t2); + } + + // Push initial token back, and throw an error. We intentionally + // indicate that the wrong type is the type of the initial token. + // Logically, the number started there. + push(t); + throw WrongTokenType(options.sourceFileName, t.line(), t.character(), + Token::NUMBER, t._type); +} + + +Token TextInput::readStringToken() { + Token t(read()); + + if (t._type == Token::STRING) { // fast path + return t; + } + + push(t); + throw WrongTokenType(options.sourceFileName, t.line(), t.character(), + Token::STRING, t._type); +} + +std::string TextInput::readString() { + return readStringToken()._string; +} + +void TextInput::readString(const std::string& s) { + Token t(readStringToken()); + + if (t._string == s) { // fast path + return; + } + + push(t); + throw WrongString(options.sourceFileName, t.line(), t.character(), + s, t._string); +} + +Token TextInput::readCommentToken() { + Token t(read()); + + if (t._type == Token::COMMENT) { // fast path + return t; + } + + push(t); + throw WrongTokenType(options.sourceFileName, t.line(), t.character(), + Token::COMMENT, t._type); +} + +std::string TextInput::readComment() { + return readCommentToken()._string; +} + +void TextInput::readComment(const std::string& s) { + Token t(readCommentToken()); + + if (t._string == s) { // fast path + return; + } + + push(t); + throw WrongString(options.sourceFileName, t.line(), t.character(), + s, t._string); +} + +Token TextInput::readNewlineToken() { + Token t(read()); + + if (t._type == Token::NEWLINE) { // fast path + return t; + } + + push(t); + throw WrongTokenType(options.sourceFileName, t.line(), t.character(), + Token::NEWLINE, t._type); +} + +std::string TextInput::readNewline() { + return readNewlineToken()._string; +} + +void TextInput::readNewline(const std::string& s) { + Token t(readNewlineToken()); + + if (t._string == s) { // fast path + return; + } + + push(t); + throw WrongString(options.sourceFileName, t.line(), t.character(), + s, t._string); +} + +Token TextInput::readSymbolToken() { + Token t(read()); + + if (t._type == Token::SYMBOL) { // fast path + return t; + } + + push(t); + throw WrongTokenType(options.sourceFileName, t.line(), t.character(), + Token::SYMBOL, t._type); +} + + +std::string TextInput::readSymbol() { + return readSymbolToken()._string; +} + +void TextInput::readSymbol(const std::string& symbol) { + Token t(readSymbolToken()); + + if (t._string == symbol) { // fast path + return; + } + + push(t); + throw WrongSymbol(options.sourceFileName, t.line(), t.character(), + symbol, t._string); +} + + +TextInput::TextInput(const std::string& filename, const Settings& opt) : options(opt) { + init(); + std::string input = readWholeFile(filename); + + if (options.sourceFileName.empty()) { + options.sourceFileName = filename; + } + int n = input.size(); + buffer.resize(n); + System::memcpy(buffer.getCArray(), input.c_str(), n); +} + + +TextInput::TextInput(FS fs, const std::string& str, const Settings& opt) : options(opt) { + (void)fs; + init(); + if (options.sourceFileName.empty()) { + if (str.length() < 14) { + options.sourceFileName = std::string("\"") + str + "\""; + } else { + options.sourceFileName = std::string("\"") + str.substr(0, 10) + "...\""; + } + } + buffer.resize(str.length()); // we don't bother copying trailing NUL. + System::memcpy(buffer.getCArray(), str.c_str(), buffer.size()); +} + + +const std::string& TextInput::filename() const { + return options.sourceFileName; +} + +/////////////////////////////////////////////////////////////////////////////////// + +TextInput::TokenException::TokenException( + const std::string& src, + int ln, + int ch) : ParseError(src, ln, ch, format("%s(%d) : ", src.c_str(), ln)), + sourceFile(src) { +} + +/////////////////////////////////////////////////////////////////////////////////// + +static const char* tokenTypeToString(Token::Type t) { + switch (t) { + case Token::SYMBOL: + return "Token::SYMBOL"; + case Token::STRING: + return "Token::STRING"; + case Token::NUMBER: + return "Token::NUMBER"; + case Token::END: + return "Token::END"; + default: + debugAssertM(false, "Fell through switch"); + return "?"; + } +} + +TextInput::WrongTokenType::WrongTokenType( + const std::string& src, + int ln, + int ch, + Token::Type e, + Token::Type a) : + TokenException(src, ln, ch), expected(e), actual(a) { + + message += format("Expected token of type %s, found type %s.", + tokenTypeToString(e), tokenTypeToString(a)); +} + + +TextInput::BadMSVCSpecial::BadMSVCSpecial( + const std::string& src, + int ln, + int ch) : + TokenException(src, ln, ch) { +} + + +TextInput::WrongSymbol::WrongSymbol( + const std::string& src, + int ln, + int ch, + const std::string& e, + const std::string& a) : + TokenException(src, ln, ch), expected(e), actual(a) { + + message += format("Expected symbol '%s', found symbol '%s'.", + e.c_str(), a.c_str()); +} + + +TextInput::WrongString::WrongString( + const std::string& src, + int ln, + int ch, + const std::string& e, + const std::string& a) : + TokenException(src, ln, ch), expected(e), actual(a) { + + message += format("Expected string '%s', found string '%s'.", + e.c_str(), a.c_str()); +} + + +void deserialize(bool& b, TextInput& ti) { + b = ti.readSymbol() == "true"; +} + + +void deserialize(int& b, TextInput& ti) { + b = iRound(ti.readNumber()); +} + + +void deserialize(uint8& b, TextInput& ti) { + b = (uint8)iRound(ti.readNumber()); +} + + +void deserialize(double& b, TextInput& ti) { + b = ti.readNumber(); +} + + +void deserialize(float& b, TextInput& ti) { + b = (float)ti.readNumber(); +} + +} // namespace + +#ifdef _MSC_VER +# pragma warning (pop) +#endif diff --git a/externals/g3dlite/TextOutput.cpp b/externals/g3dlite/TextOutput.cpp new file mode 100644 index 00000000000..11347252eba --- /dev/null +++ b/externals/g3dlite/TextOutput.cpp @@ -0,0 +1,452 @@ +/** + @file TextOutput.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + @created 2004-06-21 + @edited 2006-08-14 + + Copyright 2000-2006, Morgan McGuire. + All rights reserved. + */ + +#include "G3D/TextOutput.h" +#include "G3D/Log.h" +#include "G3D/fileutils.h" + +namespace G3D { + +TextOutput::TextOutput(const TextOutput::Settings& opt) : + startingNewLine(true), + currentColumn(0), + inDQuote(false), + filename(""), + indentLevel(0) +{ + setOptions(opt); +} + + +TextOutput::TextOutput(const std::string& fil, const TextOutput::Settings& opt) : + startingNewLine(true), + currentColumn(0), + inDQuote(false), + filename(fil), + indentLevel(0) +{ + + setOptions(opt); +} + + +void TextOutput::setIndentLevel(int i) { + indentLevel = i; + + // If there were more pops than pushes, don't let that take us below 0 indent. + // Don't ever indent more than the number of columns. + indentSpaces = + iClamp(option.spacesPerIndent * indentLevel, + 0, + option.numColumns - 1); +} + + +void TextOutput::setOptions(const Settings& _opt) { + option = _opt; + + debugAssert(option.numColumns > 1); + + setIndentLevel(indentLevel); + + newline = (option.newlineStyle == Settings::NEWLINE_WINDOWS) ? "\r\n" : "\n"; +} + + +void TextOutput::pushIndent() { + setIndentLevel(indentLevel + 1); +} + + +void TextOutput::popIndent() { + setIndentLevel(indentLevel - 1); +} + + +static std::string escape(const std::string& string) { + std::string result = ""; + + for (std::string::size_type i = 0; i < string.length(); ++i) { + char c = string.at(i); + switch (c) { + case '\0': + result += "\\0"; + break; + + case '\r': + result += "\\r"; + break; + + case '\n': + result += "\\n"; + break; + + case '\t': + result += "\\t"; + break; + + case '\\': + result += "\\\\"; + break; + + default: + result += c; + } + } + + return result; +} + +void TextOutput::writeString(const std::string& string) { + // Convert special characters to escape sequences + this->printf("\"%s\"", escape(string).c_str()); +} + + +void TextOutput::writeBoolean(bool b) { + this->printf("%s ", b ? option.trueSymbol.c_str() : option.falseSymbol.c_str()); +} + +void TextOutput::writeNumber(double n) { + this->printf("%f ", n); +} + + +void TextOutput::writeNumber(int n) { + this->printf("%d ", n); +} + + +void TextOutput::writeSymbol(const std::string& string) { + if (string.size() > 0) { + // TODO: check for legal symbols? + this->printf("%s ", string.c_str()); + } +} + +void TextOutput::writeSymbols( + const std::string& a, + const std::string& b, + const std::string& c, + const std::string& d, + const std::string& e, + const std::string& f) { + + writeSymbol(a); + writeSymbol(b); + writeSymbol(c); + writeSymbol(d); + writeSymbol(e); + writeSymbol(f); +} + + +void TextOutput::printf(const std::string formatString, ...) { + va_list argList; + va_start(argList, formatString); + this->vprintf(formatString.c_str(), argList); + va_end(argList); +} + + +void TextOutput::printf(const char* formatString, ...) { + va_list argList; + va_start(argList, formatString); + this->vprintf(formatString, argList); + va_end(argList); +} + + +void TextOutput::convertNewlines(const std::string& in, std::string& out) { + // TODO: can be significantly optimized in cases where + // single characters are copied in order by walking through + // the array and copying substrings as needed. + + if (option.convertNewlines) { + out = ""; + for (uint32 i = 0; i < in.size(); ++i) { + if (in[i] == '\n') { + // Unix newline + out += newline; + } else if ((in[i] == '\r') && (i + 1 < in.size()) && (in[i + 1] == '\n')) { + // Windows newline + out += newline; + ++i; + } else { + out += in[i]; + } + } + } else { + out = in; + } +} + + +void TextOutput::writeNewline() { + for (uint32 i = 0; i < newline.size(); ++i) { + indentAppend(newline[i]); + } +} + + +void TextOutput::writeNewlines(int numLines) { + for (int i = 0; i < numLines; ++i) { + writeNewline(); + } +} + + +void TextOutput::wordWrapIndentAppend(const std::string& str) { + // TODO: keep track of the last space character we saw so we don't + // have to always search. + + if ((option.wordWrap == Settings::WRAP_NONE) || + (currentColumn + (int)str.size() <= option.numColumns)) { + // No word-wrapping is needed + + // Add one character at a time. + // TODO: optimize for strings without newlines to add multiple + // characters. + for (uint32 i = 0; i < str.size(); ++i) { + indentAppend(str[i]); + } + return; + } + + // Number of columns to wrap against + int cols = option.numColumns - indentSpaces; + + // Copy forward until we exceed the column size, + // and then back up and try to insert newlines as needed. + for (uint32 i = 0; i < str.size(); ++i) { + + indentAppend(str[i]); + if ((str[i] == '\r') && (i + 1 < str.size()) && (str[i + 1] == '\n')) { + // \r\n, we need to hit the \n to enter word wrapping. + ++i; + indentAppend(str[i]); + } + + if (currentColumn >= cols) { + debugAssertM(str[i] != '\n' && str[i] != '\r', + "Should never enter word-wrapping on a newline character"); + + // True when we're allowed to treat a space as a space. + bool unquotedSpace = option.allowWordWrapInsideDoubleQuotes || ! inDQuote; + + // Cases: + // + // 1. Currently in a series of spaces that ends with a newline + // strip all spaces and let the newline + // flow through. + // + // 2. Currently in a series of spaces that does not end with a newline + // strip all spaces and replace them with single newline + // + // 3. Not in a series of spaces + // search backwards for a space, then execute case 2. + + // Index of most recent space + uint32 lastSpace = data.size() - 1; + + // How far back we had to look for a space + uint32 k = 0; + uint32 maxLookBackward = currentColumn - indentSpaces; + + // Search backwards (from current character), looking for a space. + while ((k < maxLookBackward) && + (lastSpace > 0) && + (! ((data[lastSpace] == ' ') && unquotedSpace))) { + --lastSpace; + ++k; + + if ((data[lastSpace] == '\"') && !option.allowWordWrapInsideDoubleQuotes) { + unquotedSpace = ! unquotedSpace; + } + } + + if (k == maxLookBackward) { + // We couldn't find a series of spaces + + if (option.wordWrap == Settings::WRAP_ALWAYS) { + // Strip the last character we wrote, force a newline, + // and replace the last character; + data.pop(); + writeNewline(); + indentAppend(str[i]); + } else { + // Must be Settings::WRAP_WITHOUT_BREAKING + // + // Don't write the newline; we'll come back to + // the word wrap code after writing another character + } + } else { + // We found a series of spaces. If they continue + // to the new string, strip spaces off both. Otherwise + // strip spaces from data only and insert a newline. + + // Find the start of the spaces. firstSpace is the index of the + // first non-space, looking backwards from lastSpace. + uint32 firstSpace = lastSpace; + while ((k < maxLookBackward) && + (firstSpace > 0) && + (data[firstSpace] == ' ')) { + --firstSpace; + ++k; + } + + if (k == maxLookBackward) { + ++firstSpace; + } + + if (lastSpace == (uint32)data.size() - 1) { + // Spaces continued up to the new string + data.resize(firstSpace + 1); + writeNewline(); + + // Delete the spaces from the new string + while ((i < str.size() - 1) && (str[i + 1] == ' ')) { + ++i; + } + } else { + // Spaces were somewhere in the middle of the old string. + // replace them with a newline. + + // Copy over the characters that should be saved + Array<char> temp; + for (uint32 j = lastSpace + 1; j < (uint32)data.size(); ++j) { + char c = data[j]; + + if (c == '\"') { + // Undo changes to quoting (they will be re-done + // when we paste these characters back on). + inDQuote = !inDQuote; + } + temp.append(c); + } + + // Remove those characters and replace with a newline. + data.resize(firstSpace + 1); + writeNewline(); + + // Write them back + for (uint32 j = 0; j < (uint32)temp.size(); ++j) { + indentAppend(temp[j]); + } + + // We are now free to continue adding from the + // new string, which may or may not begin with spaces. + + } // if spaces included new string + } // if hit indent + } // if line exceeded + } // iterate over str +} + + +void TextOutput::indentAppend(char c) { + + if (startingNewLine) { + for (int j = 0; j < indentSpaces; ++j) { + data.push(' '); + } + startingNewLine = false; + currentColumn = indentSpaces; + } + + data.push(c); + + // Don't increment the column count on return character + // newline is taken care of below. + if (c != '\r') { + ++currentColumn; + } + + if (c == '\"') { + inDQuote = ! inDQuote; + } + + startingNewLine = (c == '\n'); + if (startingNewLine) { + currentColumn = 0; + } +} + + +void TextOutput::vprintf(const char* formatString, va_list argPtr) { + std::string str = vformat(formatString, argPtr); + + std::string clean; + convertNewlines(str, clean); + wordWrapIndentAppend(clean); +} + + +void TextOutput::commit(bool flush) { + std::string p = filenamePath(filename); + if (! fileExists(p, false)) { + createDirectory(p); + } + + FILE* f = fopen(filename.c_str(), "wb"); + debugAssertM(f, "Could not open \"" + filename + "\""); + fwrite(data.getCArray(), 1, data.size(), f); + if (flush) { + fflush(f); + } + fclose(f); +} + + +void TextOutput::commitString(std::string& out) { + // Null terminate + data.push('\0'); + out = data.getCArray(); + data.pop(); +} + + +std::string TextOutput::commitString() { + std::string str; + commitString(str); + return str; +} + + + +///////////////////////////////////////////////////////////////////// + +void serialize(const float& b, TextOutput& to) { + to.writeNumber(b); +} + + +void serialize(const bool& b, TextOutput& to) { + to.writeSymbol(b ? "true" : "false"); +} + + +void serialize(const int& b, TextOutput& to) { + to.writeNumber(b); +} + + +void serialize(const uint8& b, TextOutput& to) { + to.writeNumber(b); +} + + +void serialize(const double& b, TextOutput& to) { + to.writeNumber(b); +} + + +} diff --git a/externals/g3dlite/Triangle.cpp b/externals/g3dlite/Triangle.cpp new file mode 100644 index 00000000000..253438ad5fb --- /dev/null +++ b/externals/g3dlite/Triangle.cpp @@ -0,0 +1,186 @@ +/** + @file Triangle.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2001-04-06 + @edited 2008-12-28 + + Copyright 2000-2009, Morgan McGuire. + All rights reserved. + */ + +#include "G3D/platform.h" +#include "G3D/Triangle.h" +#include "G3D/Plane.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/debugAssert.h" +#include "G3D/AABox.h" +#include "G3D/Ray.h" + +namespace G3D { + + +void Triangle::init(const Vector3& v0, const Vector3& v1, const Vector3& v2) { + + _plane = Plane(v0, v1, v2); + _vertex[0] = v0; + _vertex[1] = v1; + _vertex[2] = v2; + + static int next[] = {1,2,0}; + + for (int i = 0; i < 3; ++i) { + const Vector3& e = _vertex[next[i]] - _vertex[i]; + edgeMagnitude[i] = e.magnitude(); + + if (edgeMagnitude[i] == 0) { + edgeDirection[i] = Vector3::zero(); + } else { + edgeDirection[i] = e / (float)edgeMagnitude[i]; + } + } + + _edge01 = _vertex[1] - _vertex[0]; + _edge02 = _vertex[2] - _vertex[0]; + + _primaryAxis = _plane.normal().primaryAxis(); + _area = 0.5f * edgeDirection[0].cross(edgeDirection[2]).magnitude() * (edgeMagnitude[0] * edgeMagnitude[2]); + //0.5f * (_vertex[1] - _vertex[0]).cross(_vertex[2] - _vertex[0]).dot(_plane.normal()); +} + + +Triangle::Triangle() { + init(Vector3::zero(), Vector3::zero(), Vector3::zero()); +} + + +Triangle::Triangle(const Vector3& v0, const Vector3& v1, const Vector3& v2) { + init(v0, v1, v2); +} + + +Triangle::~Triangle() { +} + + +Triangle::Triangle(class BinaryInput& b) { + deserialize(b); +} + + +void Triangle::serialize(class BinaryOutput& b) { + _vertex[0].serialize(b); + _vertex[1].serialize(b); + _vertex[2].serialize(b); +} + + +void Triangle::deserialize(class BinaryInput& b) { + _vertex[0].deserialize(b); + _vertex[1].deserialize(b); + _vertex[2].deserialize(b); + init(_vertex[0], _vertex[1], _vertex[2]); +} + + +float Triangle::area() const { + return _area; +} + + +const Vector3& Triangle::normal() const { + return _plane.normal(); +} + + +const Plane& Triangle::plane() const { + return _plane; +} + + +Vector3 Triangle::center() const { + return (_vertex[0] + _vertex[1] + _vertex[2]) / 3.0; +} + +Vector3 Triangle::randomPoint() const { + // Choose a random point in the parallelogram + + float s = uniformRandom(); + float t = uniformRandom(); + + if (t > 1.0f - s) { + // Outside the triangle; reflect about the + // diagonal of the parallelogram + t = 1.0f - t; + s = 1.0f - s; + } + + return _edge01 * s + _edge02 * t + _vertex[0]; +} + + +void Triangle::getBounds(AABox& out) const { + Vector3 lo = _vertex[0]; + Vector3 hi = lo; + + for (int i = 1; i < 3; ++i) { + lo = lo.min(_vertex[i]); + hi = hi.max(_vertex[i]); + } + + out = AABox(lo, hi); +} + + +bool Triangle::intersect(const Ray& ray, float& distance, float baryCoord[3]) const { + static const float EPS = 1e-5f; + + // See RTR2 ch. 13.7 for the algorithm. + + const Vector3& e1 = edge01(); + const Vector3& e2 = edge02(); + const Vector3 p(ray.direction().cross(e2)); + const float a = e1.dot(p); + + if (abs(a) < EPS) { + // Determinant is ill-conditioned; abort early + return false; + } + + const float f = 1.0f / a; + const Vector3 s(ray.origin() - vertex(0)); + const float u = f * s.dot(p); + + if ((u < 0.0f) || (u > 1.0f)) { + // We hit the plane of the m_geometry, but outside the m_geometry + return false; + } + + const Vector3 q(s.cross(e1)); + const float v = f * ray.direction().dot(q); + + if ((v < 0.0f) || ((u + v) > 1.0f)) { + // We hit the plane of the triangle, but outside the triangle + return false; + } + + const float t = f * e2.dot(q); + + if ((t > 0.0f) && (t < distance)) { + // This is a new hit, closer than the previous one + distance = t; + + baryCoord[0] = 1.0 - u - v; + baryCoord[1] = u; + baryCoord[2] = v; + + return true; + } else { + // This hit is after the previous hit, so ignore it + return false; + } +} + +} // G3D diff --git a/externals/g3dlite/UprightFrame.cpp b/externals/g3dlite/UprightFrame.cpp new file mode 100644 index 00000000000..c80264bf4e8 --- /dev/null +++ b/externals/g3dlite/UprightFrame.cpp @@ -0,0 +1,132 @@ +/** + @file UprightFrame.cpp + Box class + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2007-05-02 + @edited 2007-05-05 +*/ + +#include "G3D/UprightFrame.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" + +namespace G3D { + +UprightFrame::UprightFrame(const CoordinateFrame& cframe) { + Vector3 look = cframe.lookVector(); + + yaw = G3D::pi() + atan2(look.x, look.z); + pitch = asin(look.y); + + translation = cframe.translation; +} + + +CoordinateFrame UprightFrame::toCoordinateFrame() const { + CoordinateFrame cframe; + + Matrix3 P(Matrix3::fromAxisAngle(Vector3::unitX(), pitch)); + Matrix3 Y(Matrix3::fromAxisAngle(Vector3::unitY(), yaw)); + + cframe.rotation = Y * P; + cframe.translation = translation; + + return cframe; +} + + +UprightFrame UprightFrame::operator+(const UprightFrame& other) const { + return UprightFrame(translation + other.translation, pitch + other.pitch, yaw + other.yaw); +} + + +UprightFrame UprightFrame::operator*(const float k) const { + return UprightFrame(translation * k, pitch * k, yaw * k); +} + + +void UprightFrame::unwrapYaw(UprightFrame* a, int N) { + // Use the first point to establish the wrapping convention + for (int i = 1; i < N; ++i) { + const float prev = a[i - 1].yaw; + float& cur = a[i].yaw; + + // No two angles should be more than pi (i.e., 180-degrees) apart. + if (abs(cur - prev) > G3D::pi()) { + // These angles must have wrapped at zero, causing them + // to be interpolated the long way. + + // Find canonical [0, 2pi] versions of these numbers + float p = wrap(prev, twoPi()); + float c = wrap(cur, twoPi()); + + // Find the difference -pi < diff < pi between the current and previous values + float diff = c - p; + if (diff < -G3D::pi()) { + diff += twoPi(); + } else if (diff > G3D::pi()) { + diff -= twoPi(); + } + + // Offset the current from the previous by the difference + // between them. + cur = prev + diff; + } + } +} + + +void UprightFrame::serialize(class BinaryOutput& b) const { + translation.serialize(b); + b.writeFloat32(pitch); + b.writeFloat32(yaw); +} + + +void UprightFrame::deserialize(class BinaryInput& b) { + translation.deserialize(b); + pitch = b.readFloat32(); + yaw = b.readFloat32(); +} + + +void UprightSpline::serialize(class BinaryOutput& b) const { + b.writeBool8(cyclic); + + b.writeInt32(control.size()); + for (int i = 0; i < control.size(); ++i) { + control[i].serialize(b); + } + b.writeInt32(time.size()); + for (int i = 0; i < time.size(); ++i) { + b.writeFloat32(time[i]); + } +} + + +void UprightSpline::deserialize(class BinaryInput& b) { + cyclic = b.readBool8(); + + control.resize(b.readInt32()); + for (int i = 0; i < control.size(); ++i) { + control[i].deserialize(b); + } + + if (b.hasMore()) { + time.resize(b.readInt32()); + for (int i = 0; i < time.size(); ++i) { + time[i] = b.readFloat32(); + } + debugAssert(time.size() == control.size()); + } else { + // Import legacy path + time.resize(control.size()); + for (int i = 0; i < time.size(); ++i) { + time[i] = i; + } + } +} + +} diff --git a/externals/g3dlite/Vector2.cpp b/externals/g3dlite/Vector2.cpp new file mode 100644 index 00000000000..ec0737c3755 --- /dev/null +++ b/externals/g3dlite/Vector2.cpp @@ -0,0 +1,224 @@ +/** + @file Vector2.cpp + + 2D vector class, used for texture coordinates primarily. + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @cite Portions based on Dave Eberly'x Magic Software Library + at http://www.magic-software.com + + @created 2001-06-02 + @edited 2009-11-16 + */ + +#include "G3D/platform.h" +#include <stdlib.h> +#include "G3D/Vector2.h" +#include "G3D/g3dmath.h" +#include "G3D/format.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/TextInput.h" +#include "G3D/TextOutput.h" +#include "G3D/Any.h" + +namespace G3D { + + +Vector2::Vector2(const Any& any) { + any.verifyName("Vector2"); + any.verifyType(Any::TABLE, Any::ARRAY); + any.verifySize(2); + + if (any.type() == Any::ARRAY) { + x = any[0]; + y = any[1]; + } else { + // Table + x = any["x"]; + y = any["y"]; + } +} + + +Vector2::operator Any() const { + Any any(Any::ARRAY, "Vector2"); + any.append(x, y); + return any; +} + + +const Vector2& Vector2::one() { + static const Vector2 v(1, 1); return v; +} + + +const Vector2& Vector2::zero() { + static Vector2 v(0, 0); + return v; +} + +const Vector2& Vector2::unitX() { + static Vector2 v(1, 0); + return v; +} + +const Vector2& Vector2::unitY() { + static Vector2 v(0, 1); + return v; +} + +const Vector2& Vector2::inf() { + static Vector2 v((float)G3D::finf(), (float)G3D::finf()); + return v; +} + + +const Vector2& Vector2::nan() { + static Vector2 v((float)G3D::fnan(), (float)G3D::fnan()); + return v; +} + + +const Vector2& Vector2::minFinite() { + static Vector2 v(-FLT_MAX, -FLT_MAX); + return v; +} + + +const Vector2& Vector2::maxFinite() { + static Vector2 v(FLT_MAX, FLT_MAX); + return v; +} + + +size_t Vector2::hashCode() const { + unsigned int xhash = (*(int*)(void*)(&x)); + unsigned int yhash = (*(int*)(void*)(&y)); + + return xhash + (yhash * 37); +} + + +Vector2::Vector2(BinaryInput& b) { + deserialize(b); +} + + +void Vector2::deserialize(BinaryInput& b) { + x = b.readFloat32(); + y = b.readFloat32(); +} + + +void Vector2::serialize(BinaryOutput& b) const { + b.writeFloat32(x); + b.writeFloat32(y); +} + + +void Vector2::deserialize(TextInput& t) { + t.readSymbol("("); + x = (float)t.readNumber(); + t.readSymbol(","); + y = (float)t.readNumber(); + t.readSymbol(")"); +} + + +void Vector2::serialize(TextOutput& t) const { + t.writeSymbol("("); + t.writeNumber(x); + t.writeSymbol(","); + t.writeNumber(y); + t.writeSymbol(")"); +} + +//---------------------------------------------------------------------------- + +Vector2 Vector2::random(G3D::Random& r) { + Vector2 result; + + do { + result = Vector2(r.uniform(-1, 1), r.uniform(-1, 1)); + + } while (result.squaredLength() >= 1.0f); + + result.unitize(); + + return result; +} + + +Vector2 Vector2::operator/ (float k) const { + return *this * (1.0f / k); +} + +Vector2& Vector2::operator/= (float k) { + this->x /= k; + this->y /= k; + return *this; +} + +//---------------------------------------------------------------------------- +float Vector2::unitize (float fTolerance) { + float fLength = length(); + + if (fLength > fTolerance) { + float fInvLength = 1.0f / fLength; + x *= fInvLength; + y *= fInvLength; + } else { + fLength = 0.0; + } + + return fLength; +} + +//---------------------------------------------------------------------------- + +std::string Vector2::toString() const { + return G3D::format("(%g, %g)", x, y); +} + +// 2-char swizzles + +Vector2 Vector2::xx() const { return Vector2 (x, x); } +Vector2 Vector2::yx() const { return Vector2 (y, x); } +Vector2 Vector2::xy() const { return Vector2 (x, y); } +Vector2 Vector2::yy() const { return Vector2 (y, y); } + +// 3-char swizzles + +Vector3 Vector2::xxx() const { return Vector3 (x, x, x); } +Vector3 Vector2::yxx() const { return Vector3 (y, x, x); } +Vector3 Vector2::xyx() const { return Vector3 (x, y, x); } +Vector3 Vector2::yyx() const { return Vector3 (y, y, x); } +Vector3 Vector2::xxy() const { return Vector3 (x, x, y); } +Vector3 Vector2::yxy() const { return Vector3 (y, x, y); } +Vector3 Vector2::xyy() const { return Vector3 (x, y, y); } +Vector3 Vector2::yyy() const { return Vector3 (y, y, y); } + +// 4-char swizzles + +Vector4 Vector2::xxxx() const { return Vector4 (x, x, x, x); } +Vector4 Vector2::yxxx() const { return Vector4 (y, x, x, x); } +Vector4 Vector2::xyxx() const { return Vector4 (x, y, x, x); } +Vector4 Vector2::yyxx() const { return Vector4 (y, y, x, x); } +Vector4 Vector2::xxyx() const { return Vector4 (x, x, y, x); } +Vector4 Vector2::yxyx() const { return Vector4 (y, x, y, x); } +Vector4 Vector2::xyyx() const { return Vector4 (x, y, y, x); } +Vector4 Vector2::yyyx() const { return Vector4 (y, y, y, x); } +Vector4 Vector2::xxxy() const { return Vector4 (x, x, x, y); } +Vector4 Vector2::yxxy() const { return Vector4 (y, x, x, y); } +Vector4 Vector2::xyxy() const { return Vector4 (x, y, x, y); } +Vector4 Vector2::yyxy() const { return Vector4 (y, y, x, y); } +Vector4 Vector2::xxyy() const { return Vector4 (x, x, y, y); } +Vector4 Vector2::yxyy() const { return Vector4 (y, x, y, y); } +Vector4 Vector2::xyyy() const { return Vector4 (x, y, y, y); } +Vector4 Vector2::yyyy() const { return Vector4 (y, y, y, y); } + + + +} // namespace diff --git a/externals/g3dlite/Vector3.cpp b/externals/g3dlite/Vector3.cpp new file mode 100644 index 00000000000..a53fa8269b7 --- /dev/null +++ b/externals/g3dlite/Vector3.cpp @@ -0,0 +1,507 @@ +/** + @file Vector3.cpp + + 3D vector class + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @cite Portions based on Dave Eberly's Magic Software Library at http://www.magic-software.com + + @created 2001-06-02 + @edited 2009-11-27 + */ + +#include <limits> +#include <stdlib.h> +#include "G3D/Vector3.h" +#include "G3D/g3dmath.h" +#include "G3D/stringutils.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/TextInput.h" +#include "G3D/TextOutput.h" +#include "G3D/Vector3int16.h" +#include "G3D/Matrix3.h" +#include "G3D/Vector2.h" +#include "G3D/Color3.h" +#include "G3D/Vector4int8.h" +#include "G3D/Vector3int32.h" +#include "G3D/Any.h" + +namespace G3D { + +Vector3::Vector3(const Any& any) { + any.verifyName("Vector3"); + any.verifyType(Any::TABLE, Any::ARRAY); + any.verifySize(3); + + if (any.type() == Any::ARRAY) { + x = any[0]; + y = any[1]; + z = any[2]; + } else { + // Table + x = any["x"]; + y = any["y"]; + z = any["z"]; + } +} + +Vector3::operator Any() const { + Any any(Any::ARRAY, "Vector3"); + any.append(x, y, z); + return any; +} + +Vector3::Vector3(const class Color3& v) : x(v.r), y(v.g), z(v.b) {} + +Vector3::Vector3(const class Vector3int32& v) : x((float)v.x), y((float)v.y), z((float)v.z) {} + +Vector3::Vector3(const Vector4int8& v) : x(v.x / 127.0f), y(v.y / 127.0f), z(v.z / 127.0f) {} + +Vector3::Vector3(const class Vector2& v, float _z) : x(v.x), y(v.y), z(_z) { +} + +Vector3& Vector3::ignore() { + static Vector3 v; + return v; +} + +const Vector3& Vector3::zero() { static const Vector3 v(0, 0, 0); return v; } +const Vector3& Vector3::one() { static const Vector3 v(1, 1, 1); return v; } +const Vector3& Vector3::unitX() { static const Vector3 v(1, 0, 0); return v; } +const Vector3& Vector3::unitY() { static const Vector3 v(0, 1, 0); return v; } +const Vector3& Vector3::unitZ() { static const Vector3 v(0, 0, 1); return v; } +const Vector3& Vector3::inf() { static const Vector3 v((float)G3D::finf(), (float)G3D::finf(), (float)G3D::finf()); return v; } +const Vector3& Vector3::nan() { static const Vector3 v((float)G3D::fnan(), (float)G3D::fnan(), (float)G3D::fnan()); return v; } +const Vector3& Vector3::minFinite(){ static const Vector3 v(-FLT_MAX, -FLT_MAX, -FLT_MAX); return v; } +const Vector3& Vector3::maxFinite(){ static const Vector3 v(FLT_MAX, FLT_MAX, FLT_MAX); return v; } + +Vector3::Axis Vector3::primaryAxis() const { + + Axis a = X_AXIS; + + double nx = abs(x); + double ny = abs(y); + double nz = abs(z); + + if (nx > ny) { + if (nx > nz) { + a = X_AXIS; + } else { + a = Z_AXIS; + } + } else { + if (ny > nz) { + a = Y_AXIS; + } else { + a = Z_AXIS; + } + } + + return a; +} + + +size_t Vector3::hashCode() const { + unsigned int xhash = (*(int*)(void*)(&x)); + unsigned int yhash = (*(int*)(void*)(&y)); + unsigned int zhash = (*(int*)(void*)(&z)); + + return xhash + (yhash * 37) + (zhash * 101); +} + +std::ostream& operator<<(std::ostream& os, const Vector3& v) { + return os << v.toString(); +} + + +//---------------------------------------------------------------------------- + +double frand() { + return rand() / (double) RAND_MAX; +} + +Vector3::Vector3(TextInput& t) { + deserialize(t); +} + +Vector3::Vector3(BinaryInput& b) { + deserialize(b); +} + + +Vector3::Vector3(const class Vector3int16& v) { + x = v.x; + y = v.y; + z = v.z; +} + + +void Vector3::deserialize(BinaryInput& b) { + x = b.readFloat32(); + y = b.readFloat32(); + z = b.readFloat32(); +} + + +void Vector3::deserialize(TextInput& t) { + t.readSymbol("("); + x = (float)t.readNumber(); + t.readSymbol(","); + y = (float)t.readNumber(); + t.readSymbol(","); + z = (float)t.readNumber(); + t.readSymbol(")"); +} + + +void Vector3::serialize(TextOutput& t) const { + t.writeSymbol("("); + t.writeNumber(x); + t.writeSymbol(","); + t.writeNumber(y); + t.writeSymbol(","); + t.writeNumber(z); + t.writeSymbol(")"); +} + + +void Vector3::serialize(BinaryOutput& b) const { + b.writeFloat32(x); + b.writeFloat32(y); + b.writeFloat32(z); +} + + +Vector3 Vector3::random(Random& r) { + Vector3 result; + r.sphere(result.x, result.y, result.z); + return result; +} + + +float Vector3::unitize(float fTolerance) { + float fMagnitude = magnitude(); + + if (fMagnitude > fTolerance) { + float fInvMagnitude = 1.0f / fMagnitude; + x *= fInvMagnitude; + y *= fInvMagnitude; + z *= fInvMagnitude; + } else { + fMagnitude = 0.0f; + } + + return fMagnitude; +} + + +Vector3 Vector3::reflectAbout(const Vector3& normal) const { + Vector3 out; + + Vector3 N = normal.direction(); + + // 2 * normal.dot(this) * normal - this + return N * 2 * this->dot(N) - *this; +} + + +Vector3 Vector3::cosHemiRandom(const Vector3& normal, Random& r) { + debugAssertM(G3D::fuzzyEq(normal.length(), 1.0f), + "cosHemiRandom requires its argument to have unit length"); + + float x, y, z; + r.cosHemi(x, y, z); + + // Make a coordinate system + const Vector3& Z = normal; + + Vector3 X, Y; + normal.getTangents(X, Y); + + return + x * X + + y * Y + + z * Z; +} + + +Vector3 Vector3::cosPowHemiRandom(const Vector3& normal, const float k, Random& r) { + debugAssertM(G3D::fuzzyEq(normal.length(), 1.0f), + "cosPowHemiRandom requires its argument to have unit length"); + + float x, y, z; + r.cosPowHemi(k, x, y, z); + + // Make a coordinate system + const Vector3& Z = normal; + + Vector3 X, Y; + normal.getTangents(X, Y); + + return + x * X + + y * Y + + z * Z; +} + + +Vector3 Vector3::hemiRandom(const Vector3& normal, Random& r) { + const Vector3& V = Vector3::random(r); + + if (V.dot(normal) < 0) { + return -V; + } else { + return V; + } +} + +//---------------------------------------------------------------------------- + +Vector3 Vector3::reflectionDirection(const Vector3& normal) const { + return -reflectAbout(normal).direction(); +} + +//---------------------------------------------------------------------------- + +Vector3 Vector3::refractionDirection( + const Vector3& normal, + float iInside, + float iOutside) const { + + // From pg. 24 of Henrik Wann Jensen. Realistic Image Synthesis + // Using Photon Mapping. AK Peters. ISBN: 1568811470. July 2001. + + // Invert the directions from Wann Jensen's formulation + // and normalize the vectors. + const Vector3 W = -direction(); + Vector3 N = normal.direction(); + + float h1 = iOutside; + float h2 = iInside; + + if (normal.dot(*this) > 0.0f) { + h1 = iInside; + h2 = iOutside; + N = -N; + } + + const float hRatio = h1 / h2; + const float WdotN = W.dot(N); + + float det = 1.0f - (float)square(hRatio) * (1.0f - (float)square(WdotN)); + + if (det < 0) { + // Total internal reflection + return Vector3::zero(); + } else { + return -hRatio * (W - WdotN * N) - N * sqrt(det); + } +} + +//---------------------------------------------------------------------------- +void Vector3::orthonormalize (Vector3 akVector[3]) { + // If the input vectors are v0, v1, and v2, then the Gram-Schmidt + // orthonormalization produces vectors u0, u1, and u2 as follows, + // + // u0 = v0/|v0| + // u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0| + // u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1| + // + // where |A| indicates length of vector A and A*B indicates dot + // product of vectors A and B. + + // compute u0 + akVector[0].unitize(); + + // compute u1 + float fDot0 = akVector[0].dot(akVector[1]); + akVector[1] -= akVector[0] * fDot0; + akVector[1].unitize(); + + // compute u2 + float fDot1 = akVector[1].dot(akVector[2]); + fDot0 = akVector[0].dot(akVector[2]); + akVector[2] -= akVector[0] * fDot0 + akVector[1] * fDot1; + akVector[2].unitize(); +} + +//---------------------------------------------------------------------------- +void Vector3::generateOrthonormalBasis (Vector3& rkU, Vector3& rkV, + Vector3& rkW, bool bUnitLengthW) { + if ( !bUnitLengthW ) + rkW.unitize(); + + if ( G3D::abs(rkW.x) >= G3D::abs(rkW.y) + && G3D::abs(rkW.x) >= G3D::abs(rkW.z) ) { + rkU.x = -rkW.y; + rkU.y = + rkW.x; + rkU.z = 0.0; + } else { + rkU.x = 0.0; + rkU.y = + rkW.z; + rkU.z = -rkW.y; + } + + rkU.unitize(); + rkV = rkW.cross(rkU); +} + +//---------------------------------------------------------------------------- + +std::string Vector3::toString() const { + return G3D::format("(%g, %g, %g)", x, y, z); +} + + +//---------------------------------------------------------------------------- + +Matrix3 Vector3::cross() const { + return Matrix3( 0, -z, y, + z, 0, -x, + -y, x, 0); +} + + +void serialize(const Vector3::Axis& a, class BinaryOutput& bo) { + bo.writeUInt8((uint8)a); +} + +void deserialize(Vector3::Axis& a, class BinaryInput& bi) { + a = (Vector3::Axis)bi.readUInt8(); +} + +//---------------------------------------------------------------------------- +// 2-char swizzles + +Vector2 Vector3::xx() const { return Vector2 (x, x); } +Vector2 Vector3::yx() const { return Vector2 (y, x); } +Vector2 Vector3::zx() const { return Vector2 (z, x); } +Vector2 Vector3::xy() const { return Vector2 (x, y); } +Vector2 Vector3::yy() const { return Vector2 (y, y); } +Vector2 Vector3::zy() const { return Vector2 (z, y); } +Vector2 Vector3::xz() const { return Vector2 (x, z); } +Vector2 Vector3::yz() const { return Vector2 (y, z); } +Vector2 Vector3::zz() const { return Vector2 (z, z); } + +// 3-char swizzles + +Vector3 Vector3::xxx() const { return Vector3 (x, x, x); } +Vector3 Vector3::yxx() const { return Vector3 (y, x, x); } +Vector3 Vector3::zxx() const { return Vector3 (z, x, x); } +Vector3 Vector3::xyx() const { return Vector3 (x, y, x); } +Vector3 Vector3::yyx() const { return Vector3 (y, y, x); } +Vector3 Vector3::zyx() const { return Vector3 (z, y, x); } +Vector3 Vector3::xzx() const { return Vector3 (x, z, x); } +Vector3 Vector3::yzx() const { return Vector3 (y, z, x); } +Vector3 Vector3::zzx() const { return Vector3 (z, z, x); } +Vector3 Vector3::xxy() const { return Vector3 (x, x, y); } +Vector3 Vector3::yxy() const { return Vector3 (y, x, y); } +Vector3 Vector3::zxy() const { return Vector3 (z, x, y); } +Vector3 Vector3::xyy() const { return Vector3 (x, y, y); } +Vector3 Vector3::yyy() const { return Vector3 (y, y, y); } +Vector3 Vector3::zyy() const { return Vector3 (z, y, y); } +Vector3 Vector3::xzy() const { return Vector3 (x, z, y); } +Vector3 Vector3::yzy() const { return Vector3 (y, z, y); } +Vector3 Vector3::zzy() const { return Vector3 (z, z, y); } +Vector3 Vector3::xxz() const { return Vector3 (x, x, z); } +Vector3 Vector3::yxz() const { return Vector3 (y, x, z); } +Vector3 Vector3::zxz() const { return Vector3 (z, x, z); } +Vector3 Vector3::xyz() const { return Vector3 (x, y, z); } +Vector3 Vector3::yyz() const { return Vector3 (y, y, z); } +Vector3 Vector3::zyz() const { return Vector3 (z, y, z); } +Vector3 Vector3::xzz() const { return Vector3 (x, z, z); } +Vector3 Vector3::yzz() const { return Vector3 (y, z, z); } +Vector3 Vector3::zzz() const { return Vector3 (z, z, z); } + +// 4-char swizzles + +Vector4 Vector3::xxxx() const { return Vector4 (x, x, x, x); } +Vector4 Vector3::yxxx() const { return Vector4 (y, x, x, x); } +Vector4 Vector3::zxxx() const { return Vector4 (z, x, x, x); } +Vector4 Vector3::xyxx() const { return Vector4 (x, y, x, x); } +Vector4 Vector3::yyxx() const { return Vector4 (y, y, x, x); } +Vector4 Vector3::zyxx() const { return Vector4 (z, y, x, x); } +Vector4 Vector3::xzxx() const { return Vector4 (x, z, x, x); } +Vector4 Vector3::yzxx() const { return Vector4 (y, z, x, x); } +Vector4 Vector3::zzxx() const { return Vector4 (z, z, x, x); } +Vector4 Vector3::xxyx() const { return Vector4 (x, x, y, x); } +Vector4 Vector3::yxyx() const { return Vector4 (y, x, y, x); } +Vector4 Vector3::zxyx() const { return Vector4 (z, x, y, x); } +Vector4 Vector3::xyyx() const { return Vector4 (x, y, y, x); } +Vector4 Vector3::yyyx() const { return Vector4 (y, y, y, x); } +Vector4 Vector3::zyyx() const { return Vector4 (z, y, y, x); } +Vector4 Vector3::xzyx() const { return Vector4 (x, z, y, x); } +Vector4 Vector3::yzyx() const { return Vector4 (y, z, y, x); } +Vector4 Vector3::zzyx() const { return Vector4 (z, z, y, x); } +Vector4 Vector3::xxzx() const { return Vector4 (x, x, z, x); } +Vector4 Vector3::yxzx() const { return Vector4 (y, x, z, x); } +Vector4 Vector3::zxzx() const { return Vector4 (z, x, z, x); } +Vector4 Vector3::xyzx() const { return Vector4 (x, y, z, x); } +Vector4 Vector3::yyzx() const { return Vector4 (y, y, z, x); } +Vector4 Vector3::zyzx() const { return Vector4 (z, y, z, x); } +Vector4 Vector3::xzzx() const { return Vector4 (x, z, z, x); } +Vector4 Vector3::yzzx() const { return Vector4 (y, z, z, x); } +Vector4 Vector3::zzzx() const { return Vector4 (z, z, z, x); } +Vector4 Vector3::xxxy() const { return Vector4 (x, x, x, y); } +Vector4 Vector3::yxxy() const { return Vector4 (y, x, x, y); } +Vector4 Vector3::zxxy() const { return Vector4 (z, x, x, y); } +Vector4 Vector3::xyxy() const { return Vector4 (x, y, x, y); } +Vector4 Vector3::yyxy() const { return Vector4 (y, y, x, y); } +Vector4 Vector3::zyxy() const { return Vector4 (z, y, x, y); } +Vector4 Vector3::xzxy() const { return Vector4 (x, z, x, y); } +Vector4 Vector3::yzxy() const { return Vector4 (y, z, x, y); } +Vector4 Vector3::zzxy() const { return Vector4 (z, z, x, y); } +Vector4 Vector3::xxyy() const { return Vector4 (x, x, y, y); } +Vector4 Vector3::yxyy() const { return Vector4 (y, x, y, y); } +Vector4 Vector3::zxyy() const { return Vector4 (z, x, y, y); } +Vector4 Vector3::xyyy() const { return Vector4 (x, y, y, y); } +Vector4 Vector3::yyyy() const { return Vector4 (y, y, y, y); } +Vector4 Vector3::zyyy() const { return Vector4 (z, y, y, y); } +Vector4 Vector3::xzyy() const { return Vector4 (x, z, y, y); } +Vector4 Vector3::yzyy() const { return Vector4 (y, z, y, y); } +Vector4 Vector3::zzyy() const { return Vector4 (z, z, y, y); } +Vector4 Vector3::xxzy() const { return Vector4 (x, x, z, y); } +Vector4 Vector3::yxzy() const { return Vector4 (y, x, z, y); } +Vector4 Vector3::zxzy() const { return Vector4 (z, x, z, y); } +Vector4 Vector3::xyzy() const { return Vector4 (x, y, z, y); } +Vector4 Vector3::yyzy() const { return Vector4 (y, y, z, y); } +Vector4 Vector3::zyzy() const { return Vector4 (z, y, z, y); } +Vector4 Vector3::xzzy() const { return Vector4 (x, z, z, y); } +Vector4 Vector3::yzzy() const { return Vector4 (y, z, z, y); } +Vector4 Vector3::zzzy() const { return Vector4 (z, z, z, y); } +Vector4 Vector3::xxxz() const { return Vector4 (x, x, x, z); } +Vector4 Vector3::yxxz() const { return Vector4 (y, x, x, z); } +Vector4 Vector3::zxxz() const { return Vector4 (z, x, x, z); } +Vector4 Vector3::xyxz() const { return Vector4 (x, y, x, z); } +Vector4 Vector3::yyxz() const { return Vector4 (y, y, x, z); } +Vector4 Vector3::zyxz() const { return Vector4 (z, y, x, z); } +Vector4 Vector3::xzxz() const { return Vector4 (x, z, x, z); } +Vector4 Vector3::yzxz() const { return Vector4 (y, z, x, z); } +Vector4 Vector3::zzxz() const { return Vector4 (z, z, x, z); } +Vector4 Vector3::xxyz() const { return Vector4 (x, x, y, z); } +Vector4 Vector3::yxyz() const { return Vector4 (y, x, y, z); } +Vector4 Vector3::zxyz() const { return Vector4 (z, x, y, z); } +Vector4 Vector3::xyyz() const { return Vector4 (x, y, y, z); } +Vector4 Vector3::yyyz() const { return Vector4 (y, y, y, z); } +Vector4 Vector3::zyyz() const { return Vector4 (z, y, y, z); } +Vector4 Vector3::xzyz() const { return Vector4 (x, z, y, z); } +Vector4 Vector3::yzyz() const { return Vector4 (y, z, y, z); } +Vector4 Vector3::zzyz() const { return Vector4 (z, z, y, z); } +Vector4 Vector3::xxzz() const { return Vector4 (x, x, z, z); } +Vector4 Vector3::yxzz() const { return Vector4 (y, x, z, z); } +Vector4 Vector3::zxzz() const { return Vector4 (z, x, z, z); } +Vector4 Vector3::xyzz() const { return Vector4 (x, y, z, z); } +Vector4 Vector3::yyzz() const { return Vector4 (y, y, z, z); } +Vector4 Vector3::zyzz() const { return Vector4 (z, y, z, z); } +Vector4 Vector3::xzzz() const { return Vector4 (x, z, z, z); } +Vector4 Vector3::yzzz() const { return Vector4 (y, z, z, z); } +Vector4 Vector3::zzzz() const { return Vector4 (z, z, z, z); } + + + + + + +} // namespace diff --git a/externals/g3dlite/Vector4.cpp b/externals/g3dlite/Vector4.cpp new file mode 100644 index 00000000000..f6abc1a6e0c --- /dev/null +++ b/externals/g3dlite/Vector4.cpp @@ -0,0 +1,520 @@ +/** + @file Vector4.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2001-07-09 + @edited 2009-11-29 + */ + +#include <stdlib.h> +#include <limits> +#include "G3D/Vector4.h" +#include "G3D/Color4.h" +#include "G3D/g3dmath.h" +#include "G3D/stringutils.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/Vector4int8.h" +#include "G3D/Matrix4.h" +#include "G3D/Any.h" + +namespace G3D { + +Vector4::Vector4(const Any& any) { + any.verifyName("Vector4"); + any.verifyType(Any::TABLE, Any::ARRAY); + any.verifySize(4); + + if (any.type() == Any::ARRAY) { + x = any[0]; + y = any[1]; + z = any[2]; + w = any[3]; + } else { + // Table + x = any["x"]; + y = any["y"]; + z = any["z"]; + w = any["w"]; + } +} + +Vector4::operator Any() const { + Any any(Any::ARRAY, "Vector4"); + any.append(x, y, z, w); + return any; +} + + +Vector4::Vector4(const Vector4int8& v) : x(v.x / 127.0f), y(v.y / 127.0f), z(v.z / 127.0f), w(v.w / 127.0f) { +} + + +const Vector4& Vector4::inf() { + static const Vector4 v((float)G3D::finf(), (float)G3D::finf(), (float)G3D::finf(), (float)G3D::finf()); + return v; +} + + +const Vector4& Vector4::zero() { + static const Vector4 v(0,0,0,0); + return v; +} + +const Vector4& Vector4::nan() { + static Vector4 v((float)G3D::fnan(), (float)G3D::fnan(), (float)G3D::fnan(), (float)G3D::fnan()); + return v; +} + + +size_t Vector4::hashCode() const { + unsigned int xhash = (*(int*)(void*)(&x)); + unsigned int yhash = (*(int*)(void*)(&y)); + unsigned int zhash = (*(int*)(void*)(&z)); + unsigned int whash = (*(int*)(void*)(&w)); + + return xhash + (yhash * 37) + (zhash * 101) + (whash * 241); +} + + +Vector4::Vector4(const class Color4& c) { + x = c.r; + y = c.g; + z = c.b; + w = c.a; +} + + +Vector4::Vector4(const Vector2& v1, const Vector2& v2) { + x = v1.x; + y = v1.y; + z = v2.x; + w = v2.y; +} + + +Vector4::Vector4(const Vector2& v1, float fz, float fw) { + x = v1.x; + y = v1.y; + z = fz; + w = fw; +} + +Vector4::Vector4(BinaryInput& b) { + deserialize(b); +} + + +void Vector4::deserialize(BinaryInput& b) { + x = b.readFloat32(); + y = b.readFloat32(); + z = b.readFloat32(); + w = b.readFloat32(); +} + + +void Vector4::serialize(BinaryOutput& b) const { + b.writeFloat32(x); + b.writeFloat32(y); + b.writeFloat32(z); + b.writeFloat32(w); +} + +//---------------------------------------------------------------------------- + +Vector4 Vector4::operator*(const Matrix4& M) const { + Vector4 result; + for (int i = 0; i < 4; ++i) { + result[i] = 0.0f; + for (int j = 0; j < 4; ++j) { + result[i] += (*this)[j] * M[j][i]; + } + } + return result; +} + + +Vector4 Vector4::operator/ (float fScalar) const { + Vector4 kQuot; + + if ( fScalar != 0.0 ) { + float fInvScalar = 1.0f / fScalar; + kQuot.x = fInvScalar * x; + kQuot.y = fInvScalar * y; + kQuot.z = fInvScalar * z; + kQuot.w = fInvScalar * w; + return kQuot; + } else { + return Vector4::inf(); + } +} + +//---------------------------------------------------------------------------- +Vector4& Vector4::operator/= (float fScalar) { + if (fScalar != 0.0f) { + float fInvScalar = 1.0f / fScalar; + x *= fInvScalar; + y *= fInvScalar; + z *= fInvScalar; + w *= fInvScalar; + } else { + *this = Vector4::inf(); + } + + return *this; +} + + +//---------------------------------------------------------------------------- + +std::string Vector4::toString() const { + return G3D::format("(%g, %g, %g, %g)", x, y, z, w); +} +// 2-char swizzles + +Vector2 Vector4::xx() const { return Vector2 (x, x); } +Vector2 Vector4::yx() const { return Vector2 (y, x); } +Vector2 Vector4::zx() const { return Vector2 (z, x); } +Vector2 Vector4::wx() const { return Vector2 (w, x); } +Vector2 Vector4::xy() const { return Vector2 (x, y); } +Vector2 Vector4::yy() const { return Vector2 (y, y); } +Vector2 Vector4::zy() const { return Vector2 (z, y); } +Vector2 Vector4::wy() const { return Vector2 (w, y); } +Vector2 Vector4::xz() const { return Vector2 (x, z); } +Vector2 Vector4::yz() const { return Vector2 (y, z); } +Vector2 Vector4::zz() const { return Vector2 (z, z); } +Vector2 Vector4::wz() const { return Vector2 (w, z); } +Vector2 Vector4::xw() const { return Vector2 (x, w); } +Vector2 Vector4::yw() const { return Vector2 (y, w); } +Vector2 Vector4::zw() const { return Vector2 (z, w); } +Vector2 Vector4::ww() const { return Vector2 (w, w); } + +// 3-char swizzles + +Vector3 Vector4::xxx() const { return Vector3 (x, x, x); } +Vector3 Vector4::yxx() const { return Vector3 (y, x, x); } +Vector3 Vector4::zxx() const { return Vector3 (z, x, x); } +Vector3 Vector4::wxx() const { return Vector3 (w, x, x); } +Vector3 Vector4::xyx() const { return Vector3 (x, y, x); } +Vector3 Vector4::yyx() const { return Vector3 (y, y, x); } +Vector3 Vector4::zyx() const { return Vector3 (z, y, x); } +Vector3 Vector4::wyx() const { return Vector3 (w, y, x); } +Vector3 Vector4::xzx() const { return Vector3 (x, z, x); } +Vector3 Vector4::yzx() const { return Vector3 (y, z, x); } +Vector3 Vector4::zzx() const { return Vector3 (z, z, x); } +Vector3 Vector4::wzx() const { return Vector3 (w, z, x); } +Vector3 Vector4::xwx() const { return Vector3 (x, w, x); } +Vector3 Vector4::ywx() const { return Vector3 (y, w, x); } +Vector3 Vector4::zwx() const { return Vector3 (z, w, x); } +Vector3 Vector4::wwx() const { return Vector3 (w, w, x); } +Vector3 Vector4::xxy() const { return Vector3 (x, x, y); } +Vector3 Vector4::yxy() const { return Vector3 (y, x, y); } +Vector3 Vector4::zxy() const { return Vector3 (z, x, y); } +Vector3 Vector4::wxy() const { return Vector3 (w, x, y); } +Vector3 Vector4::xyy() const { return Vector3 (x, y, y); } +Vector3 Vector4::yyy() const { return Vector3 (y, y, y); } +Vector3 Vector4::zyy() const { return Vector3 (z, y, y); } +Vector3 Vector4::wyy() const { return Vector3 (w, y, y); } +Vector3 Vector4::xzy() const { return Vector3 (x, z, y); } +Vector3 Vector4::yzy() const { return Vector3 (y, z, y); } +Vector3 Vector4::zzy() const { return Vector3 (z, z, y); } +Vector3 Vector4::wzy() const { return Vector3 (w, z, y); } +Vector3 Vector4::xwy() const { return Vector3 (x, w, y); } +Vector3 Vector4::ywy() const { return Vector3 (y, w, y); } +Vector3 Vector4::zwy() const { return Vector3 (z, w, y); } +Vector3 Vector4::wwy() const { return Vector3 (w, w, y); } +Vector3 Vector4::xxz() const { return Vector3 (x, x, z); } +Vector3 Vector4::yxz() const { return Vector3 (y, x, z); } +Vector3 Vector4::zxz() const { return Vector3 (z, x, z); } +Vector3 Vector4::wxz() const { return Vector3 (w, x, z); } +Vector3 Vector4::xyz() const { return Vector3 (x, y, z); } +Vector3 Vector4::yyz() const { return Vector3 (y, y, z); } +Vector3 Vector4::zyz() const { return Vector3 (z, y, z); } +Vector3 Vector4::wyz() const { return Vector3 (w, y, z); } +Vector3 Vector4::xzz() const { return Vector3 (x, z, z); } +Vector3 Vector4::yzz() const { return Vector3 (y, z, z); } +Vector3 Vector4::zzz() const { return Vector3 (z, z, z); } +Vector3 Vector4::wzz() const { return Vector3 (w, z, z); } +Vector3 Vector4::xwz() const { return Vector3 (x, w, z); } +Vector3 Vector4::ywz() const { return Vector3 (y, w, z); } +Vector3 Vector4::zwz() const { return Vector3 (z, w, z); } +Vector3 Vector4::wwz() const { return Vector3 (w, w, z); } +Vector3 Vector4::xxw() const { return Vector3 (x, x, w); } +Vector3 Vector4::yxw() const { return Vector3 (y, x, w); } +Vector3 Vector4::zxw() const { return Vector3 (z, x, w); } +Vector3 Vector4::wxw() const { return Vector3 (w, x, w); } +Vector3 Vector4::xyw() const { return Vector3 (x, y, w); } +Vector3 Vector4::yyw() const { return Vector3 (y, y, w); } +Vector3 Vector4::zyw() const { return Vector3 (z, y, w); } +Vector3 Vector4::wyw() const { return Vector3 (w, y, w); } +Vector3 Vector4::xzw() const { return Vector3 (x, z, w); } +Vector3 Vector4::yzw() const { return Vector3 (y, z, w); } +Vector3 Vector4::zzw() const { return Vector3 (z, z, w); } +Vector3 Vector4::wzw() const { return Vector3 (w, z, w); } +Vector3 Vector4::xww() const { return Vector3 (x, w, w); } +Vector3 Vector4::yww() const { return Vector3 (y, w, w); } +Vector3 Vector4::zww() const { return Vector3 (z, w, w); } +Vector3 Vector4::www() const { return Vector3 (w, w, w); } + +// 4-char swizzles + +Vector4 Vector4::xxxx() const { return Vector4 (x, x, x, x); } +Vector4 Vector4::yxxx() const { return Vector4 (y, x, x, x); } +Vector4 Vector4::zxxx() const { return Vector4 (z, x, x, x); } +Vector4 Vector4::wxxx() const { return Vector4 (w, x, x, x); } +Vector4 Vector4::xyxx() const { return Vector4 (x, y, x, x); } +Vector4 Vector4::yyxx() const { return Vector4 (y, y, x, x); } +Vector4 Vector4::zyxx() const { return Vector4 (z, y, x, x); } +Vector4 Vector4::wyxx() const { return Vector4 (w, y, x, x); } +Vector4 Vector4::xzxx() const { return Vector4 (x, z, x, x); } +Vector4 Vector4::yzxx() const { return Vector4 (y, z, x, x); } +Vector4 Vector4::zzxx() const { return Vector4 (z, z, x, x); } +Vector4 Vector4::wzxx() const { return Vector4 (w, z, x, x); } +Vector4 Vector4::xwxx() const { return Vector4 (x, w, x, x); } +Vector4 Vector4::ywxx() const { return Vector4 (y, w, x, x); } +Vector4 Vector4::zwxx() const { return Vector4 (z, w, x, x); } +Vector4 Vector4::wwxx() const { return Vector4 (w, w, x, x); } +Vector4 Vector4::xxyx() const { return Vector4 (x, x, y, x); } +Vector4 Vector4::yxyx() const { return Vector4 (y, x, y, x); } +Vector4 Vector4::zxyx() const { return Vector4 (z, x, y, x); } +Vector4 Vector4::wxyx() const { return Vector4 (w, x, y, x); } +Vector4 Vector4::xyyx() const { return Vector4 (x, y, y, x); } +Vector4 Vector4::yyyx() const { return Vector4 (y, y, y, x); } +Vector4 Vector4::zyyx() const { return Vector4 (z, y, y, x); } +Vector4 Vector4::wyyx() const { return Vector4 (w, y, y, x); } +Vector4 Vector4::xzyx() const { return Vector4 (x, z, y, x); } +Vector4 Vector4::yzyx() const { return Vector4 (y, z, y, x); } +Vector4 Vector4::zzyx() const { return Vector4 (z, z, y, x); } +Vector4 Vector4::wzyx() const { return Vector4 (w, z, y, x); } +Vector4 Vector4::xwyx() const { return Vector4 (x, w, y, x); } +Vector4 Vector4::ywyx() const { return Vector4 (y, w, y, x); } +Vector4 Vector4::zwyx() const { return Vector4 (z, w, y, x); } +Vector4 Vector4::wwyx() const { return Vector4 (w, w, y, x); } +Vector4 Vector4::xxzx() const { return Vector4 (x, x, z, x); } +Vector4 Vector4::yxzx() const { return Vector4 (y, x, z, x); } +Vector4 Vector4::zxzx() const { return Vector4 (z, x, z, x); } +Vector4 Vector4::wxzx() const { return Vector4 (w, x, z, x); } +Vector4 Vector4::xyzx() const { return Vector4 (x, y, z, x); } +Vector4 Vector4::yyzx() const { return Vector4 (y, y, z, x); } +Vector4 Vector4::zyzx() const { return Vector4 (z, y, z, x); } +Vector4 Vector4::wyzx() const { return Vector4 (w, y, z, x); } +Vector4 Vector4::xzzx() const { return Vector4 (x, z, z, x); } +Vector4 Vector4::yzzx() const { return Vector4 (y, z, z, x); } +Vector4 Vector4::zzzx() const { return Vector4 (z, z, z, x); } +Vector4 Vector4::wzzx() const { return Vector4 (w, z, z, x); } +Vector4 Vector4::xwzx() const { return Vector4 (x, w, z, x); } +Vector4 Vector4::ywzx() const { return Vector4 (y, w, z, x); } +Vector4 Vector4::zwzx() const { return Vector4 (z, w, z, x); } +Vector4 Vector4::wwzx() const { return Vector4 (w, w, z, x); } +Vector4 Vector4::xxwx() const { return Vector4 (x, x, w, x); } +Vector4 Vector4::yxwx() const { return Vector4 (y, x, w, x); } +Vector4 Vector4::zxwx() const { return Vector4 (z, x, w, x); } +Vector4 Vector4::wxwx() const { return Vector4 (w, x, w, x); } +Vector4 Vector4::xywx() const { return Vector4 (x, y, w, x); } +Vector4 Vector4::yywx() const { return Vector4 (y, y, w, x); } +Vector4 Vector4::zywx() const { return Vector4 (z, y, w, x); } +Vector4 Vector4::wywx() const { return Vector4 (w, y, w, x); } +Vector4 Vector4::xzwx() const { return Vector4 (x, z, w, x); } +Vector4 Vector4::yzwx() const { return Vector4 (y, z, w, x); } +Vector4 Vector4::zzwx() const { return Vector4 (z, z, w, x); } +Vector4 Vector4::wzwx() const { return Vector4 (w, z, w, x); } +Vector4 Vector4::xwwx() const { return Vector4 (x, w, w, x); } +Vector4 Vector4::ywwx() const { return Vector4 (y, w, w, x); } +Vector4 Vector4::zwwx() const { return Vector4 (z, w, w, x); } +Vector4 Vector4::wwwx() const { return Vector4 (w, w, w, x); } +Vector4 Vector4::xxxy() const { return Vector4 (x, x, x, y); } +Vector4 Vector4::yxxy() const { return Vector4 (y, x, x, y); } +Vector4 Vector4::zxxy() const { return Vector4 (z, x, x, y); } +Vector4 Vector4::wxxy() const { return Vector4 (w, x, x, y); } +Vector4 Vector4::xyxy() const { return Vector4 (x, y, x, y); } +Vector4 Vector4::yyxy() const { return Vector4 (y, y, x, y); } +Vector4 Vector4::zyxy() const { return Vector4 (z, y, x, y); } +Vector4 Vector4::wyxy() const { return Vector4 (w, y, x, y); } +Vector4 Vector4::xzxy() const { return Vector4 (x, z, x, y); } +Vector4 Vector4::yzxy() const { return Vector4 (y, z, x, y); } +Vector4 Vector4::zzxy() const { return Vector4 (z, z, x, y); } +Vector4 Vector4::wzxy() const { return Vector4 (w, z, x, y); } +Vector4 Vector4::xwxy() const { return Vector4 (x, w, x, y); } +Vector4 Vector4::ywxy() const { return Vector4 (y, w, x, y); } +Vector4 Vector4::zwxy() const { return Vector4 (z, w, x, y); } +Vector4 Vector4::wwxy() const { return Vector4 (w, w, x, y); } +Vector4 Vector4::xxyy() const { return Vector4 (x, x, y, y); } +Vector4 Vector4::yxyy() const { return Vector4 (y, x, y, y); } +Vector4 Vector4::zxyy() const { return Vector4 (z, x, y, y); } +Vector4 Vector4::wxyy() const { return Vector4 (w, x, y, y); } +Vector4 Vector4::xyyy() const { return Vector4 (x, y, y, y); } +Vector4 Vector4::yyyy() const { return Vector4 (y, y, y, y); } +Vector4 Vector4::zyyy() const { return Vector4 (z, y, y, y); } +Vector4 Vector4::wyyy() const { return Vector4 (w, y, y, y); } +Vector4 Vector4::xzyy() const { return Vector4 (x, z, y, y); } +Vector4 Vector4::yzyy() const { return Vector4 (y, z, y, y); } +Vector4 Vector4::zzyy() const { return Vector4 (z, z, y, y); } +Vector4 Vector4::wzyy() const { return Vector4 (w, z, y, y); } +Vector4 Vector4::xwyy() const { return Vector4 (x, w, y, y); } +Vector4 Vector4::ywyy() const { return Vector4 (y, w, y, y); } +Vector4 Vector4::zwyy() const { return Vector4 (z, w, y, y); } +Vector4 Vector4::wwyy() const { return Vector4 (w, w, y, y); } +Vector4 Vector4::xxzy() const { return Vector4 (x, x, z, y); } +Vector4 Vector4::yxzy() const { return Vector4 (y, x, z, y); } +Vector4 Vector4::zxzy() const { return Vector4 (z, x, z, y); } +Vector4 Vector4::wxzy() const { return Vector4 (w, x, z, y); } +Vector4 Vector4::xyzy() const { return Vector4 (x, y, z, y); } +Vector4 Vector4::yyzy() const { return Vector4 (y, y, z, y); } +Vector4 Vector4::zyzy() const { return Vector4 (z, y, z, y); } +Vector4 Vector4::wyzy() const { return Vector4 (w, y, z, y); } +Vector4 Vector4::xzzy() const { return Vector4 (x, z, z, y); } +Vector4 Vector4::yzzy() const { return Vector4 (y, z, z, y); } +Vector4 Vector4::zzzy() const { return Vector4 (z, z, z, y); } +Vector4 Vector4::wzzy() const { return Vector4 (w, z, z, y); } +Vector4 Vector4::xwzy() const { return Vector4 (x, w, z, y); } +Vector4 Vector4::ywzy() const { return Vector4 (y, w, z, y); } +Vector4 Vector4::zwzy() const { return Vector4 (z, w, z, y); } +Vector4 Vector4::wwzy() const { return Vector4 (w, w, z, y); } +Vector4 Vector4::xxwy() const { return Vector4 (x, x, w, y); } +Vector4 Vector4::yxwy() const { return Vector4 (y, x, w, y); } +Vector4 Vector4::zxwy() const { return Vector4 (z, x, w, y); } +Vector4 Vector4::wxwy() const { return Vector4 (w, x, w, y); } +Vector4 Vector4::xywy() const { return Vector4 (x, y, w, y); } +Vector4 Vector4::yywy() const { return Vector4 (y, y, w, y); } +Vector4 Vector4::zywy() const { return Vector4 (z, y, w, y); } +Vector4 Vector4::wywy() const { return Vector4 (w, y, w, y); } +Vector4 Vector4::xzwy() const { return Vector4 (x, z, w, y); } +Vector4 Vector4::yzwy() const { return Vector4 (y, z, w, y); } +Vector4 Vector4::zzwy() const { return Vector4 (z, z, w, y); } +Vector4 Vector4::wzwy() const { return Vector4 (w, z, w, y); } +Vector4 Vector4::xwwy() const { return Vector4 (x, w, w, y); } +Vector4 Vector4::ywwy() const { return Vector4 (y, w, w, y); } +Vector4 Vector4::zwwy() const { return Vector4 (z, w, w, y); } +Vector4 Vector4::wwwy() const { return Vector4 (w, w, w, y); } +Vector4 Vector4::xxxz() const { return Vector4 (x, x, x, z); } +Vector4 Vector4::yxxz() const { return Vector4 (y, x, x, z); } +Vector4 Vector4::zxxz() const { return Vector4 (z, x, x, z); } +Vector4 Vector4::wxxz() const { return Vector4 (w, x, x, z); } +Vector4 Vector4::xyxz() const { return Vector4 (x, y, x, z); } +Vector4 Vector4::yyxz() const { return Vector4 (y, y, x, z); } +Vector4 Vector4::zyxz() const { return Vector4 (z, y, x, z); } +Vector4 Vector4::wyxz() const { return Vector4 (w, y, x, z); } +Vector4 Vector4::xzxz() const { return Vector4 (x, z, x, z); } +Vector4 Vector4::yzxz() const { return Vector4 (y, z, x, z); } +Vector4 Vector4::zzxz() const { return Vector4 (z, z, x, z); } +Vector4 Vector4::wzxz() const { return Vector4 (w, z, x, z); } +Vector4 Vector4::xwxz() const { return Vector4 (x, w, x, z); } +Vector4 Vector4::ywxz() const { return Vector4 (y, w, x, z); } +Vector4 Vector4::zwxz() const { return Vector4 (z, w, x, z); } +Vector4 Vector4::wwxz() const { return Vector4 (w, w, x, z); } +Vector4 Vector4::xxyz() const { return Vector4 (x, x, y, z); } +Vector4 Vector4::yxyz() const { return Vector4 (y, x, y, z); } +Vector4 Vector4::zxyz() const { return Vector4 (z, x, y, z); } +Vector4 Vector4::wxyz() const { return Vector4 (w, x, y, z); } +Vector4 Vector4::xyyz() const { return Vector4 (x, y, y, z); } +Vector4 Vector4::yyyz() const { return Vector4 (y, y, y, z); } +Vector4 Vector4::zyyz() const { return Vector4 (z, y, y, z); } +Vector4 Vector4::wyyz() const { return Vector4 (w, y, y, z); } +Vector4 Vector4::xzyz() const { return Vector4 (x, z, y, z); } +Vector4 Vector4::yzyz() const { return Vector4 (y, z, y, z); } +Vector4 Vector4::zzyz() const { return Vector4 (z, z, y, z); } +Vector4 Vector4::wzyz() const { return Vector4 (w, z, y, z); } +Vector4 Vector4::xwyz() const { return Vector4 (x, w, y, z); } +Vector4 Vector4::ywyz() const { return Vector4 (y, w, y, z); } +Vector4 Vector4::zwyz() const { return Vector4 (z, w, y, z); } +Vector4 Vector4::wwyz() const { return Vector4 (w, w, y, z); } +Vector4 Vector4::xxzz() const { return Vector4 (x, x, z, z); } +Vector4 Vector4::yxzz() const { return Vector4 (y, x, z, z); } +Vector4 Vector4::zxzz() const { return Vector4 (z, x, z, z); } +Vector4 Vector4::wxzz() const { return Vector4 (w, x, z, z); } +Vector4 Vector4::xyzz() const { return Vector4 (x, y, z, z); } +Vector4 Vector4::yyzz() const { return Vector4 (y, y, z, z); } +Vector4 Vector4::zyzz() const { return Vector4 (z, y, z, z); } +Vector4 Vector4::wyzz() const { return Vector4 (w, y, z, z); } +Vector4 Vector4::xzzz() const { return Vector4 (x, z, z, z); } +Vector4 Vector4::yzzz() const { return Vector4 (y, z, z, z); } +Vector4 Vector4::zzzz() const { return Vector4 (z, z, z, z); } +Vector4 Vector4::wzzz() const { return Vector4 (w, z, z, z); } +Vector4 Vector4::xwzz() const { return Vector4 (x, w, z, z); } +Vector4 Vector4::ywzz() const { return Vector4 (y, w, z, z); } +Vector4 Vector4::zwzz() const { return Vector4 (z, w, z, z); } +Vector4 Vector4::wwzz() const { return Vector4 (w, w, z, z); } +Vector4 Vector4::xxwz() const { return Vector4 (x, x, w, z); } +Vector4 Vector4::yxwz() const { return Vector4 (y, x, w, z); } +Vector4 Vector4::zxwz() const { return Vector4 (z, x, w, z); } +Vector4 Vector4::wxwz() const { return Vector4 (w, x, w, z); } +Vector4 Vector4::xywz() const { return Vector4 (x, y, w, z); } +Vector4 Vector4::yywz() const { return Vector4 (y, y, w, z); } +Vector4 Vector4::zywz() const { return Vector4 (z, y, w, z); } +Vector4 Vector4::wywz() const { return Vector4 (w, y, w, z); } +Vector4 Vector4::xzwz() const { return Vector4 (x, z, w, z); } +Vector4 Vector4::yzwz() const { return Vector4 (y, z, w, z); } +Vector4 Vector4::zzwz() const { return Vector4 (z, z, w, z); } +Vector4 Vector4::wzwz() const { return Vector4 (w, z, w, z); } +Vector4 Vector4::xwwz() const { return Vector4 (x, w, w, z); } +Vector4 Vector4::ywwz() const { return Vector4 (y, w, w, z); } +Vector4 Vector4::zwwz() const { return Vector4 (z, w, w, z); } +Vector4 Vector4::wwwz() const { return Vector4 (w, w, w, z); } +Vector4 Vector4::xxxw() const { return Vector4 (x, x, x, w); } +Vector4 Vector4::yxxw() const { return Vector4 (y, x, x, w); } +Vector4 Vector4::zxxw() const { return Vector4 (z, x, x, w); } +Vector4 Vector4::wxxw() const { return Vector4 (w, x, x, w); } +Vector4 Vector4::xyxw() const { return Vector4 (x, y, x, w); } +Vector4 Vector4::yyxw() const { return Vector4 (y, y, x, w); } +Vector4 Vector4::zyxw() const { return Vector4 (z, y, x, w); } +Vector4 Vector4::wyxw() const { return Vector4 (w, y, x, w); } +Vector4 Vector4::xzxw() const { return Vector4 (x, z, x, w); } +Vector4 Vector4::yzxw() const { return Vector4 (y, z, x, w); } +Vector4 Vector4::zzxw() const { return Vector4 (z, z, x, w); } +Vector4 Vector4::wzxw() const { return Vector4 (w, z, x, w); } +Vector4 Vector4::xwxw() const { return Vector4 (x, w, x, w); } +Vector4 Vector4::ywxw() const { return Vector4 (y, w, x, w); } +Vector4 Vector4::zwxw() const { return Vector4 (z, w, x, w); } +Vector4 Vector4::wwxw() const { return Vector4 (w, w, x, w); } +Vector4 Vector4::xxyw() const { return Vector4 (x, x, y, w); } +Vector4 Vector4::yxyw() const { return Vector4 (y, x, y, w); } +Vector4 Vector4::zxyw() const { return Vector4 (z, x, y, w); } +Vector4 Vector4::wxyw() const { return Vector4 (w, x, y, w); } +Vector4 Vector4::xyyw() const { return Vector4 (x, y, y, w); } +Vector4 Vector4::yyyw() const { return Vector4 (y, y, y, w); } +Vector4 Vector4::zyyw() const { return Vector4 (z, y, y, w); } +Vector4 Vector4::wyyw() const { return Vector4 (w, y, y, w); } +Vector4 Vector4::xzyw() const { return Vector4 (x, z, y, w); } +Vector4 Vector4::yzyw() const { return Vector4 (y, z, y, w); } +Vector4 Vector4::zzyw() const { return Vector4 (z, z, y, w); } +Vector4 Vector4::wzyw() const { return Vector4 (w, z, y, w); } +Vector4 Vector4::xwyw() const { return Vector4 (x, w, y, w); } +Vector4 Vector4::ywyw() const { return Vector4 (y, w, y, w); } +Vector4 Vector4::zwyw() const { return Vector4 (z, w, y, w); } +Vector4 Vector4::wwyw() const { return Vector4 (w, w, y, w); } +Vector4 Vector4::xxzw() const { return Vector4 (x, x, z, w); } +Vector4 Vector4::yxzw() const { return Vector4 (y, x, z, w); } +Vector4 Vector4::zxzw() const { return Vector4 (z, x, z, w); } +Vector4 Vector4::wxzw() const { return Vector4 (w, x, z, w); } +Vector4 Vector4::xyzw() const { return Vector4 (x, y, z, w); } +Vector4 Vector4::yyzw() const { return Vector4 (y, y, z, w); } +Vector4 Vector4::zyzw() const { return Vector4 (z, y, z, w); } +Vector4 Vector4::wyzw() const { return Vector4 (w, y, z, w); } +Vector4 Vector4::xzzw() const { return Vector4 (x, z, z, w); } +Vector4 Vector4::yzzw() const { return Vector4 (y, z, z, w); } +Vector4 Vector4::zzzw() const { return Vector4 (z, z, z, w); } +Vector4 Vector4::wzzw() const { return Vector4 (w, z, z, w); } +Vector4 Vector4::xwzw() const { return Vector4 (x, w, z, w); } +Vector4 Vector4::ywzw() const { return Vector4 (y, w, z, w); } +Vector4 Vector4::zwzw() const { return Vector4 (z, w, z, w); } +Vector4 Vector4::wwzw() const { return Vector4 (w, w, z, w); } +Vector4 Vector4::xxww() const { return Vector4 (x, x, w, w); } +Vector4 Vector4::yxww() const { return Vector4 (y, x, w, w); } +Vector4 Vector4::zxww() const { return Vector4 (z, x, w, w); } +Vector4 Vector4::wxww() const { return Vector4 (w, x, w, w); } +Vector4 Vector4::xyww() const { return Vector4 (x, y, w, w); } +Vector4 Vector4::yyww() const { return Vector4 (y, y, w, w); } +Vector4 Vector4::zyww() const { return Vector4 (z, y, w, w); } +Vector4 Vector4::wyww() const { return Vector4 (w, y, w, w); } +Vector4 Vector4::xzww() const { return Vector4 (x, z, w, w); } +Vector4 Vector4::yzww() const { return Vector4 (y, z, w, w); } +Vector4 Vector4::zzww() const { return Vector4 (z, z, w, w); } +Vector4 Vector4::wzww() const { return Vector4 (w, z, w, w); } +Vector4 Vector4::xwww() const { return Vector4 (x, w, w, w); } +Vector4 Vector4::ywww() const { return Vector4 (y, w, w, w); } +Vector4 Vector4::zwww() const { return Vector4 (z, w, w, w); } +Vector4 Vector4::wwww() const { return Vector4 (w, w, w, w); } + + +}; // namespace diff --git a/externals/g3dlite/debugAssert.cpp b/externals/g3dlite/debugAssert.cpp new file mode 100644 index 00000000000..a87161b261f --- /dev/null +++ b/externals/g3dlite/debugAssert.cpp @@ -0,0 +1,389 @@ +/** + @file debugAssert.cpp + + Windows implementation of assertion routines. + + @maintainer Morgan McGuire, graphics3d.com + + @created 2001-08-26 + @edited 2009-06-02 + */ + +#include "G3D/debugAssert.h" +#include "G3D/platform.h" +#ifdef G3D_WIN32 + #include <tchar.h> +#endif +#include "G3D/format.h" +#include "G3D/prompt.h" +#include <string> +#include "G3D/debugPrintf.h" +#include "G3D/Log.h" + +#include <cstdlib> + +#ifdef _MSC_VER + // disable: "C++ exception handler used" +# pragma warning (push) +# pragma warning (disable : 4530) +#endif + +using namespace std; + +namespace G3D { namespace _internal { + +ConsolePrintHook _consolePrintHook; +AssertionHook _debugHook = _handleDebugAssert_; +AssertionHook _failureHook = _handleErrorCheck_; + +#ifdef G3D_LINUX +#if SOMEONE_MADE_THIS_USEFUL + Display* x11Display = NULL; + Window x11Window = 0; +#endif +#endif + + +#ifdef G3D_WIN32 +static void postToClipboard(const char *text) { + if (OpenClipboard(NULL)) { + HGLOBAL hMem = GlobalAlloc(GHND | GMEM_DDESHARE, strlen(text) + 1); + if (hMem) { + char *pMem = (char*)GlobalLock(hMem); + strcpy(pMem, text); + GlobalUnlock(hMem); + + EmptyClipboard(); + SetClipboardData(CF_TEXT, hMem); + } + + CloseClipboard(); + GlobalFree(hMem); + } +} +#endif + +/** + outTitle should be set before the call + */ +static void createErrorMessage( + const char* expression, + const std::string& message, + const char* filename, + int lineNumber, + std::string& outTitle, + std::string& outMessage) { + + std::string le = ""; + const char* newline = "\n"; + + #ifdef G3D_WIN32 + newline = "\r\n"; + + // The last error value. (Which is preserved across the call). + DWORD lastErr = GetLastError(); + + // The decoded message from FormatMessage + LPTSTR formatMsg = NULL; + + if (NULL == formatMsg) { + FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | + FORMAT_MESSAGE_IGNORE_INSERTS | + FORMAT_MESSAGE_FROM_SYSTEM, + NULL, + lastErr, + 0, + (LPTSTR)&formatMsg, + 0, + NULL); + } + + // Make sure the message got translated into something. + LPTSTR realLastErr; + if (NULL != formatMsg) { + realLastErr = formatMsg; + } else { + realLastErr = _T("Last error code does not exist."); + } + + if (lastErr != 0) { + le = G3D::format("Last Error (0x%08X): %s\r\n\r\n", lastErr, (LPCSTR)realLastErr); + } + + // Get rid of the allocated memory from FormatMessage. + if (NULL != formatMsg) { + LocalFree((LPVOID)formatMsg); + } + + char modulePath[MAX_PATH]; + GetModuleFileNameA(NULL, modulePath, MAX_PATH); + + const char* moduleName = strrchr(modulePath, '\\'); + outTitle = outTitle + string(" - ") + string(moduleName ? (moduleName + 1) : modulePath); + + #endif + + // Build the message. + outMessage = + G3D::format("%s%s%sExpression: %s%s%s:%d%s%s%s", + message.c_str(), newline, newline, expression, newline, + filename, lineNumber, newline, newline, le.c_str()); +} + + +bool _handleDebugAssert_( + const char* expression, + const std::string& message, + const char* filename, + int lineNumber, + bool useGuiPrompt) { + + std::string dialogTitle = "Assertion Failure"; + std::string dialogText = ""; + createErrorMessage(expression, message, filename, lineNumber, dialogTitle, dialogText); + + #ifdef G3D_WIN32 + DWORD lastErr = GetLastError(); + postToClipboard(dialogText.c_str()); + debugPrintf("\n%s\n", dialogText.c_str()); + #endif + + const int cBreak = 0; + const int cIgnore = 1; + const int cAbort = 2; + + static const char* choices[] = {"Debug", "Ignore", "Exit"}; + + // Log the error + Log::common()->print(std::string("\n**************************\n\n") + dialogTitle + "\n" + dialogText); + + int result = G3D::prompt(dialogTitle.c_str(), dialogText.c_str(), (const char**)choices, 3, useGuiPrompt); + +# ifdef G3D_WIN32 + // Put the incoming last error back. + SetLastError(lastErr); +# endif + + switch (result) { + // -1 shouldn't actually occur because it means + // that we're in release mode. + case -1: + case cBreak: + return true; + break; + + case cIgnore: + return false; + break; + + case cAbort: + exit(-1); + break; + } + + // Should never get here + return false; +} + + +bool _handleErrorCheck_( + const char* expression, + const std::string& message, + const char* filename, + int lineNumber, + bool useGuiPrompt) { + + std::string dialogTitle = "Critical Error"; + std::string dialogText = ""; + + createErrorMessage(expression, message, filename, lineNumber, dialogTitle, dialogText); + + // Log the error + Log::common()->print(std::string("\n**************************\n\n") + dialogTitle + "\n" + dialogText); + #ifdef G3D_WIN32 + DWORD lastErr = GetLastError(); + (void)lastErr; + postToClipboard(dialogText.c_str()); + debugPrintf("\n%s\n", dialogText.c_str()); + #endif + + static const char* choices[] = {"Ok"}; + + const std::string& m = + std::string("An internal error has occured in this program and it will now close. " + "The specific error is below. More information has been saved in \"") + + Log::getCommonLogFilename() + "\".\n" + dialogText; + + int result = G3D::prompt("Error", m.c_str(), (const char**)choices, 1, useGuiPrompt); + (void)result; + + return true; +} + + +#ifdef G3D_WIN32 +static HCURSOR oldCursor; +static RECT oldCursorRect; +static POINT oldCursorPos; +static int oldShowCursorCount; +#endif + +void _releaseInputGrab_() { + #ifdef G3D_WIN32 + + GetCursorPos(&oldCursorPos); + + // Stop hiding the cursor if the application hid it. + oldShowCursorCount = ShowCursor(true) - 1; + + if (oldShowCursorCount < -1) { + for (int c = oldShowCursorCount; c < -1; ++c) { + ShowCursor(true); + } + } + + // Set the default cursor in case the application + // set the cursor to NULL. + oldCursor = GetCursor(); + SetCursor(LoadCursor(NULL, IDC_ARROW)); + + // Allow the cursor full access to the screen + GetClipCursor(&oldCursorRect); + ClipCursor(NULL); + + #elif defined(G3D_LINUX) +#if SOMEONE_MADE_THIS_USEFUL + if (x11Display != NULL) { + XUngrabPointer(x11Display, CurrentTime); + XUngrabKeyboard(x11Display, CurrentTime); + if (x11Window != 0) { + //XUndefineCursor(x11Display, x11Window); + // TODO: Note that we leak this cursor; it should be + // freed in the restore code. + Cursor c = XCreateFontCursor(x11Display, 68); + XDefineCursor(x11Display, x11Window, c); + } + XSync(x11Display, false); + XAllowEvents(x11Display, AsyncPointer, CurrentTime); + XFlush(x11Display); + } +#endif + #elif defined(G3D_OSX) + // TODO: OS X + #endif +} + + +void _restoreInputGrab_() { + #ifdef G3D_WIN32 + + // Restore the old clipping region + ClipCursor(&oldCursorRect); + + SetCursorPos(oldCursorPos.x, oldCursorPos.y); + + // Restore the old cursor + SetCursor(oldCursor); + + // Restore old visibility count + if (oldShowCursorCount < 0) { + for (int c = 0; c > oldShowCursorCount; --c) { + ShowCursor(false); + } + } + + #elif defined(G3D_LINUX) + // TODO: Linux + #elif defined(G3D_OSX) + // TODO: OS X + #endif +} + + +}; // internal namespace + +void setAssertionHook(AssertionHook hook) { + G3D::_internal::_debugHook = hook; +} + +AssertionHook assertionHook() { + return G3D::_internal::_debugHook; +} + +void setFailureHook(AssertionHook hook) { + G3D::_internal::_failureHook = hook; +} + +AssertionHook failureHook() { + return G3D::_internal::_failureHook; +} + + +void setConsolePrintHook(ConsolePrintHook h) { + G3D::_internal::_consolePrintHook = h; +} + +ConsolePrintHook consolePrintHook() { + return G3D::_internal::_consolePrintHook; +} + + +std::string __cdecl debugPrint(const std::string& s) { +# ifdef G3D_WIN32 + const int MAX_STRING_LEN = 1024; + + // Windows can't handle really long strings sent to + // the console, so we break the string. + if (s.size() < MAX_STRING_LEN) { + OutputDebugStringA(s.c_str()); + } else { + for (unsigned int i = 0; i < s.size(); i += MAX_STRING_LEN) { + std::string sub = s.substr(i, MAX_STRING_LEN); + OutputDebugStringA(sub.c_str()); + } + } +# else + fprintf(stderr, "%s", s.c_str()); + fflush(stderr); +# endif + + return s; +} + +std::string __cdecl debugPrintf(const char* fmt ...) { + va_list argList; + va_start(argList, fmt); + std::string s = G3D::vformat(fmt, argList); + va_end(argList); + + return debugPrint(s); +// return debugPrint(consolePrint(s)); +} + +std::string consolePrint(const std::string& s) { + FILE* L = Log::common()->getFile(); + fprintf(L, "%s", s.c_str()); + + if (consolePrintHook()) { + consolePrintHook()(s); + } + + fflush(L); + return s; +} + + +std::string __cdecl consolePrintf(const char* fmt ...) { + va_list argList; + va_start(argList, fmt); + std::string s = G3D::vformat(fmt, argList); + va_end(argList); + + return consolePrint(s); +} + +} // namespace + +#ifdef _MSC_VER +# pragma warning (pop) +#endif diff --git a/externals/g3dlite/fileutils.cpp b/externals/g3dlite/fileutils.cpp new file mode 100644 index 00000000000..3f5eb579ba9 --- /dev/null +++ b/externals/g3dlite/fileutils.cpp @@ -0,0 +1,1165 @@ +/** + @file fileutils.cpp + + @author Morgan McGuire, graphics3d.com + + @author 2002-06-06 + @edited 2010-02-05 + */ + +#include <cstring> +#include <cstdio> +#include "G3D/platform.h" +#include "G3D/fileutils.h" +#include "G3D/BinaryInput.h" +#include "G3D/BinaryOutput.h" +#include "G3D/g3dmath.h" +#include "G3D/stringutils.h" +#include "G3D/Set.h" +#include "G3D/g3dfnmatch.h" + +#include <sys/stat.h> +#include <sys/types.h> +#if _HAVE_ZIP + #include "zip.h" +#endif + +#ifdef G3D_WIN32 + // Needed for _getcwd + #include <direct.h> + #include <io.h> +#else + #include <dirent.h> + #include <fnmatch.h> + #include <unistd.h> + #define _getcwd getcwd + #define _stat stat +#endif + + +namespace G3D { + +namespace _internal { + Set<std::string> currentFilesUsed; +} + +std::string pathConcat(const std::string& dirname, const std::string& file) { + // Ensure that the directory ends in a slash + if ((dirname.size() != 0) && + (dirname[dirname.size() - 1] != '/') && + (dirname[dirname.size() - 1] != '\\') && + (dirname[dirname.size() - 1] != ':')) { + return dirname + '/' + file; + } else { + return dirname + file; + } +} + +std::string resolveFilename(const std::string& filename) { + if (filename.size() >= 1) { + if ((filename[0] == '/') || (filename[0] == '\\')) { + // Already resolved + return filename; + } else { + + #ifdef G3D_WIN32 + if ((filename.size() >= 2) && (filename[1] == ':')) { + // There is a drive spec on the front. + if ((filename.size() >= 3) && ((filename[2] == '\\') || + (filename[2] == '/'))) { + // Already fully qualified + return filename; + } else { + // The drive spec is relative to the + // working directory on that drive. + debugAssertM(false, "Files of the form d:path are" + " not supported (use a fully qualified" + " name)."); + return filename; + } + } + #endif + } + } + + char buffer[1024]; + + // Prepend the working directory. + _getcwd(buffer, 1024); + + return format("%s/%s", buffer, filename.c_str()); +} + +bool zipfileExists(const std::string& filename) { + std::string outZipfile; + std::string outInternalFile; + return zipfileExists(filename, outZipfile, outInternalFile); +} + +std::string readWholeFile( + const std::string& filename) { + + _internal::currentFilesUsed.insert(filename); + + std::string s; + + debugAssert(filename != ""); + if (fileExists(filename, false)) { + + int64 length = fileLength(filename); + + char* buffer = (char*)System::alignedMalloc(length + 1, 16); + debugAssert(buffer); + FILE* f = fopen(filename.c_str(), "rb"); + debugAssert(f); + int ret = fread(buffer, 1, length, f); + debugAssert(ret == length);(void)ret; + fclose(f); + + buffer[length] = '\0'; + s = std::string(buffer); + + System::alignedFree(buffer); + + } else if (zipfileExists(filename)) { + + void* zipBuffer; + size_t length; + zipRead(filename, zipBuffer, length); + + char* buffer = (char*)System::alignedMalloc(length + 1, 16); + System::memcpy(buffer,zipBuffer, length + 1); + zipClose(zipBuffer); + + buffer[length] = '\0'; + s = std::string(buffer); + System::alignedFree(buffer); + } else { + debugAssertM(false, filename + " not found"); + } + + return s; +} + + +void zipRead(const std::string& file, + void*& data, + size_t& length) { + std::string zip, desiredFile; +#if _HAVE_ZIP + if (zipfileExists(file, zip, desiredFile)) { + struct zip *z = zip_open( zip.c_str(), ZIP_CHECKCONS, NULL ); + { + struct zip_stat info; + zip_stat_init( &info ); // TODO: Docs unclear if zip_stat_init is required. + zip_stat( z, desiredFile.c_str(), ZIP_FL_NOCASE, &info ); + length = info.size; + // sets machines up to use MMX, if they want + data = System::alignedMalloc(length, 16); + struct zip_file *zf = zip_fopen( z, desiredFile.c_str(), ZIP_FL_NOCASE ); + { + int test = zip_fread( zf, data, length ); + debugAssertM((size_t)test == length, + desiredFile + " was corrupt because it unzipped to the wrong size."); + (void)test; + } + zip_fclose( zf ); + } + zip_close( z ); + } else { + data = NULL; + } +#else + data = NULL; +#endif +} + + +void zipClose(void* data) { + System::alignedFree(data); +} + + +int64 fileLength(const std::string& filename) { + struct _stat st; + int result = _stat(filename.c_str(), &st); + + if (result == -1) { +#if _HAVE_ZIP + std::string zip, contents; + if(zipfileExists(filename, zip, contents)){ + int64 requiredMem; + + struct zip *z = zip_open( zip.c_str(), ZIP_CHECKCONS, NULL ); + debugAssertM(z != NULL, zip + ": zip open failed."); + { + struct zip_stat info; + zip_stat_init( &info ); // TODO: Docs unclear if zip_stat_init is required. + int success = zip_stat( z, contents.c_str(), ZIP_FL_NOCASE, &info ); + debugAssertM(success == 0, zip + ": " + contents + ": zip stat failed."); + requiredMem = info.size; + } + zip_close( z ); + return requiredMem; + } else { + return -1; + } +#else + return -1; +#endif + } + + return st.st_size; +} + +/** Used by robustTmpfile. Returns nonzero if fread, fwrite, and fseek all +succeed on the file. + @author Morgan McGuire, http://graphics.cs.williams.edu */ +static int isFileGood(FILE* f) { + + int x, n, result; + + /* Must be a valid file handle */ + if (f == NULL) { + return 0; + } + + /* Try to write */ + x = 1234; + n = fwrite(&x, sizeof(int), 1, f); + + if (n != 1) { + return 0; + } + + /* Seek back to the beginning */ + result = fseek(f, 0, SEEK_SET); + if (result != 0) { + return 0; + } + + /* Read */ + n = fread(&x, sizeof(int), 1, f); + if (n != 1) { + return 0; + } + + /* Seek back to the beginning again */ + fseek(f, 0, SEEK_SET); + + return 1; +} + +FILE* createTempFile() { + FILE* t = NULL; + +//# ifdef G3D_WIN32 + t = tmpfile(); +//# else +// // On Unix, tmpfile generates a warning for any code that links against it. +// const char* tempfilename = "/tmp/g3dtemp.XXXXXXXX"; +// mktemp(tempfilename); +// t = fopen(tempfilename, "w"); +//# endif + +# ifdef _WIN32 + char* n = NULL; +# endif + char name[256]; + + if (isFileGood(t)) { + return t; + } + +# ifdef G3D_WIN32 + /* tmpfile failed; try the tmpnam routine */ + t = fopen(tmpnam(NULL), "w+"); + if (isFileGood(t)) { + return t; + } + + n = _tempnam("c:/tmp/", "t"); + /* Try to create something in C:\tmp */ + t = fopen(n, "w+"); + if (isFileGood(t)) { + return t; + } + + /* Try c:\temp */ + n = _tempnam("c:/temp/", "t"); + t = fopen(n, "w+"); + if (isFileGood(t)) { + return t; + } + + /* try the current directory */ + n = _tempnam("./", "t"); + t = fopen(n, "w+"); + if (isFileGood(t)) { + return t; + } + + sprintf(name, "%s/tmp%d", "c:/temp", rand()); + t = fopen(name, "w+"); + if (isFileGood(t)) { + return t; + } + + /* Try some hardcoded paths */ + sprintf(name, "%s/tmp%d", "c:/tmp", rand()); + t = fopen(name, "w+"); + if (isFileGood(t)) { + return t; + } +# else + sprintf(name, "%s/tmp%d", "/tmp", rand()); + t = fopen(name, "w+"); + if (isFileGood(t)) { + return t; + } +#endif + + sprintf(name, "tmp%d", rand()); + t = fopen(name, "w+"); + if (isFileGood(t)) { + return t; + } + + fprintf(stderr, "Unable to create a temporary file; robustTmpfile returning NULL\n"); + + return NULL; +} + +/////////////////////////////////////////////////////////////////////////////// +void writeWholeFile( + const std::string& filename, + const std::string& str, + bool flush) { + + // Make sure the directory exists. + std::string root, base, ext, path; + Array<std::string> pathArray; + parseFilename(filename, root, pathArray, base, ext); + + path = root + stringJoin(pathArray, '/'); + if (! fileExists(path, false)) { + createDirectory(path); + } + + FILE* file = fopen(filename.c_str(), "wb"); + + debugAssert(file); + + fwrite(str.c_str(), str.size(), 1, file); + + if (flush) { + fflush(file); + } + fclose(file); +} + +/////////////////////////////////////////////////////////////////////////////// + +/** + Creates the directory (which may optionally end in a /) + and any parents needed to reach it. + */ +void createDirectory( + const std::string& dir) { + + if (dir == "") { + return; + } + + std::string d; + + // Add a trailing / if there isn't one. + switch (dir[dir.size() - 1]) { + case '/': + case '\\': + d = dir; + break; + + default: + d = dir + "/"; + } + + // If it already exists, do nothing + if (fileExists(d.substr(0, d.size() - 1)), false) { + return; + } + + // Parse the name apart + std::string root, base, ext; + Array<std::string> path; + + std::string lead; + parseFilename(d, root, path, base, ext); + debugAssert(base == ""); + debugAssert(ext == ""); + + // Begin with an extra period so "c:\" becomes "c:\.\" after + // appending a path and "c:" becomes "c:.\", not root: "c:\" + std::string p = root + "."; + + // Create any intermediate that doesn't exist + for (int i = 0; i < path.size(); ++i) { + p += "/" + path[i]; + if (! fileExists(p, false)) { + // Windows only requires one argument to mkdir, + // where as unix also requires the permissions. +# ifndef G3D_WIN32 + mkdir(p.c_str(), 0777); +# else + _mkdir(p.c_str()); +# endif + } + } +} + +/////////////////////////////////////////////////////////////////////////////// + +class FileSystemCache { +private: + + Table<std::string, Array<std::string> > m_files; + +public: + + bool fileExists(const std::string& filename) { + const std::string& path = resolveFilename(filenamePath(filename)); + const std::string& name = filenameBaseExt(filename); + + bool neverBeforeSeen = false; + Array<std::string>& fileList = m_files.getCreate(path, neverBeforeSeen); + if (neverBeforeSeen) { + if (! G3D::fileExists(path, true, false)) { + // The path itself doesn't exist... back out our insertion (which makes fileList& invalid) + m_files.remove(path); + return false; + } + + std::string spec = pathConcat(path, "*"); + + // Will automatically recurse into zipfiles + getFiles(spec, fileList); + getDirs(spec, fileList); + +# ifdef G3D_WIN32 + { + // Case insensitive + for (int i = 0; i < fileList.size(); ++i) { + fileList[i] = toLower(fileList[i]); + } + } +# endif + } + + if (filenameContainsWildcards(name)) { + // See if anything matches + for (int i = 0; i < fileList.size(); ++i) { + if (g3dfnmatch(name.c_str(), fileList[i].c_str(), 0) == 0) { + return true; + } + } + return false; + } else { + // On windows, this is a lower-lower comparison, so it is case insensitive + return fileList.contains(name); + } + } + + void clear() { + m_files.clear(); + } + + static FileSystemCache& instance() { + static FileSystemCache i; + return i; + } +}; + + +void clearFileSystemCache() { + FileSystemCache::instance().clear(); +} + +bool fileExists +(const std::string& _filename, + bool lookInZipfiles, + bool trustCache) { + + if (_filename.empty()) { + return false; + } + + // Remove trailing slash from directories + const std::string& filename = (endsWith(_filename, "/") || endsWith(_filename, "\\")) ? _filename.substr(0, _filename.length() - 1) : _filename; + + if (trustCache && lookInZipfiles) { +# ifdef G3D_WIN32 + // Case insensitive + return FileSystemCache::instance().fileExists(toLower(filename)); +# else + return FileSystemCache::instance().fileExists(filename); +# endif + } + + // Useful for debugging + //char curdir[1024]; _getcwd(curdir, 1024); + + struct _stat st; + int ret = _stat(filename.c_str(), &st); + + // _stat returns zero on success + bool exists = (ret == 0); + + if (! exists && lookInZipfiles) { + // Does not exist standalone, but might exist in a zipfile + + // These output arguments will be ignored + std::string zipDir, internalPath; + return zipfileExists(filename, zipDir, internalPath); + } else { + return exists; + } +} + +/////////////////////////////////////////////////////////////////////////////// + +#if _HAVE_ZIP +/* Helper methods for zipfileExists()*/ +// Given a string (the drive) and an array (the path), computes the directory +static void _zip_resolveDirectory(std::string& completeDir, const std::string& drive, const Array<std::string>& path, const int length){ + completeDir = drive; + int tempLength; + // if the given length is longer than the array, we correct it + if(length > path.length()){ + tempLength = path.length(); + } else{ + tempLength = length; + } + + for(int t = 0; t < tempLength; ++t){ + if(t > 0){ + completeDir += "/"; + } + completeDir += path[t]; + } +} + + +// assumes that zipDir references a .zip file +static bool _zip_zipContains(const std::string& zipDir, const std::string& desiredFile){ + struct zip *z = zip_open( zipDir.c_str(), ZIP_CHECKCONS, NULL ); + //the last parameter, an int, determines case sensitivity: + //1 is sensitive, 2 is not, 0 is default + int test = zip_name_locate( z, desiredFile.c_str(), ZIP_FL_NOCASE ); + zip_close( z ); + if(test == -1){ + return false; + } + return true; +} +#endif + +// If no zipfile exists, outZipfile and outInternalFile are unchanged +bool zipfileExists(const std::string& filename, std::string& outZipfile, + std::string& outInternalFile){ +#if _HAVE_ZIP + Array<std::string> path; + std::string drive, base, ext, zipfile, infile; + parseFilename(filename, drive, path, base, ext); + + // Put the filename back together + if ((base != "") && (ext != "")) { + infile = base + "." + ext; + } else { + infile = base + ext; + } + + // Remove "." from path + for (int i = 0; i < path.length(); ++i) { + if (path[i] == ".") { + path.remove(i); + --i; + } + } + + // Remove ".." from path + for (int i = 1; i < path.length(); ++i) { + if ((path[i] == "..") && (i > 0) && (path[i - 1] != "..")) { + // Remove both i and i - 1 + path.remove(i - 1, 2); + i -= 2; + } + } + + // Walk the path backwards, accumulating pieces onto the infile until + // we find a zipfile that contains it + for (int t = 0; t < path.length(); ++t){ + _zip_resolveDirectory(zipfile, drive, path, path.length() - t); + if (t > 0) { + infile = path[path.length() - t] + "/" + infile; + } + + if (endsWith(zipfile, "..")) { + return false; + } + + if (fileExists(zipfile, false)) { + // test if it actually is a zipfile + // if not, return false, a bad + // directory structure has been given, + // not a .zip + if (isZipfile(zipfile)){ + + if (_zip_zipContains(zipfile, infile)){ + outZipfile = zipfile; + outInternalFile = infile; + return true; + } else { + return false; + } + } else { + // the directory structure was valid but did not point to a .zip + return false; + } + } + + } +#endif + // not a valid directory structure ever, + // obviously no .zip was found within the path + return false; +} + +/////////////////////////////////////////////////////////////////////////////// + +std::string generateFilenameBase(const std::string& prefix, const std::string& suffix) { + Array<std::string> exist; + + // Note "template" is a reserved word in C++ + std::string templat = prefix + System::currentDateString() + "_"; + getFiles(templat + "*", exist); + + // Remove extensions + for (int i = 0; i < exist.size(); ++i) { + exist[i] = filenameBase(exist[i]); + } + + int num = 0; + std::string result; + templat += "%03d" + suffix; + do { + result = format(templat.c_str(), num); + ++num; + } while (exist.contains(result)); + + return result; +} + +/////////////////////////////////////////////////////////////////////////////// + +void copyFile( + const std::string& source, + const std::string& dest) { + + #ifdef G3D_WIN32 + CopyFileA(source.c_str(), dest.c_str(), FALSE); + #else + // TODO: don't use BinaryInput and BinaryOutput + // Read it all in, then dump it out + BinaryInput in(source, G3D_LITTLE_ENDIAN); + BinaryOutput out(dest, G3D_LITTLE_ENDIAN); + out.writeBytes(in.getCArray(), in.size()); + out.commit(false); + #endif +} + +////////////////////////////////////////////////////////////////////////////// + +void parseFilename( + const std::string& filename, + std::string& root, + Array<std::string>& path, + std::string& base, + std::string& ext) { + + std::string f = filename; + + root = ""; + path.clear(); + base = ""; + ext = ""; + + if (f == "") { + // Empty filename + return; + } + + // See if there is a root/drive spec. + if ((f.size() >= 2) && (f[1] == ':')) { + + if ((f.size() > 2) && isSlash(f[2])) { + + // e.g. c:\foo + root = f.substr(0, 3); + f = f.substr(3, f.size() - 3); + + } else { + + // e.g. c:foo + root = f.substr(2); + f = f.substr(2, f.size() - 2); + + } + + } else if ((f.size() >= 2) & isSlash(f[0]) && isSlash(f[1])) { + + // e.g. //foo + root = f.substr(0, 2); + f = f.substr(2, f.size() - 2); + + } else if (isSlash(f[0])) { + + root = f.substr(0, 1); + f = f.substr(1, f.size() - 1); + + } + + // Pull the extension off + { + // Find the period + size_t i = f.rfind('.'); + + if (i != std::string::npos) { + // Make sure it is after the last slash! + size_t j = iMax(f.rfind('/'), f.rfind('\\')); + if ((j == std::string::npos) || (i > j)) { + ext = f.substr(i + 1, f.size() - i - 1); + f = f.substr(0, i); + } + } + } + + // Pull the basename off + { + // Find the last slash + size_t i = iMax(f.rfind('/'), f.rfind('\\')); + + if (i == std::string::npos) { + + // There is no slash; the basename is the whole thing + base = f; + f = ""; + + } else if ((i != std::string::npos) && (i < f.size() - 1)) { + + base = f.substr(i + 1, f.size() - i - 1); + f = f.substr(0, i); + + } + } + + // Parse what remains into path. + size_t prev, cur = 0; + + while (cur < f.size()) { + prev = cur; + + // Allow either slash + size_t i = f.find('/', prev + 1); + size_t j = f.find('\\', prev + 1); + if (i == std::string::npos) { + i = f.size(); + } + + if (j == std::string::npos) { + j = f.size(); + } + + cur = iMin(i, j); + + if (cur == std::string::npos) { + cur = f.size(); + } + + path.append(f.substr(prev, cur - prev)); + ++cur; + } +} + + +/** + Helper for getFileList and getDirectoryList. + + @param wantFiles If false, returns the directories, otherwise + returns the files. + @param includePath If true, the names include paths + */ +static void getFileOrDirListNormal +(const std::string& filespec, + Array<std::string>& files, + bool wantFiles, + bool includePath) { + + bool test = wantFiles ? true : false; + + std::string path = ""; + + // Find the place where the path ends and the file-spec begins + size_t i = filespec.rfind('/'); + size_t j = filespec.rfind('\\'); + + // Drive letters on Windows can separate a path + size_t k = filespec.rfind(':'); + + if (((j != std::string::npos) && (j > i)) || + (i == std::string::npos)) { + i = j; + } + + if (((k != std::string::npos) && (k > i)) || + (i == std::string::npos)) { + i = k; + } + + // If there is a path, pull it off + if (i != std::string::npos) { + path = filespec.substr(0, i + 1); + } + + std::string prefix = path; + + if (path.size() > 0) { + // Strip the trailing character + path = path.substr(0, path.size() - 1); + } + +# ifdef G3D_WIN32 + { + struct _finddata_t fileinfo; + + long handle = _findfirst(filespec.c_str(), &fileinfo); + int result = handle; + + while (result != -1) { + if ((((fileinfo.attrib & _A_SUBDIR) == 0) == test) && + strcmp(fileinfo.name, ".") && + strcmp(fileinfo.name, "..")) { + + if (includePath) { + files.append(prefix + fileinfo.name); + } else { + files.append(fileinfo.name); + } + } + + result = _findnext(handle, &fileinfo); + } + } +# else + { + if (path == "") { + // Empty paths don't work on Unix + path = "."; + } + + // Unix implementation + DIR* dir = opendir(path.c_str()); + + if (dir != NULL) { + struct dirent* entry = readdir(dir); + + while (entry != NULL) { + + // Exclude '.' and '..' + if ((strcmp(entry->d_name, ".") != 0) && + (strcmp(entry->d_name, "..") != 0)) { + + // Form a name with a path + std::string filename = prefix + entry->d_name; + // See if this is a file or a directory + struct _stat st; + bool exists = _stat(filename.c_str(), &st) != -1; + + if (exists && + + // Make sure it has the correct type + (((st.st_mode & S_IFDIR) == 0) == test) && + + // Make sure it matches the wildcard + (fnmatch(filespec.c_str(), + filename.c_str(), + FNM_PATHNAME) == 0)) { + + if (includePath) { + files.append(filename); + } else { + files.append(entry->d_name); + } + } + } + + entry = readdir(dir); + } + closedir(dir); + } + } +# endif +} + +#if _HAVE_ZIP +/** + @param path The zipfile name (no trailing slash) + @param prefix Directory inside the zipfile. No leading slash, must have trailing slash if non-empty. + @param file Name inside the zipfile that we are testing to see if it matches prefix + "*" + */ +static void _zip_addEntry(const std::string& path, + const std::string& prefix, + const std::string& file, + Set<std::string>& files, + bool wantFiles, + bool includePath) { + + // Make certain we are within the desired parent folder (prefix) + if (beginsWith(file, prefix)) { + // validityTest was prefix/file + + // Extract everything to the right of the prefix + std::string s = file.substr(prefix.length()); + + if (s == "") { + // This was the name of the prefix + return; + } + + // See if there are any slashes + size_t slashPos = s.find('/'); + + bool add = false; + + if (slashPos == std::string::npos) { + // No slashes, so s must be a file + add = wantFiles; + } else if (! wantFiles) { + // Not all zipfiles list directories as explicit entries. + // Because of this, if we're looking for directories and see + // any path longer than prefix, we must add the subdirectory. + // The Set will fix duplicates for us. + s = s.substr(0, slashPos); + add = true; + } + + if (add) { + if (includePath) { + files.insert(path + "/" + prefix + s); + } else { + files.insert(s); + } + } + } +} +#endif + +static void getFileOrDirListZip(const std::string& path, + const std::string& prefix, + Array<std::string>& files, + bool wantFiles, + bool includePath){ +#if _HAVE_ZIP + struct zip *z = zip_open( path.c_str(), ZIP_CHECKCONS, NULL ); + + Set<std::string> fileSet; + + int count = zip_get_num_files( z ); + for( int i = 0; i < count; ++i ) { + struct zip_stat info; + zip_stat_init( &info ); // TODO: Docs unclear if zip_stat_init is required. + zip_stat_index( z, i, ZIP_FL_NOCASE, &info ); + _zip_addEntry(path, prefix, info.name, fileSet, wantFiles, includePath); + } + + zip_close( z ); + + fileSet.getMembers(files); +#endif +} + + +static void determineFileOrDirList( + const std::string& filespec, + Array<std::string>& files, + bool wantFiles, + bool includePath) { + + // if it is a .zip, prefix will specify the folder within + // whose contents we want to see + std::string prefix = ""; + std::string path = filenamePath(filespec); + + if ((path.size() > 0) && isSlash(path[path.size() - 1])) { + // Strip the trailing slash + path = path.substr(0, path.length() -1); + } + + if ((path == "") || fileExists(path, false)) { + if ((path != "") && isZipfile(path)) { + // .zip should only work if * is specified as the Base + Ext + // Here, we have been asked for the root's contents + debugAssertM(filenameBaseExt(filespec) == "*", "Can only call getFiles/getDirs on zipfiles using '*' wildcard"); + getFileOrDirListZip(path, prefix, files, wantFiles, includePath); + } else { + // It is a normal directory + getFileOrDirListNormal(filespec, files, wantFiles, includePath); + } + } else if (zipfileExists(filenamePath(filespec), path, prefix)) { + // .zip should only work if * is specified as the Base + Ext + // Here, we have been asked for the contents of a folder within the .zip + debugAssertM(filenameBaseExt(filespec) == "*", "Can only call getFiles/getDirs on zipfiles using '*' wildcard"); + getFileOrDirListZip(path, prefix, files, wantFiles, includePath); + } +} + + +void getFiles(const std::string& filespec, + Array<std::string>& files, + bool includePath) { + + determineFileOrDirList(filespec, files, true, includePath); +} + + +void getDirs( + const std::string& filespec, + Array<std::string>& files, + bool includePath) { + + determineFileOrDirList(filespec, files, false, includePath); +} + + +std::string filenameBaseExt(const std::string& filename) { + int i = filename.rfind("/"); + int j = filename.rfind("\\"); + + if ((j > i) && (j >= 0)) { + i = j; + } + +# ifdef G3D_WIN32 + j = filename.rfind(":"); + if ((i == -1) && (j >= 0)) { + i = j; + } +# endif + + if (i == -1) { + return filename; + } else { + return filename.substr(i + 1, filename.length() - i); + } +} + + +std::string filenameBase(const std::string& s) { + std::string drive; + std::string base; + std::string ext; + Array<std::string> path; + + parseFilename(s, drive, path, base, ext); + return base; +} + + +std::string filenameExt(const std::string& filename) { + int i = filename.rfind("."); + if (i >= 0) { + return filename.substr(i + 1, filename.length() - i); + } else { + return ""; + } +} + + +std::string filenamePath(const std::string& filename) { + int i = filename.rfind("/"); + int j = filename.rfind("\\"); + + if ((j > i) && (j >= 0)) { + i = j; + } + +# ifdef G3D_WIN32 + j = filename.rfind(":"); + if ((i == -1) && (j >= 0)) { + i = j; + } +# endif + + if (i == -1) { + return ""; + } else { + return filename.substr(0, i+1); + } +} + + +bool isZipfile(const std::string& filename) { + + FILE* f = fopen(filename.c_str(), "r"); + if (f == NULL) { + return false; + } + uint8 header[4]; + fread(header, 4, 1, f); + + const uint8 zipHeader[4] = {0x50, 0x4b, 0x03, 0x04}; + for (int i = 0; i < 4; ++i) { + if (header[i] != zipHeader[i]) { + fclose(f); + return false; + } + } + + fclose(f); + return true; +} + + +bool isDirectory(const std::string& filename) { + struct _stat st; + bool exists = _stat(filename.c_str(), &st) != -1; + return exists && ((st.st_mode & S_IFDIR) != 0); +} + + +bool filenameContainsWildcards(const std::string& filename) { + return (filename.find('*') != std::string::npos) || (filename.find('?') != std::string::npos); +} + + +bool fileIsNewer(const std::string& src, const std::string& dst) { + struct _stat sts; + bool sexists = _stat(src.c_str(), &sts) != -1; + + struct _stat dts; + bool dexists = _stat(dst.c_str(), &dts) != -1; + + return sexists && ((! dexists) || (sts.st_mtime > dts.st_mtime)); +} + + +Array<std::string> filesUsed() { + Array<std::string> f; + _internal::currentFilesUsed.getMembers(f); + return f; +} + +} + +#ifndef G3D_WIN32 + #undef _stat +#endif diff --git a/externals/g3dlite/format.cpp b/externals/g3dlite/format.cpp new file mode 100644 index 00000000000..d9d1b516393 --- /dev/null +++ b/externals/g3dlite/format.cpp @@ -0,0 +1,164 @@ +/** + @file format.cpp + + @author Morgan McGuire, graphics3d.com + + @created 2000-09-09 + @edited 2006-08-14 +*/ + +#include "G3D/format.h" +#include "G3D/platform.h" +#include "G3D/System.h" + +#ifdef _MSC_VER + // disable: "C++ exception handler used" +# pragma warning (push) +# pragma warning (disable : 4530) +#endif // _MSC_VER + +// If your platform does not have vsnprintf, you can find a +// implementation at http://www.ijs.si/software/snprintf/ + +namespace G3D { + +std::string __cdecl format(const char* fmt,...) { + va_list argList; + va_start(argList,fmt); + std::string result = vformat(fmt, argList); + va_end(argList); + + return result; +} + +#if defined(_MSC_VER) && (_MSC_VER >= 1300) +// Both MSVC seems to use the non-standard vsnprintf +// so we are using vscprintf to determine buffer size, however +// only MSVC7 and up headers include vscprintf for some reason. +std::string vformat(const char *fmt, va_list argPtr) { + // We draw the line at a 1MB string. + const int maxSize = 1000000; + + // If the string is less than 161 characters, + // allocate it on the stack because this saves + // the malloc/free time. + const int bufSize = 161; + char stackBuffer[bufSize]; + + // MSVC does not support va_copy + int actualSize = _vscprintf(fmt, argPtr) + 1; + + if (actualSize > bufSize) { + + // Now use the heap. + char* heapBuffer = NULL; + + if (actualSize < maxSize) { + + heapBuffer = (char*)System::malloc(maxSize + 1); + _vsnprintf(heapBuffer, maxSize, fmt, argPtr); + heapBuffer[maxSize] = '\0'; + } else { + heapBuffer = (char*)System::malloc(actualSize); + vsprintf(heapBuffer, fmt, argPtr); + } + + std::string formattedString(heapBuffer); + System::free(heapBuffer); + return formattedString; + } else { + + vsprintf(stackBuffer, fmt, argPtr); + return std::string(stackBuffer); + } +} + +#elif defined(_MSC_VER) && (_MSC_VER < 1300) + +std::string vformat(const char *fmt, va_list argPtr) { + // We draw the line at a 1MB string. + const int maxSize = 1000000; + + // If the string is less than 161 characters, + // allocate it on the stack because this saves + // the malloc/free time. + const int bufSize = 161; + char stackBuffer[bufSize]; + + // MSVC6 doesn't support va_copy, however it also seems to compile + // correctly if we just pass our argument list along. Note that + // this whole code block is only compiled if we're on MSVC6 anyway + int actualWritten = _vsnprintf(stackBuffer, bufSize, fmt, argPtr); + + // Not a big enough buffer, bufSize characters written + if (actualWritten == -1) { + + int heapSize = 512; + double powSize = 1.0; + char* heapBuffer = (char*)System::malloc(heapSize); + + while ((_vsnprintf(heapBuffer, heapSize, fmt, argPtr) == -1) && + (heapSize < maxSize)) { + + heapSize = iCeil(heapSize * ::pow((double)2.0, powSize++)); + heapBuffer = (char*)System::realloc(heapBuffer, heapSize); + } + + heapBuffer[heapSize-1] = '\0'; + + std::string heapString(heapBuffer); + System::free(heapBuffer); + + return heapString; + } else { + + return std::string(stackBuffer); + } +} + +#else + +// glibc 2.1 has been updated to the C99 standard +std::string vformat(const char* fmt, va_list argPtr) { + // If the string is less than 161 characters, + // allocate it on the stack because this saves + // the malloc/free time. The number 161 is chosen + // to support two lines of text on an 80 character + // console (plus the null terminator). + const int bufSize = 161; + char stackBuffer[bufSize]; + + va_list argPtrCopy; + va_copy(argPtrCopy, argPtr); + int numChars = vsnprintf(stackBuffer, bufSize, fmt, argPtrCopy); + va_end(argPtrCopy); + + if (numChars >= bufSize) { + // We didn't allocate a big enough string. + char* heapBuffer = (char*)System::malloc((numChars + 1) * sizeof(char)); + + debugAssert(heapBuffer); + int numChars2 = vsnprintf(heapBuffer, numChars + 1, fmt, argPtr); + debugAssert(numChars2 == numChars); + (void)numChars2; + + std::string result(heapBuffer); + + System::free(heapBuffer); + + return result; + + } else { + + return std::string(stackBuffer); + + } +} + +#endif + +} // namespace + +#ifdef _MSC_VER +# pragma warning (pop) +#endif diff --git a/externals/g3dlite/g3dfnmatch.cpp b/externals/g3dlite/g3dfnmatch.cpp new file mode 100644 index 00000000000..39ef7b31914 --- /dev/null +++ b/externals/g3dlite/g3dfnmatch.cpp @@ -0,0 +1,204 @@ +/*- +* Copyright (c) 1992, 1993 +*The Regents of the University of California. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. 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. +* 3. All advertising materials mentioning features or use of this software +* must display the following acknowledgement: +*This product includes software developed by the University of +*California, Berkeley and its contributors. +* 4. Neither the name of the University 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 REGENTS 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 REGENTS 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. +* +*@(#)fnmatch.h8.1 (Berkeley) 6/2/93 +* +* From FreeBSD fnmatch.h 1.7 +* $Id: g3dfnmatch.cpp,v 1.2 2010/02/06 10:03:24 corey_taylor Exp $ +*/ +#include "G3D/g3dfnmatch.h" + +#ifdef G3D_WIN32 + +#include <ctype.h> +#include <string.h> +#include <stdio.h> + +namespace G3D { + +#define EOS '\0' + +static const char *rangematch(const char *, char, int); + +int g3dfnmatch(const char *pattern, const char *string, int flags) +{ + const char *stringstart; + char c, test; + + for (stringstart = string;;) + switch (c = *pattern++) { + case EOS: + if ((flags & FNM_LEADING_DIR) && *string == '/') + return (0); + return (*string == EOS ? 0 : FNM_NOMATCH); + case '?': + if (*string == EOS) + return (FNM_NOMATCH); + if (*string == '/' && (flags & FNM_PATHNAME)) + return (FNM_NOMATCH); + if (*string == '.' && (flags & FNM_PERIOD) && + (string == stringstart || + ((flags & FNM_PATHNAME) && *(string - 1) == '/'))) + return (FNM_NOMATCH); + ++string; + break; + case '*': + c = *pattern; + /* Collapse multiple stars. */ + while (c == '*') + c = *++pattern; + + if (*string == '.' && (flags & FNM_PERIOD) && + (string == stringstart || + ((flags & FNM_PATHNAME) && *(string - 1) == '/'))) + return (FNM_NOMATCH); + + /* Optimize for pattern with * at end or before /. */ + if (c == EOS) + if (flags & FNM_PATHNAME) + return ((flags & FNM_LEADING_DIR) || + strchr(string, '/') == NULL ? + 0 : FNM_NOMATCH); + else + return (0); + else if (c == '/' && flags & FNM_PATHNAME) { + if ((string = strchr(string, '/')) == NULL) + return (FNM_NOMATCH); + break; + } + + /* General case, use recursion. */ + while ((test = *string) != EOS) { + if (!rangematch(pattern, *string, flags & ~FNM_PERIOD)) + return (0); + if (test == '/' && flags & FNM_PATHNAME) + break; + ++string; + } + return (FNM_NOMATCH); + case '[': + if (*string == EOS) + return (FNM_NOMATCH); + if (*string == '/' && flags & FNM_PATHNAME) + return (FNM_NOMATCH); + if ((pattern = + rangematch(pattern, *string, flags)) == NULL) + return (FNM_NOMATCH); + ++string; + break; + case '\\': + if (!(flags & FNM_NOESCAPE)) { + if ((c = *pattern++) == EOS) { + c = '\\'; + --pattern; + } + } + /* FALLTHROUGH */ + default: + if (c == *string) + ; + else if ((flags & FNM_CASEFOLD) && + (tolower((unsigned char)c) == + tolower((unsigned char)*string))) + ; + else if ((flags & FNM_PREFIX_DIRS) && *string == EOS && + ((c == '/' && string != stringstart) || + (string == stringstart+1 && *stringstart == '/'))) + return (0); + else + return (FNM_NOMATCH); + string++; + break; + } + /* NOTREACHED */ +} + +static const char * +rangematch(const char *pattern, char test, int flags) +{ + int negate, ok; + char c, c2; + + /* + * A bracket expression starting with an unquoted circumflex + * character produces unspecified results (IEEE 1003.2-1992, + * 3.13.2). This implementation treats it like '!', for + * consistency with the regular expression syntax. + * J.T. Conklin (conklin@ngai.kaleida.com) + */ + if ( (negate = (*pattern == '!' || *pattern == '^')) ) + ++pattern; + + if (flags & FNM_CASEFOLD) + test = tolower((unsigned char)test); + + for (ok = 0; (c = *pattern++) != ']';) { + if (c == '\\' && !(flags & FNM_NOESCAPE)) + c = *pattern++; + if (c == EOS) + return (NULL); + + if (flags & FNM_CASEFOLD) + c = tolower((unsigned char)c); + + if (*pattern == '-' + && (c2 = *(pattern+1)) != EOS && c2 != ']') { + pattern += 2; + if (c2 == '\\' && !(flags & FNM_NOESCAPE)) + c2 = *pattern++; + if (c2 == EOS) + return (NULL); + + if (flags & FNM_CASEFOLD) + c2 = tolower((unsigned char)c2); + + if ((unsigned char)c <= (unsigned char)test && + (unsigned char)test <= (unsigned char)c2) + ok = 1; + } else if (c == test) + ok = 1; + } + return (ok == negate ? NULL : pattern); +} + +} + +#else + +namespace G3D { +int g3dfnmatch(const char * a, const char *b, int c) { + return fnmatch(a, b, c); +} +} + +#endif + diff --git a/externals/g3dlite/g3dmath.cpp b/externals/g3dlite/g3dmath.cpp new file mode 100644 index 00000000000..ad85e9efb9b --- /dev/null +++ b/externals/g3dlite/g3dmath.cpp @@ -0,0 +1,108 @@ +/** + @file g3dmath.cpp + + @author Morgan McGuire, graphics3d.com + + @created 2001-06-02 + @edited 2004-02-24 + */ + +#include "G3D/g3dmath.h" +#include <cstdlib> +#include <cstring> + +namespace G3D { + +float gaussRandom(float mean, float stdev) { + + // Using Box-Mueller method from http://www.taygeta.com/random/gaussian.html + // Modified to specify standard deviation and mean of distribution + float w, x1, x2; + + // Loop until w is less than 1 so that log(w) is negative + do { + x1 = uniformRandom(-1.0, 1.0); + x2 = uniformRandom(-1.0, 1.0); + + w = float(square(x1) + square(x2)); + } while (w > 1.0f); + + // Transform to gassian distribution + // Multiply by sigma (stdev ^ 2) and add mean. + return x2 * (float)square(stdev) * sqrtf((-2.0f * logf(w) ) / w) + mean; +} + +/** + This value should not be tested against directly, instead + G3D::isNan() and G3D::isFinite() will return reliable results. */ +double inf() { + return std::numeric_limits<double>::infinity(); +} + +bool isNaN(float x) { + static const float n = nan(); + return memcmp(&x, &n, sizeof(float)) == 0; +} + +bool isNaN(double x) { + static const double n = nan(); + return memcmp(&x, &n, sizeof(double)) == 0; +} + + +/** + This value should not be tested against directly, instead + G3D::isNan() and G3D::isFinite() will return reliable results. */ +float finf() { + return std::numeric_limits<float>::infinity(); +} + +/** This value should not be tested against directly, instead + G3D::isNan() and G3D::isFinite() will return reliable results. */ +double nan() { + // double is a standard type and should have quiet NaN + return std::numeric_limits<double>::quiet_NaN(); +} + +float fnan() { + // double is a standard type and should have quiet NaN + return std::numeric_limits<float>::quiet_NaN(); +} + + +int highestBit(uint32 x) { + // Binary search. + int base = 0; + + if (x & 0xffff0000) { + base = 16; + x >>= 16; + } + if (x & 0x0000ff00) { + base += 8; + x >>= 8; + } + if (x & 0x000000f0) { + base += 4; + x >>= 4; + } + + static const int lut[] = {-1,0,1,1,2,2,2,2,3,3,3,3,3,3,3,3}; + return base + lut[x]; +} + + +int iRandom(int low, int high) { + int r = iFloor(low + (high - low + 1) * (double)rand() / RAND_MAX); + + // There is a *very small* chance of generating + // a number larger than high. + if (r > high) { + return high; + } else { + return r; + } +} + + +} diff --git a/externals/g3dlite/prompt.cpp b/externals/g3dlite/prompt.cpp new file mode 100644 index 00000000000..6a28e6462b4 --- /dev/null +++ b/externals/g3dlite/prompt.cpp @@ -0,0 +1,729 @@ +/** + @file prompt.cpp + + @author Morgan McGuire, http://graphics.cs.williams.edu + @cite Windows dialog interface by Max McGuire, mmcguire@ironlore.com + @cite Font setting code by Kurt Miller, kurt@flipcode.com + + @created 2000-08-26 + @edited 2005-01-14 + */ + +#include "G3D/prompt.h" +#include "G3D/platform.h" + +#include <stdio.h> + +#ifdef G3D_WIN32 +# include <sstream> +# include <conio.h> +#else +# define _getch getchar +#endif + +#ifdef G3D_OSX + +/*#ifdef __LP64__ +# undef __LP64__ +#endif +*/ + +# include <Carbon/Carbon.h> + +/* +#ifdef G3D_64BIT +# define __LP64__ +#endif +*/ + +#endif + +namespace G3D { + +#ifdef G3D_WIN32 + +namespace _internal { +/** + Generic Win32 dialog facility. + @author Max McGuire + */ +class DialogTemplate { +public: + + DialogTemplate(LPCSTR caption, DWORD style, + int x, int y, int w, int h, + LPCSTR font = NULL, WORD fontSize = 8) { + + usedBufferLength = sizeof(DLGTEMPLATE); + totalBufferLength = usedBufferLength; + + dialogTemplate = (DLGTEMPLATE*)malloc(totalBufferLength); + + dialogTemplate->style = style; + + if (font != NULL) { + dialogTemplate->style |= DS_SETFONT; + } + + dialogTemplate->x = (short)x; + dialogTemplate->y = (short)y; + dialogTemplate->cx = (short)w; + dialogTemplate->cy = (short)h; + dialogTemplate->cdit = 0; + + dialogTemplate->dwExtendedStyle = 0; + + // The dialog box doesn't have a menu or a special class + AppendData("\0", 2); + AppendData("\0", 2); + + // Add the dialog's caption to the template + + AppendString(caption); + + if (font != NULL) { + AppendData(&fontSize, sizeof(WORD)); + AppendString(font); + } + } + + void AddComponent(LPCSTR type, LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + DLGITEMTEMPLATE item; + + item.style = style; + item.x = (short)x; + item.y = (short)y; + item.cx = (short)w; + item.cy = (short)h; + item.id = id; + + item.dwExtendedStyle = exStyle; + + AppendData(&item, sizeof(DLGITEMTEMPLATE)); + + AppendString(type); + AppendString(caption); + + WORD creationDataLength = 0; + AppendData(&creationDataLength, sizeof(WORD)); + + // Increment the component count + dialogTemplate->cdit++; + + } + + + void AddButton(LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + AddStandardComponent(0x0080, caption, style, exStyle, x, y, w, h, id); + + WORD creationDataLength = 0; + AppendData(&creationDataLength, sizeof(WORD)); + + } + + + void AddEditBox(LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + AddStandardComponent(0x0081, caption, style, exStyle, x, y, w, h, id); + + WORD creationDataLength = 0; + AppendData(&creationDataLength, sizeof(WORD)); + + } + + + void AddStatic(LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + AddStandardComponent(0x0082, caption, style, exStyle, x, y, w, h, id); + + WORD creationDataLength = 0; + AppendData(&creationDataLength, sizeof(WORD)); + + } + + + void AddListBox(LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + AddStandardComponent(0x0083, caption, style, exStyle, x, y, w, h, id); + + WORD creationDataLength = sizeof(WORD) + 5 * sizeof(WCHAR); + AppendData(&creationDataLength, sizeof(WORD)); + + AppendString("TEST"); + + } + + + void AddScrollBar(LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + AddStandardComponent(0x0084, caption, style, exStyle, x, y, w, h, id); + + WORD creationDataLength = 0; + AppendData(&creationDataLength, sizeof(WORD)); + + } + + + void AddComboBox(LPCSTR caption, DWORD style, DWORD exStyle, int x, int y, int w, int h, WORD id) { + + AddStandardComponent(0x0085, caption, style, exStyle, x, y, w, h, id); + + WORD creationDataLength = 0; + AppendData(&creationDataLength, sizeof(WORD)); + + } + + + /** + * + * Returns a pointer to the Win32 dialog template which the object + * represents. This pointer may become invalid if additional components + * are added to the template. + * + */ + operator const DLGTEMPLATE*() const { + return dialogTemplate; + } + + virtual ~DialogTemplate() { + free(dialogTemplate); + } + +protected: + + void AddStandardComponent(WORD type, LPCSTR caption, DWORD style, DWORD exStyle, + int x, int y, int w, int h, WORD id, LPSTR font = NULL, WORD fontSize = 8) { + + DLGITEMTEMPLATE item; + + // DWORD align the beginning of the component data + + AlignData(sizeof(DWORD)); + + item.style = style; + if (font != NULL) { + item.style |= DS_SETFONT; + } + item.x = (short)x; + item.y = (short)y; + item.cx = (short)w; + item.cy = (short)h; + item.id = id; + + item.dwExtendedStyle = exStyle; + + AppendData(&item, sizeof(DLGITEMTEMPLATE)); + + WORD preType = 0xFFFF; + + AppendData(&preType, sizeof(WORD)); + AppendData(&type, sizeof(WORD)); + + AppendString(caption); + + if (font != NULL) { + AppendData(&fontSize, sizeof(WORD)); + AppendString(font); + } + + // Increment the component count + dialogTemplate->cdit++; + } + + + void AlignData(int size) { + + int paddingSize = usedBufferLength % size; + + if (paddingSize != 0) { + EnsureSpace(paddingSize); + usedBufferLength += paddingSize; + } + + } + + void AppendString(LPCSTR string) { + + int length = MultiByteToWideChar(CP_ACP, 0, string, -1, NULL, 0); + + WCHAR* wideString = (WCHAR*)malloc(sizeof(WCHAR) * length); + MultiByteToWideChar(CP_ACP, 0, string, -1, wideString, length); + + AppendData(wideString, length * sizeof(WCHAR)); + free(wideString); + + } + + void AppendData(const void* data, int dataLength) { + + EnsureSpace(dataLength); + + memcpy((char*)dialogTemplate + usedBufferLength, data, dataLength); + usedBufferLength += dataLength; + + } + + void EnsureSpace(int length) { + if (length + usedBufferLength > totalBufferLength) { + totalBufferLength += length * 2; + + void* newBuffer = malloc(totalBufferLength); + memcpy(newBuffer, dialogTemplate, usedBufferLength); + + free(dialogTemplate); + dialogTemplate = (DLGTEMPLATE*)newBuffer; + } + } + +private: + + DLGTEMPLATE* dialogTemplate; + + int totalBufferLength; + int usedBufferLength; + +}; + + +struct PromptParams { + const char* message; + const char* title; +}; + +/** + * Constants for controls. + */ +#define IDC_MESSAGE 1000 +#define IDC_BUTTON0 2000 + +INT_PTR CALLBACK PromptDlgProc(HWND hDlg, UINT msg, + WPARAM wParam, LPARAM lParam) { + switch(msg) { + case WM_INITDIALOG: + { + PromptParams *params = (PromptParams*)lParam; + ::SetWindowTextA(::GetDlgItem(hDlg, IDC_MESSAGE), params->message); + + ::SetFocus(::GetDlgItem(hDlg, IDC_BUTTON0)); + + SetWindowTextA(hDlg, params->title); + + HFONT hfont = + CreateFontA(16, 0, 0, 0, FW_NORMAL, + FALSE, FALSE, FALSE, + ANSI_CHARSET,OUT_DEFAULT_PRECIS,CLIP_DEFAULT_PRECIS, + PROOF_QUALITY, FIXED_PITCH | FF_MODERN, "Courier New"); + + SendDlgItemMessage(hDlg, IDC_MESSAGE, WM_SETFONT, (WPARAM)hfont, MAKELPARAM(TRUE,0)); + + + break; + } + case WM_COMMAND: + { + int choiceNumber = LOWORD(wParam) - IDC_BUTTON0; + if ((choiceNumber >= 0) && (choiceNumber < 10)) { + EndDialog(hDlg, choiceNumber); + return TRUE; + } + } + + break; + + case WM_NCDESTROY: + // Under SDL 1.2.6 we get a NCDESTROY message for no reason and the + // window is immediately closed. This is here to debug the problem. + (void)0; + break; + + } + + return FALSE; +} + +}; // namespace _internal + + +using namespace _internal; + +/** + * Show a dialog prompt. + */ +static int guiPrompt( + const char* windowTitle, + const char* prompt, + const char** choice, + int numChoices) { + + int width = 280; + int height = 128; + + const int buttonSpacing = 2; + const int buttonWidth = + (width - buttonSpacing * 2 - + buttonSpacing * (numChoices - 1)) / numChoices; + const int buttonHeight = 13; + + + DialogTemplate dialogTemplate( + windowTitle, + WS_CAPTION | DS_CENTER | WS_SYSMENU, + 10, 10, width, height, + "Tahoma"); + + dialogTemplate.AddEditBox( + "Edit", WS_VISIBLE | ES_READONLY | ES_OEMCONVERT | ES_MULTILINE | WS_TABSTOP, WS_EX_STATICEDGE, + 2, 2, width - 4, height - buttonHeight - 7, IDC_MESSAGE); + + int i; + for (i = 0; i < numChoices; i++) { + + int x = buttonSpacing + i * (buttonWidth + buttonSpacing); + int y = height - buttonHeight - buttonSpacing; + + dialogTemplate.AddButton(choice[i], WS_VISIBLE | WS_TABSTOP, 0, + x, y, buttonWidth, buttonHeight, IDC_BUTTON0 + (WORD)i); + + } + + // Convert all single \n characters to \r\n for proper printing + int strLen = 0; + const char* pStr = prompt; + + while (*pStr != '\0') { + if ((*pStr == '\n') && (pStr != prompt)) { + if (*(pStr - 1) != '\r') { + ++strLen; + } + } + ++strLen; + ++pStr; + } + + char* newStr = (char*)malloc(strLen + 1); + + const char* pStr2 = prompt; + char* pNew = newStr; + + while (*pStr2 != '\0') { + if ((*pStr2 == '\n') && (pStr2 != prompt)) { + if (*(pStr2 - 1) != '\r') { + *pNew = '\r'; + ++pNew; + } + } + *pNew = *pStr2; + ++pNew; + ++pStr2; + } + + *pNew = '\0'; + + PromptParams params; + params.message = newStr;; + params.title = windowTitle; + + HMODULE module = GetModuleHandle(0); + int ret = DialogBoxIndirectParam(module, dialogTemplate, NULL, (DLGPROC) PromptDlgProc, (DWORD)¶ms); + + free(newStr); + + /* + For debugging when DialogBoxIndirectParam fails: + + // The last error value. (Which is preserved across the call). + DWORD lastErr = GetLastError(); + + // The decoded message from FormatMessage + LPTSTR formatMsg = NULL; + + if (NULL == formatMsg) { + FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | + FORMAT_MESSAGE_IGNORE_INSERTS | + FORMAT_MESSAGE_FROM_SYSTEM, + NULL, + lastErr, + 0, + (LPTSTR)&formatMsg, + 0, + NULL); + } + + // Make sure the message got translated into something. + LPTSTR realLastErr; + if (NULL != formatMsg) { + realLastErr = formatMsg; + } else { + realLastErr = "Last error code does not exist."; + } + + // Get rid of the allocated memory from FormatMessage. + if (NULL != formatMsg) { + LocalFree((LPVOID)formatMsg); + } + */ + + return ret; +} + +#endif + + +/** + * Show a prompt on stdout + */ +static int textPrompt( + const char* windowTitle, + const char* prompt, + const char** choice, + int numChoices) { + + printf("\n___________________________________________________\n"); + printf("%s\n", windowTitle); + printf("%s", prompt); + + if (numChoices > 10) { + numChoices = 10; + } + + int c = -1; + if (numChoices > 1) { + printf("\n"); + printf("Choose an option by number:"); + + while ((c < 0) || (c >= numChoices)) { + printf("\n"); + + for (int i = 0; i < numChoices; i++) { + if (numChoices <= 3) { + printf(" (%d) %s ", i, choice[i]); + } else { + printf(" (%d) %s\n", i, choice[i]); + } + } + + printf("\n> "); + c = _getch() - '0'; + + if ((c < 0) || (c >= numChoices)) { + printf("'%d' is not a valid choice.", c); + } else { + printf("%d", c); + } + } + + } else if (numChoices == 1) { + + printf("\nPress any key for '%s'...", choice[0]); + _getch(); + c = 0; + + } else { + + printf("\nPress any key..."); + _getch(); + c = 0; + } + + printf("\n___________________________________________________\n"); + return c; +} + +#ifdef G3D_OSX + +// See http://developer.apple.com/documentation/Carbon/Reference/Carbon_Event_Manager_Ref/index.html + +#define CARBON_COMMANDID_START 128 +#define CARBON_BUTTON_SPACING 12 +#define CARBON_BUTTON_HEIGHT 20 +#define CARBON_BUTTON_MINWIDTH 69 +#define CARBON_WINDOW_PADDING 20 + +struct CallbackData { + WindowRef refWindow; + + /** Index of this particular button */ + int myIndex; + + /** Buttons store their index into here when pressed. */ + int* whichButton; +}; + +/** + Assumes that userData is a pointer to a carbon_evt_data_t. + + */ +static pascal OSStatus DoCommandEvent(EventHandlerCallRef handlerRef, EventRef event, void* userData) { + // See http://developer.apple.com/documentation/Carbon/Conceptual/HandlingWindowsControls/index.html + + CallbackData& callbackData = *(CallbackData*)userData; + +# pragma unused(handlerRef) + + callbackData.whichButton[0] = callbackData.myIndex; + + // If we get here we can close the window + ::QuitAppModalLoopForWindow(callbackData.refWindow); + + // Return noErr to indicate that we handled the event + return noErr; +} + +static int guiPrompt +(const char* windowTitle, + const char* prompt, + const char** choice, + int numChoices) { + + WindowRef window; + + int iNumButtonRows = 0; + int iButtonWidth = -1; + OSStatus err = noErr; + + // Determine number of rows of buttons + while (iButtonWidth < CARBON_BUTTON_MINWIDTH) { + ++iNumButtonRows; + iButtonWidth = + (550 - (CARBON_WINDOW_PADDING*2 + + (CARBON_BUTTON_SPACING*numChoices))) / + (numChoices/iNumButtonRows); + } + + // Window Variables + Rect rectWin = {0, 0, 200 + ((iNumButtonRows-1) * (CARBON_BUTTON_HEIGHT+CARBON_BUTTON_SPACING)), 550}; // top, left, bottom, right + CFStringRef szWindowTitle = CFStringCreateWithCString(kCFAllocatorDefault, windowTitle, kCFStringEncodingUTF8); + + window = NULL; + + err = CreateNewWindow(kMovableAlertWindowClass, kWindowStandardHandlerAttribute|kWindowCompositingAttribute, &rectWin, &window); + err = SetWindowTitleWithCFString(window, szWindowTitle); + err = SetThemeWindowBackground(window, kThemeBrushAlertBackgroundActive, false); + assert(err == noErr); + + // Event Handler Variables + EventTypeSpec buttonSpec[] = {{ kEventClassControl, kEventControlHit }, { kEventClassCommand, kEventCommandProcess }}; + EventHandlerUPP buttonHandler = NewEventHandlerUPP(DoCommandEvent); + + // Static Text Variables + Rect rectStatic = {20, 20, 152, 530}; + CFStringRef szStaticText = CFStringCreateWithCString(kCFAllocatorDefault, prompt, kCFStringEncodingUTF8); + ControlRef refStaticText = NULL; + err = CreateStaticTextControl(window, &rectStatic, szStaticText, NULL, &refStaticText); + + // Button Variables + Rect bounds[numChoices]; + CFStringRef caption[numChoices]; + ControlRef button[numChoices]; + + int whichButton=-1; + CallbackData callbackData[numChoices]; + + // Create the Buttons and assign event handlers + for (int i = 0; i < numChoices; ++i) { + bounds[i].top = 160 + ((CARBON_BUTTON_HEIGHT+CARBON_BUTTON_SPACING)*(i%iNumButtonRows)); + bounds[i].right = 530 - ((iButtonWidth+CARBON_BUTTON_SPACING)*(i/iNumButtonRows)); + bounds[i].left = bounds[i].right - iButtonWidth; + bounds[i].bottom = bounds[i].top + CARBON_BUTTON_HEIGHT; + + // Convert the button captions to Apple strings + caption[i] = CFStringCreateWithCString(kCFAllocatorDefault, choice[i], kCFStringEncodingUTF8); + + err = CreatePushButtonControl(window, &bounds[i], caption[i], &button[i]); + assert(err == noErr); + + err = SetControlCommandID(button[i], CARBON_COMMANDID_START + i); + assert(err == noErr); + + callbackData[i].refWindow = window; + callbackData[i].myIndex = i; + callbackData[i].whichButton = &whichButton; + + err = InstallControlEventHandler(button[i], buttonHandler, + GetEventTypeCount(buttonSpec), buttonSpec, + &callbackData[i], NULL); + assert(err == noErr); + } + + // Show Dialog + err = RepositionWindow(window, NULL, kWindowCenterOnMainScreen); + ShowWindow(window); + BringToFront(window); + err = ActivateWindow(window, true); + + // Hack to get our window/process to the front... + ProcessSerialNumber psn = { 0, kCurrentProcess}; + TransformProcessType(&psn, kProcessTransformToForegroundApplication); + SetFrontProcess (&psn); + + // Run in Modal State + err = RunAppModalLoopForWindow(window); + + // Dispose of Button Related Data + for (int i = 0; i < numChoices; ++i) { + // Dispose of controls + DisposeControl(button[i]); + + // Release CFStrings + CFRelease(caption[i]); + } + + // Dispose of Other Controls + DisposeControl(refStaticText); + + // Dispose of Event Handlers + DisposeEventHandlerUPP(buttonHandler); + + // Dispose of Window + DisposeWindow(window); + + // Release CFStrings + CFRelease(szWindowTitle); + CFRelease(szStaticText); + + // Return Selection + return whichButton; +} + +#endif + +int prompt( + const char* windowTitle, + const char* prompt, + const char** choice, + int numChoices, + bool useGui) { + + #ifdef G3D_WIN32 + if (useGui) { + // Build the message box + return guiPrompt(windowTitle, prompt, choice, numChoices); + } + #endif + + #ifdef G3D_OSX + if (useGui){ + //Will default to text prompt if numChoices > 4 + return guiPrompt(windowTitle, prompt, choice, numChoices); + } + #endif + return textPrompt(windowTitle, prompt, choice, numChoices); +} + + +void msgBox( + const std::string& message, + const std::string& title) { + + const char *choice[] = {"Ok"}; + prompt(title.c_str(), message.c_str(), choice, 1, true); +} + +#ifndef G3D_WIN32 + #undef _getch +#endif + +};// namespace + diff --git a/externals/g3dlite/stringutils.cpp b/externals/g3dlite/stringutils.cpp new file mode 100644 index 00000000000..c3876ebb6a4 --- /dev/null +++ b/externals/g3dlite/stringutils.cpp @@ -0,0 +1,275 @@ +/** + @file stringutils.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2000-09-09 + @edited 2008-01-10 +*/ + +#include "G3D/platform.h" +#include "G3D/stringutils.h" +#include "G3D/BinaryInput.h" +#include <algorithm> + +namespace G3D { + +#ifdef _MSC_VER + // disable: "C++ exception handler used" +# pragma warning (push) +# pragma warning (disable : 4530) +#endif +#ifdef G3D_WIN32 + const char* NEWLINE = "\r\n"; +#else + const char* NEWLINE = "\n"; + static bool iswspace(int ch) { return (ch==' ' || ch=='\t' || ch=='\n' || ch=='\r'); } +#endif + +void parseCommaSeparated(const std::string s, Array<std::string>& array, bool stripQuotes) { + array.fastClear(); + if (s == "") { + return; + } + + size_t begin = 0; + const char delimiter = ','; + const char quote = '\"'; + do { + size_t end = begin; + // Find the next comma, or the end of the string + bool inQuotes = false; + while ((end < s.length()) && (inQuotes || (s[end] != delimiter))) { + if (s[end] == quote) { + if ((end < s.length() - 2) && (s[end + 1] == quote) && (s[end + 2]) == quote) { + // Skip over the superquote + end += 2; + } + inQuotes = ! inQuotes; + } + ++end; + } + array.append(s.substr(begin, end - begin)); + begin = end + 1; + } while (begin < s.length()); + + if (stripQuotes) { + for (int i = 0; i < array.length(); ++i) { + std::string& t = array[i]; + int L = t.length(); + if ((L > 1) && (t[0] == quote) && (t[L - 1] == quote)) { + if ((L > 6) && (t[1] == quote) && (t[2] == quote) && (t[L - 3] == quote) && (t[L - 2] == quote)) { + // Triple-quote + t = t.substr(3, L - 6); + } else { + // Double-quote + t = t.substr(1, L - 2); + } + } + } + } +} + +bool beginsWith( + const std::string& test, + const std::string& pattern) { + + if (test.size() >= pattern.size()) { + for (int i = 0; i < (int)pattern.size(); ++i) { + if (pattern[i] != test[i]) { + return false; + } + } + return true; + } else { + return false; + } +} + + +bool endsWith( + const std::string& test, + const std::string& pattern) { + + if (test.size() >= pattern.size()) { + int te = test.size() - 1; + int pe = pattern.size() - 1; + for (int i = pattern.size() - 1; i >= 0; --i) { + if (pattern[pe - i] != test[te - i]) { + return false; + } + } + return true; + } else { + return false; + } +} + + +std::string wordWrap( + const std::string& input, + int numCols) { + + std::string output; + size_t c = 0; + int len; + + // Don't make lines less than this length + int minLength = numCols / 4; + size_t inLen = input.size(); + + bool first = true; + while (c < inLen) { + if (first) { + first = false; + } else { + output += NEWLINE; + } + + if ((int)inLen - (int)c - 1 < numCols) { + // The end + output += input.substr(c, inLen - c); + break; + } + + len = numCols; + + // Look at character c + numCols, see if it is a space. + while ((len > minLength) && + (input[c + len] != ' ')) { + len--; + } + + if (len == minLength) { + // Just crop + len = numCols; + + } + + output += input.substr(c, len); + c += len; + if (c < input.size()) { + // Collapse multiple spaces. + while ((input[c] == ' ') && (c < input.size())) { + c++; + } + } + } + + return output; +} + + +int stringCompare( + const std::string& s1, + const std::string& s2) { + + return stringPtrCompare(&s1, &s2); +} + + +int stringPtrCompare( + const std::string* s1, + const std::string* s2) { + + return s1->compare(*s2); +} + + +std::string toUpper(const std::string& x) { + std::string result = x; + std::transform(result.begin(), result.end(), result.begin(), toupper); + return result; +} + + +std::string toLower(const std::string& x) { + std::string result = x; + std::transform(result.begin(), result.end(), result.begin(), tolower); + return result; +} + + +Array<std::string> stringSplit( + const std::string& x, + char splitChar) { + + Array<std::string> out; + + // Pointers to the beginning and end of the substring + const char* start = x.c_str(); + const char* stop = start; + + while ((stop = strchr(start, splitChar))) { + out.append(std::string(start, stop - start)); + start = stop + 1; + } + + // Append the last one + out.append(std::string(start)); + + return out; +} + + +std::string stringJoin( + const Array<std::string>& a, + char joinChar) { + + std::string out; + + for (int i = 0; i < (int)a.size() - 1; ++i) { + out += a[i] + joinChar; + } + + if (a.size() > 0) { + return out + a.last(); + } else { + return out; + } +} + + +std::string stringJoin( + const Array<std::string>& a, + const std::string& joinStr) { + + std::string out; + + for (int i = 0; i < (int)a.size() - 1; ++i) { + out += a[i] + joinStr; + } + + if (a.size() > 0) { + return out + a.last(); + } else { + return out; + } +} + + +std::string trimWhitespace( + const std::string& s) { + + size_t left = 0; + + // Trim from left + while ((left < s.length()) && iswspace(s[left])) { + ++left; + } + + int right = s.length() - 1; + // Trim from right + while ((right > (int)left) && iswspace(s[right])) { + --right; + } + + return s.substr(left, right - left + 1); +} + +}; // namespace + +#undef NEWLINE +#ifdef _MSC_VER +# pragma warning (pop) +#endif |