void float3x4::RemoveScale()
{
///\todo SSE.
float x = Row3(0).Normalize();
float y = Row3(1).Normalize();
float z = Row3(2).Normalize();
assume(x != 0 && y != 0 && z != 0 && "float3x4::RemoveScale failed!");
MARK_UNUSED(x);
MARK_UNUSED(y);
MARK_UNUSED(z);
}
Quat Quat::Normalized() const
{
Quat copy = *this;
float success = copy.Normalize();
assume(success > 0 && "Quat::Normalized failed!");
MARK_UNUSED(success);
return copy;
}
/// For Plane-float3x4 transform code, see Eric Lengyel's Mathematics for 3D Game Programming And Computer Graphics 2nd ed., p.110, chapter 4.2.3. [groupSyntax]
void Plane::Transform(const float3x4 &transform)
{
///@todo Could optimize this function by switching to plane convention ax+by+cz+d=0 instead of ax+by+cz=d.
float3x3 r = transform.Float3x3Part();
bool success = r.Inverse(); ///@todo Can optimize the inverse here by assuming orthogonality or orthonormality.
assume(success);
MARK_UNUSED(success);
d = d + normal.Dot(DIR_VEC(r * transform.TranslatePart()));
normal = normal * r;
}
std::string Sphere::SerializeToString() const
{
char str[256];
char *s = SerializeFloat(pos.x, str); *s = ','; ++s;
s = SerializeFloat(pos.y, s); *s = ','; ++s;
s = SerializeFloat(pos.z, s); *s = ','; ++s;
s = SerializeFloat(r, s);
assert(s+1 - str < 256);
MARK_UNUSED(s);
return str;
}
std::string Plane::SerializeToString() const
{
char str[256];
char *s = SerializeFloat(normal.x, str); *s = ','; ++s;
s = SerializeFloat(normal.y, s); *s = ','; ++s;
s = SerializeFloat(normal.z, s); *s = ','; ++s;
s = SerializeFloat(d, s);
assert(s+1 - str < 256);
MARK_UNUSED(s);
return str;
}
std::string MUST_USE_RESULT Quat::SerializeToString() const
{
char str[256];
char *s = SerializeFloat(x, str); *s = ','; ++s;
s = SerializeFloat(y, s); *s = ','; ++s;
s = SerializeFloat(z, s); *s = ','; ++s;
s = SerializeFloat(w, s);
assert(s+1 - str < 256);
MARK_UNUSED(s);
return str;
}
unsigned long Clock::TickU32()
{
#ifdef WIN32
LARGE_INTEGER ddwTimer;
BOOL success = QueryPerformanceCounter(&ddwTimer);
assume(success != 0);
MARK_UNUSED(success);
return ddwTimer.LowPart;
#else
return (unsigned long) Tick();
#endif
}
Quat Quat::Normalized() const
{
#ifdef MATH_AUTOMATIC_SSE
return Quat(vec4_normalize(q));
#else
Quat copy = *this;
float success = copy.Normalize();
assume(success > 0 && "Quat::Normalized failed!");
MARK_UNUSED(success);
return copy;
#endif
}
std::string AABB::SerializeToString() const
{
char str[256];
char *s = SerializeFloat(minPoint.x, str); *s = ','; ++s;
s = SerializeFloat(minPoint.y, s); *s = ','; ++s;
s = SerializeFloat(minPoint.z, s); *s = ','; ++s;
s = SerializeFloat(maxPoint.x, s); *s = ','; ++s;
s = SerializeFloat(maxPoint.y, s); *s = ','; ++s;
s = SerializeFloat(maxPoint.z, s);
assert(s+1 - str < 256);
MARK_UNUSED(s);
return str;
}
bool float3x4::Inverse(float epsilon)
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
MARK_UNUSED(epsilon);
float det = mat3x4_inverse(row, row);
return MATH_NS::Abs(det) > 1e-5f;
#else
float4x4 temp(*this); ///@todo It is possible optimize to avoid copying here by writing the inverse function specifically for float3x4.
bool success = temp.Inverse(epsilon);
*this = temp.Float3x4Part();
return success;
#endif
}
std::vector<std::string> CliParser::retrieveUtf8Args(int argc, const char *argv[])
{
std::vector<std::string> result;
#if _WIN32
MARK_UNUSED(argc);
MARK_UNUSED(argv);
LPCWSTR args = GetCommandLineW();
int numArgs;
LPWSTR *splitArgs = CommandLineToArgvW(args, &numArgs);
if (splitArgs) {
for (int i = 0; i < numArgs; ++i)
result.emplace_back(UnicodeUtils::wcharToUtf8(splitArgs[i]));
LocalFree(splitArgs);
}
#else
for (int i = 0; i < argc; ++i)
result.emplace_back(argv[i]);
#endif
return std::move(result);
}
void SetQuatFrom(Quat &q, const M &m)
{
// The rotation matrix is of form: (Eric Lengyel's Mathematics for 3D Game Programming and Computer Graphics 2nd ed., p. 92)
// 1 - 2y^2 - 2z^2 2xy - 2wz 2xz + 2wy
// 2xy + 2wz 1 - 2x^2 - 2z^2 2yz - 2wx
// 2xz - 2wy 2yz + 2wx 1 - 2x^2 - 2y^2
float r = m[0][0] + m[1][1] + m[2][2]; // The element w is easiest picked up as a sum of the diagonals.
// Above, r == 3 - 4(x^2+y^2+z^2) == 4(1-x^2-y^2-z^2) - 1 == 4*w^2 - 1.
if (r > 0) // In this case, |w| > 1/2.
{
q.w = sqrtf(r + 1.f) * 0.5f; // We have two choices for the sign of w, arbitrarily pick the positive.
float inv4w = 1.f / (4.f * q.w);
q.x = (m[2][1] - m[1][2]) * inv4w;
q.y = (m[0][2] - m[2][0]) * inv4w;
q.z = (m[1][0] - m[0][1]) * inv4w;
}
else if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) // If |q.x| is larger than |q.y| and |q.z|, extract it first. This gives
{ // best stability, and we know below x can't be zero.
q.x = sqrtf(1.f + m[0][0] - m[1][1] - m[2][2]) * 0.5f; // We have two choices for the sign of x, arbitrarily pick the positive.
const float x4 = 1.f / (4.f * q.x);
q.y = (m[0][1] + m[1][0]) * x4;
q.z = (m[0][2] + m[2][0]) * x4;
q.w = (m[2][1] - m[1][2]) * x4;
}
else if (m[1][1] > m[2][2]) // |q.y| is larger than |q.x| and |q.z|
{
q.y = sqrtf(1.f + m[1][1] - m[0][0] - m[2][2]) * 0.5f; // We have two choices for the sign of y, arbitrarily pick the positive.
const float y4 = 1.f / (4.f * q.y);
q.x = (m[0][1] + m[1][0]) * y4;
q.z = (m[1][2] + m[2][1]) * y4;
q.w = (m[0][2] - m[2][0]) * y4;
}
else // |q.z| is larger than |q.x| or |q.y|
{
q.z = sqrtf(1.f + m[2][2] - m[0][0] - m[1][1]) * 0.5f; // We have two choices for the sign of z, arbitrarily pick the positive.
const float z4 = 1.f / (4.f * q.z);
q.x = (m[0][2] + m[2][0]) * z4;
q.y = (m[1][2] + m[2][1]) * z4;
q.w = (m[1][0] - m[0][1]) * z4;
}
float oldLength = q.Normalize();
assume(oldLength > 0.f);
MARK_UNUSED(oldLength);
}
std::string float3x4::SerializeToString() const
{
char str[512];
char *s = SerializeFloat(v[0][0], str); *s = ','; ++s;
s = SerializeFloat(v[0][1], s); *s = ','; ++s;
s = SerializeFloat(v[0][2], s); *s = ','; ++s;
s = SerializeFloat(v[0][3], s); *s = ','; ++s;
s = SerializeFloat(v[1][0], s); *s = ','; ++s;
s = SerializeFloat(v[1][1], s); *s = ','; ++s;
s = SerializeFloat(v[1][2], s); *s = ','; ++s;
s = SerializeFloat(v[1][3], s); *s = ','; ++s;
s = SerializeFloat(v[2][0], s); *s = ','; ++s;
s = SerializeFloat(v[2][1], s); *s = ','; ++s;
s = SerializeFloat(v[2][2], s); *s = ','; ++s;
s = SerializeFloat(v[2][3], s);
assert(s+1 - str < 512);
MARK_UNUSED(s);
return str;
}
tick_t Clock::Tick()
{
//android
#if defined(ANDROID)
struct timespec res;
clock_gettime(CLOCK_REALTIME, &res);
return 1000000000ULL*res.tv_sec + (tick_t)res.tv_nsec;
//android
#elif defined(EMSCRIPTEN)
#ifdef _TICK_IS_FLOAT
return (tick_t)emscripten_get_now();
#else
// emscripten_get_now() returns a wallclock time as a float in milliseconds (1e-3).
// scale it to microseconds (1e-6) and return as a tick.
return (tick_t)(((double)emscripten_get_now()) * 1e3);
#endif
#elif defined(WIN32)
LARGE_INTEGER ddwTimer;
BOOL success = QueryPerformanceCounter(&ddwTimer);
assume(success != 0);
MARK_UNUSED(success);
return ddwTimer.QuadPart;
#elif defined(__APPLE__)
return mach_absolute_time();
#elif defined(_POSIX_MONOTONIC_CLOCK)
timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return (tick_t)t.tv_sec * 1000 * 1000 * 1000 + (tick_t)t.tv_nsec;
#elif defined(_POSIX_C_SOURCE)
timeval t;
gettimeofday(&t, NULL);
return (tick_t) t.tv_sec * 1000 * 1000 + (tick_t) t.tv_usec;
#else
return (tick_t)clock();
#endif
}
void Polyhedron::MergeConvex(const float3 &point)
{
// LOGI("mergeconvex.");
std::set<std::pair<int, int> > deletedEdges;
std::map<std::pair<int, int>, int> remainingEdges;
for(size_t i = 0; i < v.size(); ++i)
if (point.DistanceSq(v[i]) < 1e-3f)
return;
// bool hadDisconnectedHorizon = false;
for(int i = 0; i < (int)f.size(); ++i)
{
// Delete all faces that don't contain the given point. (they have point in their positive side)
Plane p = FacePlane(i);
Face &face = f[i];
if (p.SignedDistance(point) > 1e-5f)
{
bool isConnected = (deletedEdges.empty());
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size() && !isConnected; ++j)
{
int v1 = face.v[j];
if (deletedEdges.find(std::make_pair(v1, v0)) != deletedEdges.end())
{
isConnected = true;
break;
}
v0 = v1;
}
if (isConnected)
{
v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
deletedEdges.insert(std::make_pair(v0, v1));
// LOGI("Edge %d,%d is to be deleted.", v0, v1);
v0 = v1;
}
// LOGI("Deleting face %d: %s. Distance to vertex %f", i, face.ToString().c_str(), p.SignedDistance(point));
std::swap(f[i], f.back());
f.pop_back();
--i;
continue;
}
// else
// hadDisconnectedHorizon = true;
}
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
remainingEdges[std::make_pair(v0, v1)] = i;
// LOGI("Edge %d,%d is to be deleted.", v0, v1);
v0 = v1;
}
}
// The polyhedron contained our point, nothing to merge.
if (deletedEdges.empty())
return;
// Add the new point to this polyhedron.
// if (!v.back().Equals(point))
v.push_back(point);
/*
// Create a look-up index of all remaining uncapped edges of the polyhedron.
std::map<std::pair<int,int>, int> edgesToFaces;
for(size_t i = 0; i < f.size(); ++i)
{
Face &face = f[i];
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
edgesToFaces[std::make_pair(v1, v0)] = i;
v0 = v1;
}
}
*/
// Now fix all edges by adding new triangular faces for the point.
// for(size_t i = 0; i < deletedEdges.size(); ++i)
for(std::set<std::pair<int, int> >::iterator iter = deletedEdges.begin(); iter != deletedEdges.end(); ++iter)
{
std::pair<int, int> opposite = std::make_pair(iter->second, iter->first);
if (deletedEdges.find(opposite) != deletedEdges.end())
continue;
// std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
// std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
// if (iter != edgesToFaces.end())
{
// If the adjoining face is planar to the triangle we'd like to add, instead extend the face to enclose
// this vertex.
//float3 newTriangleNormal = (v[v.size()-1]-v[iter->second]).Cross(v[iter->first]-v[iter->second]).Normalized();
std::map<std::pair<int, int>, int>::iterator existing = remainingEdges.find(opposite);
assert(existing != remainingEdges.end());
MARK_UNUSED(existing);
#if 0
int adjoiningFace = existing->second;
if (FaceNormal(adjoiningFace).Dot(newTriangleNormal) >= 0.99999f) ///\todo float3::IsCollinear
{
bool added = false;
Face &adjoining = f[adjoiningFace];
for(size_t i = 0; i < adjoining.v.size(); ++i)
if (adjoining.v[i] == iter->second)
{
adjoining.v.insert(adjoining.v.begin() + i + 1, v.size()-1);
added = true;
/*
int prev2 = (i + adjoining.v.size() - 1) % adjoining.v.size();
int prev = i;
int cur = i + 1;
int next = (i + 2) % adjoining.v.size();
int next2 = (i + 3) % adjoining.v.size();
if (float3::AreCollinear(v[prev2], v[prev], v[cur]))
adjoining.v.erase(adjoining.v.begin() + prev);
else if (float3::AreCollinear(v[prev], v[cur], v[next]))
adjoining.v.erase(adjoining.v.begin() + cur);
else if (float3::AreCollinear(v[cur], v[next], v[next2]))
adjoining.v.erase(adjoining.v.begin() + next2);
*/
break;
}
assert(added);
assume(added);
}
else
#endif
// if (!v[deletedEdges[i].first].Equals(point) && !v[deletedEdges[i].second].Equals(point))
{
Face tri;
tri.v.push_back(iter->second);
tri.v.push_back((int)v.size()-1);
tri.v.push_back(iter->first);
f.push_back(tri);
// LOGI("Added face %d: %s.", (int)f.size()-1, tri.ToString().c_str());
}
}
}
#define mathasserteq(lhs, op, rhs) do { if (!((lhs) op (rhs))) { LOGE("Condition %s %s %s (%g %s %g) failed!", #lhs, #op, #rhs, (double)(lhs), #op, (double)(rhs)); assert(false); } } while(0)
// mathasserteq(NumVertices() + NumFaces(), ==, 2 + NumEdges());
assert(FaceIndicesValid());
// assert(EulerFormulaHolds());
// assert(IsClosed());
// assert(FacesAreNondegeneratePlanar());
// assert(IsConvex());
// if (hadDisconnectedHorizon)
// MergeConvex(point);
}
