//=============================================================================
bool Intersect (IntersectInfo2 & out, const Ray2 & r, const Sphere2 & s)
{
const Vector2 m = r.origin - s.center;
const float32 b = 2.0f * Dot(m, r.direction);
const float32 c = LengthSq(m) - Sq(s.radius);
// the ray starts outside of the sphere and is pointing away
if (c > 0.0f && b > 0.0f)
return false;
const float32 a = LengthSq(r.direction);
const float32 discr = Sq(b) - 4.0f * a * c;
// ray missed the sphere?
if (discr < 0.0f)
return false;
out.time = (-b - Sqrt(discr)) / (2.0f * a);
// ray started inside sphere?
if (out.time < 0.0f)
out.time = 0.0f;
out.point = r.origin + out.time * r.direction;
return true;
}
bool CollisionDetector::SphereSphereCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
return false;
//get the collision data
CollisionSphere& s0 = *(CollisionSphere*)p0.GetCollisionVolume();
CollisionSphere& s1 = *(CollisionSphere*)p1.GetCollisionVolume();
//get the normal
Vector3 normal = p0.GetPosition() - p1.GetPosition();
//get the distance (squared)
const float distSq = LengthSq(normal);
//get the max distance before collision
const float sumRadius = s0.GetRadius() + s1.GetRadius();
//if the distance is less than the max distance
if (distSq < sumRadius*sumRadius) {
//if there is collision data storage
if (data) {
//set the penetration depth
data->m_penetration = sumRadius - sqrtf(distSq);
//get the normal of the collision
normal.Normalise();
data->m_normal = normal;
//get the point of collision
data->m_point = p0.GetPosition() - normal*(s0.GetRadius() - data->m_penetration*0.5f);
}
return true;
}
return false;
}
bool CollisionDetector::CylinderSphereCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
CollisionCylinder& cylinder = *(CollisionCylinder*)p0.GetCollisionVolume();
CollisionSphere& sphere = *(CollisionSphere*)p1.GetCollisionVolume();
Vector3 cylCenterVector = cylinder.GetEnd() - cylinder.GetStart();
Vector3 pos1 = p1.GetPosition() - cylinder.GetStart();
float distanceFactorFromEP1 = Vector3::Dot(p1.GetPosition() - cylinder.GetStart(), cylCenterVector) / Vector3::Dot(cylCenterVector, cylCenterVector);
if(distanceFactorFromEP1 < 0) distanceFactorFromEP1 = 0;// clamp to endpoints if neccesary
if(distanceFactorFromEP1 > 1) distanceFactorFromEP1 = 1;
Vector3 closestPoint = cylinder.GetStart() + (cylCenterVector * distanceFactorFromEP1);
Vector3 collisionVector = p1.GetPosition() - closestPoint;
float distance = collisionVector.Length();
Vector3 collisionNormal = collisionVector / distance;
if(distance < sphere.GetRadius() + cylinder.GetRadius())
{
//collision occurred. use collisionNormal to reflect sphere off cyl
float factor = Vector3::Dot(p1.GetLinearVelocity(), collisionNormal);
p1.SetLinearVelocity(p1.GetLinearVelocity() - (collisionNormal * factor * 0.8f));
const float distSq = LengthSq(collisionNormal);
//get the max distance before collision
const float sumRadius = sphere.GetRadius() + cylinder.GetRadius();
p1.SetPosition(p1.GetPosition() + Vector3(collisionNormal * (sumRadius - sqrtf(distSq))));
return true;
}
return false;
}
float Quat::Length() const
{
#ifdef MATH_AUTOMATIC_SSE
return vec4_length_float(q);
#else
return Sqrt(LengthSq());
#endif
}
Quaternion Quaternion::Inverted(void) const
{
float lengthSq = LengthSq();
if (Math::IsZero(lengthSq))
ErrorIf(Math::IsZero(lengthSq), "Quaternion - Division by zero.");
lengthSq = 1.0f / lengthSq;
return Quaternion(-x * lengthSq, -y * lengthSq, -z * lengthSq, w * lengthSq);
}
void Quaternion::Invert(void)
{
Conjugate();
float lengthSq = LengthSq();
if (Math::IsZero(lengthSq))
ErrorIf(Math::IsZero(lengthSq), "Quaternion - Division by zero.");
*this /= lengthSq;
}
//=============================================================================
bool Intersect (IntersectInfo3 & out, const Ray3 & r, const Sphere3 & s)
{
#define RAY_SPHERE_HANDLE_START_INSIDE
const Point3 & p = r.origin;
const Vector3 & d = r.direction;
const Vector3 m = p - s.center;
const float32 a = LengthSq(d);
const float32 b = 2.0f * Dot(m, d);
const float32 c = LengthSq(m) - Sq(s.radius);
// the ray starts outside of the sphere and is pointing away
const bool isOutside = c >= -0.001f;
const bool isAway = b >= 0.0;
if (isOutside && isAway)
return false;
const float32 discr = Sq(b) - 4.0f * a * c;
// ray missed the sphere?
if (discr < 0.0f)
return false;
#ifdef RAY_SPHERE_HANDLE_START_INSIDE
//ASSERT(bOutside);
const float32 sign = isOutside ? -1.0f : 1.0f;
out.time = (-b + sign * Sqrt(discr)) / (2.0f * a);
#else
out.time = (-b - Sqrt(discr)) / (2.0f * a);
// ray started inside sphere?
if (out.time < 0.0f)
return false;
#endif
out.point = r.origin + out.time * r.direction;
out.normal = out.point - s.center;
return true;
}
Quaternion Quaternion::Normalized(void) const
{
float length = LengthSq();
if(Math::Equal(length, 0.0f))
{
return Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
}
else
{
length = Math::Rsqrt(length);
return (*this) * length;
}
}
bool calculateParticleCollision(float rA, float rB,
Vector3 *pPrePosA, Vector3 *pPosA,
Vector3 *pPrePosB, Vector3 *pPosB,
float *pOutTime,
Vector3 *pOutCollidedA, Vector3 *pOutCollidedB)
{
// 前位置及び到達位置におけるパーティクル間のベクトルを算出
Vector3 C0 = *pPrePosB - *pPrePosA;
Vector3 C1 = *pPosB - *pPosA;
Vector3 D = C1 - C0;
// 衝突判定用の2次関数係数の算出
float P = LengthSq(D);
if(P == 0) return false; // 同じ方向に移動
float Q = Dot(C0, D);
float R = LengthSq(C0);
// パーティクル距離
float r = rA + rB;
// 衝突判定式
float Judge = (Q * Q) - (P * (R - (r * r)));
if(Judge < 0) {
// 衝突していない
return false;
}
// 衝突時間の算出
float t_plus = (-Q + sqrt(Judge)) / P;
float t_minus = (-Q - sqrt(Judge)) / P;
// 衝突時間が0未満1より大きい場合、衝突しない
if( (t_plus < 0 || t_plus > 1) && (t_minus < 0 || t_minus > 1)) return false;
// 衝突時間の決定（t_minus側が常に最初の衝突）
*pOutTime = t_minus;
// 衝突位置の決定
*pOutCollidedA = *pPrePosA + t_minus * (*pPosA - *pPrePosA);
*pOutCollidedB = *pPrePosB + t_minus * (*pPosB - *pPrePosB);
// 衝突報告
return true;
}
bool SphereCollision::IntersectsSphere(SphereCollisionPtr other)
{
auto difVector = mCenter - other->mCenter;
float sqrDistance = difVector.LengthSq();
//Make Sum
float sumOfRad = mActualRadius + other->mActualRadius;
sumOfRad *= sumOfRad; //Squared
if (sqrDistance <= sumOfRad)
{
return true;
}
return false;
}
//=============================================================================
bool Intersect (const Ray3 & r, const Sphere3 & s)
{
const Vector3 m = r.origin - s.center;
const float32 c = LengthSq(m) - Sq(s.radius);
// Ray starts inside?
if (c <= 0.0f)
return true;
const float32 b = Dot(m, r.direction);
// ray outside and pointing away
if (b > 0.0f)
return false;
const float32 a = LengthSq(r.direction);
const float32 discr = Sq(b) - a * c;
// ray missed sphere
if (discr < 0.0f)
return false;
return true;
}
float Quaternion::Normalize(void)
{
float length = LengthSq();
if(Math::Equal(length, 0.0f))
{
x = 0.0f;
y = x;
z = x;
w = 1.0f;
return 0.0f;
}
else
{
length = Math::Rsqrt(length);
*this *= length;
return length;
}
}
int PointGrid::Find(const vec3& pt) const
{
float invBucketDim = 1.f/m_dim;
vec3 lo = pt - vec3(kEpsilon) - m_bounds.m_min;
vec3 hi = pt + vec3(kEpsilon) - m_bounds.m_min;
lo *= invBucketDim;
hi *= invBucketDim;
int startZ = int(lo.z), endZ = int(hi.z);
int startY = int(lo.y), endY = int(hi.y);
int startX = int(lo.x), endX = int(hi.x);
startZ = Clamp(startZ, 0, m_cellsz - 1);
endZ = Clamp(endZ, 0, m_cellsz - 1);
startY = Clamp(startY, 0, m_cellsy - 1);
endY = Clamp(endY, 0, m_cellsy - 1);
startX = Clamp(startX, 0, m_cellsx - 1);
endX = Clamp(endX, 0, m_cellsx - 1);
const int pitch = m_cellsx;
const int slicePitch = m_cellsx * m_cellsy;
for(int z = startZ; z <= endZ; ++z)
{
for(int y = startY; y <= endY; ++y)
{
for(int x = startX; x <= endX; ++x)
{
const VecListType* list = m_grid[x + y * pitch + z * slicePitch].get();
if(list)
{
for(const auto& pair: *list)
{
vec3 diff = pair.vec - pt;
if(LengthSq(diff) < kEpsilonSq) {
return pair.index;
}
}
}
}
}
}
return -1;
}
bool PhysicsSystem::SphereSphereCollision(const CollisionSphere &s0, const CollisionSphere &s1, CollisionData *collisionData) const {
const float distSq = LengthSq(s0.pos - s1.pos);
//DBG_ASSERT (distSq > 0.00001f);
const float sumRadius = (s0.radius + s1.radius);
if( distSq < sumRadius*sumRadius && distSq > 0.001f){
if(collisionData){
collisionData->m_penetration = sumRadius - sqrtf(distSq);
collisionData->m_normal = s0.pos - s1.pos;
collisionData->m_normal.Normalise();
collisionData->m_point = s0.pos - collisionData->m_normal * (s0.radius - collisionData->m_penetration * 0.5f );
}
//cout << "TRUE" << endl;
return true; // A Hit! - Collision
}
return false;
}
bool PhysicsSystem::SphereSphereCollision(const c_Sphere &s0, const c_Sphere &s1, CollisionData *collisionData) const {
const float distSq = LengthSq(s0.m_pos - s1.m_pos);
//DBG_ASSERT (distSq > 0.00001f);
const float sumRadius = (s0.m_radius + s1.m_radius);
if( distSq < sumRadius*sumRadius && distSq > 0.001f){
if(collisionData){
collisionData->m_penetration = sumRadius - sqrtf(distSq);
Vector3 NormVec = (s0.m_pos - s1.m_pos);
NormVec.Normalise();
collisionData->m_normal = NormVec;
collisionData->m_point = s0.m_pos - collisionData->m_normal * (s0.m_radius - collisionData->m_penetration * 0.5f );
}
//cout << "TRUE" << endl;
return true; // A Hit!
}
//system("cls");
//cout << "FAL" << endl;
return false;
}
inline float DistanceSq(const VEC3 &p1, const VEC3 &p2)
{
return LengthSq(p2-p1);
}
float Vector2i::Length(void) const
{
return sqrt(static_cast<float>(LengthSq()));
}
float Vector2::Length() const
{
return (float)sqrt(LengthSq());
}
void Entity::Move(ID3D11Device* device, Vector3_t moveAccel, GameWorld* gameWorld, float frameTime)
{
// CHANGE MOVE_BITMASK TO V3 inAccel
float dt = frameTime/1000.0f;
Vector3_t accelVec = {0, 0, 0};
// Get the rotation in radians to correct in the movement
float moveDirCos = 1.0f;
float strafeDirCos = 0.0f;
// Set the acceleration vector to just the x & z components first for normalization purposes
accelVec.x = moveAccel.x;
accelVec.z = moveAccel.z;
// Normalize the X & Z movement
float accelLength = LengthSq(accelVec);
if(accelLength > 1.0f)
{
accelVec *= (1.0f / SquareRoot(accelLength));
}
// Twiddle factor for movment speed (8.0 is used to combat the drag for movement)
float entitySpeed = WALK_SPEED*8.0f;
accelVec *= entitySpeed;
// TODO(ebd): ODE here!
if (m_groundDragEn)
{
accelVec.x += -8.0f*m_velocity.x;
accelVec.z += -8.0f*m_velocity.z;
}
else
{
accelVec.x += -5.0f*m_velocity.x;
accelVec.z += -5.0f*m_velocity.z;
}
// Now add in the y acceleration value
accelVec.y = moveAccel.y*entitySpeed*20.0f;
// Add gravity
accelVec.y -= GRAVITY_ACCEL;
// Now do physics based movement
// Need to add Physics objects to the entity
// RigidBody object -> holds postion, rotation, velocity, accel, and handles movement for the entity
// Collider object -> holds information about the bounding box and handles collision functions
// m_RigidBody->Move(&oldPosition);
// m_Collider->CollisionTest(&position, &rotation); // This should return a bool for if there was a collision or not
// Do the movement internally for now
Vector3_t oldPosition = m_position;
Vector3_t positionDelta = (0.5f*accelVec*Square(dt) + m_velocity*dt);
m_velocity = accelVec*dt + m_velocity;
m_position = oldPosition + positionDelta;
// Do minkowski based collision here
// First get a list of game map tiles to search through
/*
uint32 minTileX = FloorFloattoUint32(Minimum(m_position.x, oldPosition.x) - m_aabb.x/2);
uint32 maxTileX = FloorFloattoUint32(Maximum(m_position.x, oldPosition.x) + m_aabb.x/2);
uint32 minTileZ = FloorFloattoUint32(Minimum(m_position.z, oldPosition.z) - m_aabb.z/2);
uint32 maxTileZ = FloorFloattoUint32(Maximum(m_position.z, oldPosition.z) + m_aabb.z/2);
*/
uint32 minTileX = FloorFloattoUint32(Minimum(m_position.x, oldPosition.x));
uint32 maxTileX = FloorFloattoUint32(Maximum(m_position.x, oldPosition.x));
uint32 minTileZ = FloorFloattoUint32(Minimum(m_position.z, oldPosition.z));
uint32 maxTileZ = FloorFloattoUint32(Maximum(m_position.z, oldPosition.z));
// Set the time remaining on the search to 1.0
// We use this to find where the collision was along the movement vector
float tMin = 1.0f;
Vector3_t collisionNormal = {0.0f, 0.0f, 0.0f};
bool floorCollision = false;
// Test each tile in the search space
for (uint32 tileX = minTileX; tileX <= maxTileX; tileX++)
{
for (uint32 tileZ = minTileZ; tileZ <= maxTileZ; tileZ++)
{
// Get the normal for the current tile (is it a wall or floor or neither)
collisionNormal = gameWorld->GetTileNormal(tileX, tileZ);
// Get the current tile value, and if there is a valid tile then do collision there
if (collisionNormal.y == 1.0f)
{
if (TestFloorCollision(oldPosition, positionDelta, &tMin))
{
collisionNormal = {0.0f, 1.0f, 0.0f};
floorCollision = true;
}
}
}
}
if (!floorCollision)
collisionNormal = {0.0f, 0.0f, 0.0f};
// Now update the entity position based on the output of the collision detection
// Update the timeRemaining using tMin
m_position.y = oldPosition.y + tMin*positionDelta.y;
m_velocity = m_velocity - 1*Inner(m_velocity, collisionNormal)*collisionNormal;
/*
if (m_position.y < 0)
{
m_position.y = 0;
m_velocity.y = 0;
}
*/
// Now updat the model position to be where the entity is
m_Model->UpdatePosition(device, m_position);
}
float Quaternion::Length(void) const
{
return Math::Sqrt(LengthSq());
}
bool getCollideSpheres(Vector3& posA, Vector3& posB, float rA, float rB)
{
return LengthSq(posA - posB) < ((rA + rB) * (rA + rB));
}
//---------------------------------
//--- Return length of Vector2D ---
//--- Using LengthSq() ------------
//---------------------------------
float Length(const Vector2D* const vect) {
return sqrt(LengthSq(vect));
}
bool Quat::IsInvertible(float epsilon) const
{
return LengthSq() > epsilon && IsFinite();
}
bool Quat::IsNormalized(float epsilon) const
{
return EqualAbs(LengthSq(), 1.f, epsilon);
}
float Quat::Length() const
{
return sqrtf(LengthSq());
}
double Vector2::Length() const { return sqrt(LengthSq()); }
/**
* クォータニオンの長さを取得する
*
* |Q| = sqrt(Qw^2 + Qx^2 + Qy^2 + Qz^2)
* @return クォータニオンの長さ
*/
float Quaternion::Length() const
{
return sqrt(LengthSq());
}
static void ddDrawPlane(const Plane& plane, const AABB& bounds, const Color& color)
{
vec3 points[6];
struct edge_t
{
int start;
int end;
};
static const edge_t edges[12] =
{
// bottom
{0, 1},
{1, 3},
{3, 2},
{2, 0},
// top
{4, 5},
{5, 7},
{7, 6},
{6, 4},
// top to bottom sides
{0, 4},
{1, 5},
{2, 6},
{3, 7},
};
// clip plane to bounds and render a quad
vec3 corners[8];
const vec3 *minmax[2] = { &bounds.m_min, &bounds.m_max };
for(int j = 0; j < 8; ++j)
{
int iz = j & 1;
int ix = (j >> 1) & 1;
int iy = (j >> 2) & 1;
corners[j].Set(minmax[ix]->x, minmax[iy]->y, minmax[iz]->z);
}
int numPoints = 0;
// add corners as points if they are close to the plane
for(int j = 0; j < 8; ++j)
{
float planeDist = PlaneDist(plane, corners[j]);
if(fabs(planeDist) < EPSILON)
points[numPoints++] = corners[j];
}
// add edge intersections
for(int j = 0; j < 12; ++j)
{
vec3 a = corners[edges[j].start],
b = corners[edges[j].end],
ab = b - a;
// intersect edge with plane
float t = (-plane.m_d - Dot(plane.m_n, a)) / Dot(plane.m_n, ab);
if(t >= 0.f && t <= 1.f)
{
vec3 pt = a + t * ab,
ptA = a - pt,
ptB = b - pt;
float distSqA = LengthSq(ptA);
float distSqB = LengthSq(ptB);
if(distSqA > EPSILON_SQ && distSqB > EPSILON_SQ)
{
points[numPoints++] = pt;
if(numPoints == 6)
break;
}
}
}
if(numPoints < 3)
return;
// Sort results
const float inv_num = 1.f / numPoints;
vec3 center = {0,0,0};
for(int j = 0; j < numPoints; ++j)
center += inv_num * points[j];
vec3 sideVec = Normalize(points[0] - center);
vec3 upVec = Normalize(Cross(plane.m_n, sideVec));
for(int j = 1; j < numPoints; ++j)
{
vec3 toPointJ = points[j] - center;
float angleJ = AngleWrap(atan2(Dot(upVec, toPointJ), Dot(sideVec, toPointJ)));
for(int k = j+1; k < numPoints; ++k)
{
vec3 toPointK = points[k] - center;
float angleK = AngleWrap(atan2(Dot(upVec, toPointK), Dot(sideVec, toPointK)));
if(angleK < angleJ)
{
angleJ = angleK;
std::swap(points[j], points[k]);
}
}
}
// Draw outline
const ShaderInfo* shader = g_dbgdrawShader.get();
GLint posLoc = shader->m_attrs[GEOM_Pos];
GLint colorLoc = shader->m_attrs[GEOM_Color];
glVertexAttrib3fv(colorLoc, &color.r);
glLineWidth(2.f);
glBegin(GL_LINES);
for(int j = 0; j < numPoints; ++j)
{
int next = (j + 1) % numPoints;
glVertexAttrib3fv(posLoc, &points[j].x);
glVertexAttrib3fv(posLoc, &points[next].x);
}
glEnd();
glLineWidth(1.f);
// Draw triangles
glVertexAttrib3fv(colorLoc, &color.r);
glBegin(GL_TRIANGLE_FAN);
glVertexAttrib3fv(posLoc, ¢er.x);
for(int j = 0; j < numPoints; ++j)
glVertexAttrib3fv(posLoc, &points[j].x);
glVertexAttrib3fv(posLoc, &points[0].x);
glEnd();
glBegin(GL_TRIANGLE_FAN);
glVertexAttrib3fv(posLoc, ¢er.x);
for(int j = numPoints-1; j >= 0; --j)
glVertexAttrib3fv(posLoc, &points[j].x);
glVertexAttrib3fv(posLoc, &points[numPoints-1].x);
glEnd();
}
