bool RaySphere(const Vector3& rayStart, const Vector3& rayDir,
const Vector3& sphereCenter, float sphereRadius,
float& t)
{
++Application::mStatistics.mRaySphereTests;
//project sphere center on to line
Math::Vector3 sphereVec = sphereCenter - rayStart;
float floatingPointEpsilon = 0.0001f;
float tClosestToRay = sphereVec.Dot(rayDir);
if(tClosestToRay >= 0.0f)
{
Math::Vector3 pointClosestToSphere = rayStart + (rayDir * tClosestToRay);
if((sphereCenter - pointClosestToSphere).LengthSq() <= (sphereRadius * sphereRadius))
{
//may be inside sphere, so we must calculate the length of ray that is inside the sphere and subtract
//we can use cosine for this.
// adjacent hypotenuse
float lenInSphere;
if((sphereCenter - pointClosestToSphere).LengthSq() > floatingPointEpsilon)
{
lenInSphere = Math::ArcCos((sphereCenter - pointClosestToSphere).Length() / sphereRadius);
}
else
{
lenInSphere = sphereRadius;
}
//since raydir should be unit length, we can simply subtract this from our closest t
//we max to make sure that we don't subtract past the start of the ray
t = Math::Max(tClosestToRay - lenInSphere, 0.0f);
return true;
}
}
return false;
}
float DistFromPlane(const Vector3& point, const Vector3& normal, float planeDistance)
{
//compute the vector from plane's point to poi
Math::Vector3 vectorToPoint = point - (normal * planeDistance);
//project this vector on to the normal to find the distance from the plane
return vectorToPoint.Dot(normal);
}
void TCurveBase::normal(Math::Vector3 &normal, const Math::Vector3 &point) const
{
Math::Vector2 d;
derivative(point.project_xy(), d);
normal = Math::Vector3(d.x(), d.y(), -1.0);
normal.normalize();
}
void parse (StringParser& p, Math::Vector3<T>& out) {
size_t start = p.pos ();
p.expect (typeid (Math::Vector3<T>), start, '(');
parse (p, out.x ());
p.expect (typeid (Math::Vector3<T>), start, ',');
parse (p, out.y ());
p.expect (typeid (Math::Vector3<T>), start, ',');
parse (p, out.z ());
p.expect (typeid (Math::Vector3<T>), start, ')');
}
void TLens::set_thickness(double thickness, unsigned int index)
{
double diff = thickness - get_thickness(index);
for (unsigned int i = index; i < _surfaces.size(); i++)
{
Math::Vector3 p = _surfaces[i].get_local_position();
p.z() += diff;
_surfaces[i].set_local_position(p);
}
_last_pos += diff;
}
bool RayTriangle(const Vector3& rayStart, const Vector3& rayDir,
const Vector3& triP0, const Vector3& triP1, const Vector3& triP2,
float& t, float triExpansionEpsilon)
{
++Application::mStatistics.mRayTriangleTests;
Math::Vector3 normal = (triP1 - triP0).Cross(triP2 - triP0).Normalized();
if(RayPlane(rayStart, rayDir, Math::Vector4(normal.x, normal.y, normal.z, normal.Dot(triP0)), t))
{
Math::Vector3 pointOnPlane = (rayDir * t) + rayStart;
Math::Vector3 barycentric;
return BarycentricCoordinates(pointOnPlane, triP0, triP1, triP2, barycentric.x, barycentric.y, barycentric.z, triExpansionEpsilon);
}
return false;
}
void Box::createDipoleGeometry (DipoleGeometry& dipoleGeometry) const {
Math::Vector3<ldouble> size = this->size () / dipoleGeometry.gridUnit ();
size_t mat = dipoleGeometry.materials ().size ();
dipoleGeometry.materials ().push_back (material ());
for (uint32_t z = 0; z < size.z (); z++) {
for (uint32_t y = 0; y < size.y (); y++) {
for (uint32_t x = 0; x < size.x (); x++) {
ASSERT (mat <= 255);
dipoleGeometry.addDipole (x, y, z, static_cast<uint8_t> (mat));
}
}
}
dipoleGeometry.moveToCenter ();
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
//see http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4152&hilit=raytest
//for reference
void
PhysicsZone::rayCast(Math::Ray _ray, Math::Real _maxDistance, I_CollisionVisitor& _visitor)
{
Math::Vector3 offset = _ray.getDirection();
offset.normalize();
offset *= _maxDistance;
Math::Point3 endpoint = _ray.getOrigin() + offset;
//(m_pZone, _ray.getOrigin().m_array, endpoint.m_array, rayCastFilter, &rayCastQuery, rayCastPrefilter);
btVector3 tquatFrom = btVector3(_ray.getOrigin().m_x,_ray.getOrigin().m_y,_ray.getOrigin().m_z);
btVector3 tquatTo = btVector3(_ray.getOrigin().m_x,_ray.getOrigin().m_y,_ray.getOrigin().m_z);
RayResultCallback resultCallback(tquatFrom,tquatTo);
m_pZone->rayTest(tquatFrom,tquatTo,resultCallback);
}
void GameProjectile::launch( const math::Vector3& direction )
{
setVelocity( direction.normalize() * m_launchVelocity );
m_removable = false;
m_hasHit = false;
m_age = 0;
m_movedDistance = 0;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
// set the transformation of a rigid body
void
PhysicsActor::TransformCallback(const NewtonBody* _body, const Zen::Math::Real* _matrix)
{
void* pBody = NewtonBodyGetUserData(_body);
if (pBody != NULL)
{
PhysicsActor* pShape = static_cast<PhysicsActor*>(pBody);
// Only use the transform callback if the state is active
if (pShape->m_activationState != 0)
{
Math::Matrix4 transform;
for(int x = 0; x < 16; x++)
{
transform.m_array[x] = _matrix[x];
}
TransformEventData evenData(*pShape, transform);
pShape->onTransformEvent(evenData);
}
}
#if 0
// HACK Keep the object to a constant velocity
Math::Vector3 velocity;
NewtonBodyGetVelocity(_body, velocity.m_array);
velocity.normalize();
velocity = velocity * 50;
NewtonBodySetVelocity(_body, velocity.m_array);
#endif
//std::cout << "TransformCallback()" << std::endl;
#if 0
RenderPrimitive* primitive;
// get the graphic object form the rigid body
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
// set the transformation matrix for this rigid body
dMatrix& mat = *((dMatrix*)matrix);
primitive->SetMatrix (mat);
#endif
}
void Node::lookAt( const math::Vector3& target, const math::Vector3& up )
{
assert( Math::abs(up.length()-1.f) < 1e-3f ); // Up direction must be normalized
// src parent->world space
Matrix4x4 parentToWorld = Matrix4x4(1);
const Node* parent = this->parent();
while ( parent )
{
parentToWorld = parent->transform() * parentToWorld;
parent = parent->parent();
}
// src->world space
Matrix4x4 sourceToWorld = parentToWorld * transform();
// src -> target (world space)
Vector3 sourceRotZ = target - sourceToWorld.translation();
if ( sourceRotZ.lengthSquared() > Float::MIN_VALUE )
{
// src->target direction (world space)
sourceRotZ = sourceRotZ.normalize();
// src rotation (world space)
Vector3 sourceRotX = up.cross( sourceRotZ );
if ( sourceRotX.lengthSquared() > Float::MIN_VALUE )
sourceRotX = sourceRotX.normalize();
else
sourceRotX = Vector3(1,0,0);
Vector3 sourceRotY = sourceRotZ.cross( sourceRotX );
Matrix3x3 sourceRot;
sourceRot.setColumn( 0, sourceRotX );
sourceRot.setColumn( 1, sourceRotY );
sourceRot.setColumn( 2, sourceRotZ );
// src world space rotation back to src parent space
Matrix3x3 parentToWorldRot = parentToWorld.rotation();
Matrix3x3 rot = sourceRot * parentToWorldRot.inverse();
setRotation( rot );
}
}
DipoleGeometry::DipoleGeometry (ldouble gridUnit,
Math::Vector3<ldouble> periodicity1,
Math::Vector3<ldouble> periodicity2)
: box_ (0, 0, 0), matCount_ (0), validNvCount_ (0), origin_ (0, 0, 0),
orientation_ (EMSim::Rotation<ldouble>::none ()),
orientationInverse_ (EMSim::Rotation<ldouble>::none ()),
gridUnit_ (gridUnit),
periodicity1_ (periodicity1),
periodicity2_ (periodicity2)
{
ASSERT (gridUnit >= 0);
bool periodicity1IsZero = periodicity1.x () == 0 && periodicity1.y () == 0 && periodicity1.z () == 0;
bool periodicity2IsZero = periodicity2.x () == 0 && periodicity2.y () == 0 && periodicity2.z () == 0;
if (periodicity1IsZero) {
ASSERT (periodicity2IsZero);
periodicityDimension_ = 0;
} else {
if (periodicity2IsZero) {
periodicityDimension_ = 1;
} else {
periodicityDimension_ = 2;
}
}
gridUnitVol_ = gridUnit * gridUnit * gridUnit;
}
template <typename T> inline Math::Vector3<T> operator* (SymMatrix3<T> m, Math::Vector3<T> v) {
return Math::Vector3<T> (
m.aa()*v.x() + m.ab()*v.y() + m.ac()*v.z(),
m.ab()*v.x() + m.bb()*v.y() + m.bc()*v.z(),
m.ac()*v.x() + m.bc()*v.y() + m.cc()*v.z()
);
}
DebugShape& DebugDrawer::DrawSphere(const Sphere& sphere)
{
// To access the camera's position for the horizon disc calculation use mApplication->mCamera.mTranslation
DebugShape& shape = GetNewShape();
int it;
std::vector<LineSegment> segments[4];
segments[0] = makeCircle(sphere.mCenter, Math::Vector3(1.0f, 0.0f, 0.0f), sphere.mRadius);
segments[1] = makeCircle(sphere.mCenter, Math::Vector3(0.0f, 1.0f, 0.0f), sphere.mRadius);
segments[2] = makeCircle(sphere.mCenter, Math::Vector3(0.0f, 0.0f, 1.0f), sphere.mRadius);
if(mApplication)
{
//horizon circle
Math::Vector3 dVec = sphere.mCenter - mApplication->mCamera.mTranslation;
Math::Vector3 dVecNorm = dVec.Normalized();
float radSquared = sphere.mRadius * sphere.mRadius;
float lLen = Math::Sqrt(dVec.LengthSq() - radSquared);
float rPrime = (sphere.mRadius * lLen) / dVec.Length();
float z = radSquared - (rPrime * rPrime);
Math::Vector3 newCenter = mApplication->mCamera.mTranslation + (dVec - (dVecNorm * z));
segments[3] = makeCircle(newCenter, dVecNorm, rPrime);
}
for(int i = 0; i < 4; ++i)
{
shape.mSegments.insert(shape.mSegments.end(), segments[i].begin(), segments[i].end());
}
return shape;
}
/*!
* Alias for glVertex(x,y,z).
*/
inline void tglVertex(const math::Vector3& v)
{
#if defined(TAU_SCALAR_DOUBLE)
glVertex3d(v.getX(), v.getY(), v.getZ());
#else
glVertex3f(v.getX(), v.getY(), v.getZ());
#endif
}
bool BarycentricCoordinates(const Vector3& point, const Vector3& a, const Vector3& b,
float& u, float& v, float epsilon)
{
if(a != b)
{
Math::Vector3 ba = a - b;
float baLen = ba.Length();
Math::Vector3 normal = ba / baLen;
u = normal.Dot(point - b) / baLen;
v = 1.0f - u;
if(u == Math::Clamp(u, -epsilon, 1.0f + epsilon))
{
if(v == Math::Clamp(v, -epsilon, 1.0f + epsilon))
{
return true;
}
}
}
return false;
}
// Checks if the ray intersects with a triangle defined by points p1, p2, p3
// Returns true if it intersects and the 't' distace of the intersection point
// from the ray origin
bool Ray::CheckIntersection(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, float &t)
{
Math::Vector3 vecAB = p2 - p1;
Math::Vector3 vecAC = p3 - p1;
Math::Vector3 cross;
cross = mDirection.Cross(vecAC);
float det = vecAB.Dot(cross);
/*
if(cull && det < 0.0001f)
{
return false;
}
else
*/
if(det < 0.0001f && det > -0.0001f)
return false;
Math::Vector3 rayPointVec = mOrigin - p1;
float test1 = rayPointVec.Dot(cross);
if(test1 < 0.0f || test1 > det)
return false;
Math::Vector3 cross2;
cross2 = rayPointVec.Cross(vecAB);
float test2 = mDirection.Dot(cross2);
if(test2 < 0.0f || test1 + test2 > det)
return false;
float inverseDet = 1.0f / det;
t = vecAC.Dot(cross2);
t *= inverseDet;
return true;
}
void Ivy::Graphics::Mesh::SetRotation(Math::Vector3 rotation)
{
this->rotation = (this->rotation.RotateAlongX(rotation.GetX()) *
this->rotation.RotateAlongY(rotation.GetY()) *
this->rotation.RotateAlongZ(rotation.GetZ())).Transpose();
}
void GameObject::setPosition( Math::Vector3<Config::Real> const & position )
{
m_position = position.projectZ() ;
updateCollisionShapeTransform() ;
EntityAdapter::setPosition(position) ;
}
void DipoleGeometry::normalize () {
if (nvCount () == 0)
return;
Math::Vector3<uint32_t> min = Math::Vector3<uint32_t> (std::numeric_limits<uint32_t>::max (), std::numeric_limits<uint32_t>::max (), std::numeric_limits<uint32_t>::max ());
for (uint32_t i = 0; i < nvCount (); i++) {
Math::Vector3<uint32_t> coords = getGridCoordinates (i);
if (coords.x () < min.x ())
min.x () = coords.x ();
if (coords.y () < min.y ())
min.y () = coords.y ();
if (coords.z () < min.z ())
min.z () = coords.z ();
}
if (min != Math::Vector3<uint32_t> (0, 0, 0)) {
for (uint32_t i = 0; i < nvCount (); i++) {
positions_[i] -= min;
}
box_ -= min;
origin () += min * gridUnit ();
}
}
void set (std::vector<U>& vector, size_t index, const Math::Vector3<U>& value) const {
vector[index] = value.x ();
vector[index + vecStride ()] = value.y ();
vector[index + 2 * vecStride ()] = value.z ();
}
private: static double hic(const double timeI, const double timeJ, const math::Vector3 accAverage){
return pow(accAverage.GetLength() / G, 2.5) * (timeJ - timeI);
}
void Ivy::Graphics::Mesh::SetPosition(Math::Vector3 position)
{
translation = translation.Translate(position.GetX(), position.GetY(), position.GetZ()).Transpose();
}
bool PolyhedronColliderGeometry::RayCast(const Ray3 &ray, float maxDistance, float &t, Math::Vector3 &n) const
{
Ray3 localRay;
auto &body = mParent->Parent();
localRay.pos = body.GlobalToLocal(ray.pos);
localRay.dir = body.GlobalToLocalVec(ray.dir);
const Math::Vector3 &p = localRay.pos;
const Math::Vector3 &d = maxDistance * localRay.dir;
const Math::Vector3 q = p + d;
float tEnter = 0.0f;
float tExit = 1.0f;
auto &verts = mAdjacency.Verts();
auto &edges = mAdjacency.Edges();
auto &faces = mAdjacency.Faces();
for (auto &face : faces)
{
if (!face.active)
continue;
// triangle edges
auto &edge0 = edges[face.edge];
auto &edge1 = edges[edge0.next];
auto &edge2 = edges[edge1.next];
// triangle verts & normal
const Math::Vector3 &a = verts[edge0.vert].position;
const Math::Vector3 &b = verts[edge1.vert].position;
const Math::Vector3 &c = verts[edge2.vert].position;
const Math::Vector3 normal = (b - a).Cross(c - a);
float denom = normal.Dot(d);
float dist = normal.Dot(p - a); // positive: outside plane
// test if segment runs parallel to the plane
if (std::fabs(denom) < EPSILON && dist > 0.0f)
return false;
const float tempT = -dist / denom;
if (denom < -EPSILON)
{
if (tempT > tEnter)
{
n = normal;
tEnter = tempT;
}
}
else if (denom > EPSILON)
{
tExit = (tExit < tempT) ? tExit : tempT;
}
// early out
if (tEnter > tExit) return false;
}
n = body.LocalToGlobalVec(n);
n.Normalize();
t = tEnter;
return true;
}
void PolyhedronColliderGeometry::UpdateMassAndLocalCentroid(const ResolutionMaterial &material, float &mass, Math::Matrix3 &unitInertiaTensor, Math::Vector3 &localCentroid)
{
auto &verts = mAdjacency.Verts();
auto &edges = mAdjacency.Edges();
auto &faces = mAdjacency.Faces();
const Math::Vector3 &c = mAdjacency.Centroid();
mass = 0.0f;
unitInertiaTensor.ZeroOut();
localCentroid.ZeroOut();
float totalVolume = 0.0f;
for (auto &face : faces)
{
if (!face.active)
continue;
// grab face verts
int e = face.edge;
const Math::Vector3 &v0 = verts[edges[e].vert].position;
e = edges[e].next;
const Math::Vector3 &v1 = verts[edges[e].vert].position;
e = edges[e].next;
const Math::Vector3 &v2 = verts[edges[e].vert].position;
// volume of tetrahedron formed by face & centroid (not divided by 6)
const float tetrahedronVolume = std::fabs((v0 - c).Dot((v1 - c).Cross(v2 - c)));
// accumulate volume
totalVolume += tetrahedronVolume;
// accumulate weighted centroid
const Math::Vector3 tetrahedronCentroid = 0.25f * (c + v0 + v1 + v2);
localCentroid += tetrahedronVolume * tetrahedronCentroid;
}
if (totalVolume == 0)
totalVolume = 1;
localCentroid /= totalVolume;
//mass = material.mDensity * totalVolume / 6.0f;
mass = mParent->Parent().mParent->cphy->mMass * totalVolume / 6.0f;
// compute inertia tensor
for (auto &face : faces)
{
if (!face.active)
continue;
// grab face verts
int e = face.edge;
const Math::Vector3 &v0 = verts[edges[e].vert].position;
e = edges[e].next;
const Math::Vector3 &v1 = verts[edges[e].vert].position;
e = edges[e].next;
const Math::Vector3 &v2 = verts[edges[e].vert].position;
// accumulate inertia tensor
unitInertiaTensor += UnitInertiaTensorTetrahedron(c, v0, v1, v2, localCentroid);
}
}
