示例#1
0
//=============================================================================
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;
}
示例#2
0
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;
}
示例#3
0
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;
}
示例#4
0
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;
}
示例#7
0
//=============================================================================
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;
  }
}
示例#9
0
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;
}
示例#10
0
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;
}
示例#11
0
//=============================================================================
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;
}
示例#12
0
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;
  }
}
示例#13
0
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;
}
示例#14
0
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;
}
示例#16
0
文件: Base.cpp 项目: edeksumo/carpg
inline float DistanceSq(const VEC3 &p1, const VEC3 &p2)
{
	return LengthSq(p2-p1);
}
示例#17
0
 float Vector2i::Length(void) const
 {
   return sqrt(static_cast<float>(LengthSq()));
 }
示例#18
0
float Vector2::Length() const
{
	return (float)sqrt(LengthSq());
}
示例#19
0
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);
}
示例#20
0
float Quaternion::Length(void) const
{
  return Math::Sqrt(LengthSq());
}
示例#21
0
bool getCollideSpheres(Vector3& posA, Vector3& posB, float rA, float rB)
{
    return LengthSq(posA - posB) < ((rA + rB) * (rA + rB));
}
示例#22
0
//---------------------------------
//--- Return length of Vector2D ---
//--- Using LengthSq() ------------
//---------------------------------
float Length(const Vector2D* const vect) {
	return sqrt(LengthSq(vect));
}
示例#23
0
bool Quat::IsInvertible(float epsilon) const
{
	return LengthSq() > epsilon && IsFinite();
}
示例#24
0
bool Quat::IsNormalized(float epsilon) const
{
	return EqualAbs(LengthSq(), 1.f, epsilon);
}
示例#25
0
float Quat::Length() const
{
	return sqrtf(LengthSq());
}
示例#26
0
double Vector2::Length() const { return sqrt(LengthSq()); }
示例#27
0
/**
 *	クォータニオンの長さを取得する
 *
 *	|Q| = sqrt(Qw^2 + Qx^2 + Qy^2 + Qz^2)
 *	@return	クォータニオンの長さ
 */
float Quaternion::Length() const
{
	return sqrt(LengthSq());
}
示例#28
0
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, &center.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, &center.x);
	for(int j = numPoints-1; j >= 0; --j)
		glVertexAttrib3fv(posLoc, &points[j].x);
	glVertexAttrib3fv(posLoc, &points[numPoints-1].x);
	glEnd();

}