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); } }