/// Implement Shape.
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex){
throw new NotImplementedException();
//Vec2 position = transform.p + Utilities.Mul(transform.q, m_p);
//Vec2 s = input.p1 - position;
//float b = Utilities.Dot(s, s) - m_radius * m_radius;
//// Solve quadratic equation.
//Vec2 r = input.p2 - input.p1;
//float c = Utilities.Dot(s, r);
//float rr = Utilities.Dot(r, r);
//float sigma = c * c - rr * b;
//// Check for negative discriminant and short segment.
//if (sigma < 0.0f || rr < Single.Epsilon)
//{
// return false;
//}
//// Find the point of intersection of the line with the circle.
//float a = -(c + (float)Math.Sqrt(sigma));
//// Is the intersection point on the segment?
//if (0.0f <= a && a <= input.maxFraction * rr)
//{
// a /= rr;
// output.fraction = a;
// output.normal = s + a * r;
// output.normal.Normalize();
// return true;
//}
//return false;
}
/// Implement Shape.
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex){
throw new NotImplementedException();
//// Put the ray into the edge's frame of reference.
//Vec2 p1 = Utilities.MulT(xf.q, input.p1 - xf.p);
//Vec2 p2 = Utilities.MulT(xf.q, input.p2 - xf.p);
//Vec2 d = p2 - p1;
//Vec2 v1 = m_vertex1;
//Vec2 v2 = m_vertex2;
//Vec2 e = v2 - v1;
//Vec2 normal(e.Y, -e.X);
//normal.Normalize();
//// q = p1 + t * d
//// dot(normal, q - v1) = 0
//// dot(normal, p1 - v1) + t * dot(normal, d) = 0
//float numerator = Utilities.Dot(normal, v1 - p1);
//float denominator = Utilities.Dot(normal, d);
//if (denominator == 0.0f)
//{
// return false;
//}
//float t = numerator / denominator;
//if (t < 0.0f || input.maxFraction < t)
//{
// return false;
//}
//Vec2 q = p1 + t * d;
//// q = v1 + s * r
//// s = dot(q - v1, r) / dot(r, r)
//Vec2 r = v2 - v1;
//float rr = Utilities.Dot(r, r);
//if (rr == 0.0f)
//{
// return false;
//}
//float s = Utilities.Dot(q - v1, r) / rr;
//if (s < 0.0f || 1.0f < s)
//{
// return false;
//}
//output.fraction = t;
//if (numerator > 0.0f)
//{
// output.normal = -normal;
//}
//else
//{
// output.normal = normal;
//}
//return true;
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
//Debug.Assert(childIndex < _count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == _count)
{
i2 = 0;
}
s_edgeShape._vertex1 = _vertices[i1];
s_edgeShape._vertex2 = _vertices[i2];
return(s_edgeShape.RayCast(out output, ref input, ref transform, 0));
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex)
{
throw new NotImplementedException();
//Utilities.Assert(childIndex < m_count);
//EdgeShape edgeShape;
//int i1 = childIndex;
//int i2 = childIndex + 1;
//if (i2 == m_count)
//{
// i2 = 0;
//}
//edgeShape.m_vertex1 = m_vertices[i1];
//edgeShape.m_vertex2 = m_vertices[i2];
//return edgeShape.RayCast(output, input, xf, 0);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = new RayCastOutput();
Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
Vector2 s = input.p1 - position;
float b = Vector2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vector2 r = input.p2 - input.p1;
float c = Vector2.Dot(s, r);
float rr = Vector2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.b2_epsilon)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(c + (float)Math.Sqrt((double)sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
Vector2 norm = (s + a * r);
norm.Normalize();
output.normal = norm;
return(true);
}
return(false);
}
/// Implement Shape.
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex)
{
throw new NotImplementedException();
//Vec2 position = transform.p + Utilities.Mul(transform.q, m_p);
//Vec2 s = input.p1 - position;
//float b = Utilities.Dot(s, s) - m_radius * m_radius;
//// Solve quadratic equation.
//Vec2 r = input.p2 - input.p1;
//float c = Utilities.Dot(s, r);
//float rr = Utilities.Dot(r, r);
//float sigma = c * c - rr * b;
//// Check for negative discriminant and short segment.
//if (sigma < 0.0f || rr < Single.Epsilon)
//{
// return false;
//}
//// Find the point of intersection of the line with the circle.
//float a = -(c + (float)Math.Sqrt(sigma));
//// Is the intersection point on the segment?
//if (0.0f <= a && a <= input.maxFraction * rr)
//{
// a /= rr;
// output.fraction = a;
// output.normal = s + a * r;
// output.normal.Normalize();
// return true;
//}
//return false;
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex){
throw new NotImplementedException();
//Utilities.Assert(childIndex < m_count);
//EdgeShape edgeShape;
//int i1 = childIndex;
//int i2 = childIndex + 1;
//if (i2 == m_count)
//{
// i2 = 0;
//}
//edgeShape.m_vertex1 = m_vertices[i1];
//edgeShape.m_vertex2 = m_vertices[i2];
//return edgeShape.RayCast(output, input, xf, 0);
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
{
throw new NotImplementedException();
//// Put the ray into the polygon's frame of reference.
//Vec2 p1 = Utilities.MulT(xf.q, input.p1 - xf.p);
//Vec2 p2 = Utilities.MulT(xf.q, input.p2 - xf.p);
//Vec2 d = p2 - p1;
//float lower = 0.0f, upper = input.maxFraction;
//int index = -1;
//for (int i = 0; i < m_count; ++i)
//{
// // p = p1 + a * d
// // dot(normal, p - v) = 0
// // dot(normal, p1 - v) + a * dot(normal, d) = 0
// float numerator = Utilities.Dot(m_normals[i], m_vertices[i] - p1);
// float denominator = Utilities.Dot(m_normals[i], d);
// if (denominator == 0.0f)
// {
// if (numerator < 0.0f)
// {
// return false;
// }
// }
// else
// {
// // Note: we want this predicate without division:
// // lower < numerator / denominator, where denominator < 0
// // Since denominator < 0, we have to flip the inequality:
// // lower < numerator / denominator <==> denominator * lower > numerator.
// if (denominator < 0.0f && numerator < lower * denominator)
// {
// // Increase lower.
// // The segment enters this half-space.
// lower = numerator / denominator;
// index = i;
// }
// else if (denominator > 0.0f && numerator < upper * denominator)
// {
// // Decrease upper.
// // The segment exits this half-space.
// upper = numerator / denominator;
// }
// }
// // The use of epsilon here causes the assert on lower to trip
// // in some cases. Apparently the use of epsilon was to make edge
// // shapes work, but now those are handled separately.
// //if (upper < lower - Single.Epsilon)
// if (upper < lower)
// {
// return false;
// }
//}
//Utilities.Assert(0.0f <= lower && lower <= input.maxFraction);
//if (index >= 0)
//{
// output.fraction = lower;
// output.normal = Utilities.Mul(xf.q, m_normals[index]);
// return true;
//}
//return false;
}
private void RayCast()
{
m_rayActor = null;
RayCastInput input = m_rayCastInput;
// Ray cast against the dynamic tree.
m_tree.RayCast(this, input);
// Brute force ray cast.
Actor bruteActor = null;
RayCastOutput bruteOutput = new RayCastOutput();
for (int i = 0; i < e_actorCount; ++i)
{
if (m_actors[i].proxyId == TreeNode._nullNode)
{
continue;
}
RayCastOutput output;
bool hit = m_actors[i].aabb.RayCast(out output, input);
if (hit)
{
bruteActor = m_actors[i];
bruteOutput = output;
input.maxFraction = output.fraction;
}
}
if (bruteActor != null)
{
Utilities.Assert(bruteOutput.fraction == m_rayCastOutput.fraction);
}
}
/// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. /// @param childIndex the child shape index public abstract bool RayCast(out RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
public bool RayCast(out RayCastOutput output, RayCastInput input) {
float tmin = -Single.MaxValue;
float tmax = Single.MaxValue;
Vec2 p = input.p1;
Vec2 d = input.p2 - input.p1;
Vec2 absD = Utilities.Abs(d);
output = new RayCastOutput();
Vec2 normal = new Vec2();
for (int i = 0; i < 2; ++i) {
if (Math.Abs(i) < Single.Epsilon) {
// Parallel.
if (p[i] < lowerBound[i] || upperBound[i] < p[i]) {
return false;
}
} else {
float inv_d = 1.0f / d[i];
float t1 = (lowerBound[i] - p[i]) * inv_d;
float t2 = (upperBound[i] - p[i]) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2) {
float temp = t1;
t1 = t2;
t2 = temp;
s = 1.0f;
}
// Push the min up
if (t1 > tmin) {
normal.SetZero();
normal[i] = s;
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax) {
return false;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin) {
return false;
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
return true;
}
/// Implement Shape.
///
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
//
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.p1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.p2 - transform.Position);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return(false);
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return(true);
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, RayCastInput input, Transform transform, int childIndex) {
throw new NotImplementedException();
//// Put the ray into the polygon's frame of reference.
//Vec2 p1 = Utilities.MulT(xf.q, input.p1 - xf.p);
//Vec2 p2 = Utilities.MulT(xf.q, input.p2 - xf.p);
//Vec2 d = p2 - p1;
//float lower = 0.0f, upper = input.maxFraction;
//int index = -1;
//for (int i = 0; i < m_count; ++i)
//{
// // p = p1 + a * d
// // dot(normal, p - v) = 0
// // dot(normal, p1 - v) + a * dot(normal, d) = 0
// float numerator = Utilities.Dot(m_normals[i], m_vertices[i] - p1);
// float denominator = Utilities.Dot(m_normals[i], d);
// if (denominator == 0.0f)
// {
// if (numerator < 0.0f)
// {
// return false;
// }
// }
// else
// {
// // Note: we want this predicate without division:
// // lower < numerator / denominator, where denominator < 0
// // Since denominator < 0, we have to flip the inequality:
// // lower < numerator / denominator <==> denominator * lower > numerator.
// if (denominator < 0.0f && numerator < lower * denominator)
// {
// // Increase lower.
// // The segment enters this half-space.
// lower = numerator / denominator;
// index = i;
// }
// else if (denominator > 0.0f && numerator < upper * denominator)
// {
// // Decrease upper.
// // The segment exits this half-space.
// upper = numerator / denominator;
// }
// }
// // The use of epsilon here causes the assert on lower to trip
// // in some cases. Apparently the use of epsilon was to make edge
// // shapes work, but now those are handled separately.
// //if (upper < lower - Single.Epsilon)
// if (upper < lower)
// {
// return false;
// }
//}
//Utilities.Assert(0.0f <= lower && lower <= input.maxFraction);
//if (index >= 0)
//{
// output.fraction = lower;
// output.normal = Utilities.Mul(xf.q, m_normals[index]);
// return true;
//}
//return false;
}
public bool RayCast(out RayCastOutput output, RayCastInput input)
{
float tmin = -Single.MaxValue;
float tmax = Single.MaxValue;
Vec2 p = input.p1;
Vec2 d = input.p2 - input.p1;
Vec2 absD = Utilities.Abs(d);
output = new RayCastOutput();
Vec2 normal = new Vec2();
for (int i = 0; i < 2; ++i)
{
if (Math.Abs(i) < Single.Epsilon)
{
// Parallel.
if (p[i] < lowerBound[i] || upperBound[i] < p[i])
{
return(false);
}
}
else
{
float inv_d = 1.0f / d[i];
float t1 = (lowerBound[i] - p[i]) * inv_d;
float t2 = (upperBound[i] - p[i]) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal[i] = s;
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
return(false);
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
return(true);
}
/// Cast a ray against this shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
public bool RayCast(out RayCastOutput output, RayCastInput input, int childIndex)
{
return(m_shape.RayCast(out output, input, m_body.GetTransform(), childIndex));
}
/// Cast a ray against this shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
public bool RayCast(out RayCastOutput output, RayCastInput input, int childIndex){
return m_shape.RayCast(out output, input, m_body.GetTransform(), childIndex);
}
/// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. /// @param childIndex the child shape index public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
Vector2 d = p2 - p1;
if (_vertexCount == 2)
{
Vector2 v1 = _vertices[0];
Vector2 v2 = _vertices[1];
Vector2 normal = _normals[0];
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return(false);
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return(true);
}
else
{
float lower = 0.0f, upper = input.maxFraction;
int index = -1;
for (int i = 0; i < _vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vector2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(false);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return(false);
}
}
//Debug.Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output.fraction = lower;
output.normal = MathUtils.Multiply(ref xf.R, _normals[index]);
return(true);
}
}
return(false);
}
/// Implement Shape.
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex)
{
throw new NotImplementedException();
//// Put the ray into the edge's frame of reference.
//Vec2 p1 = Utilities.MulT(xf.q, input.p1 - xf.p);
//Vec2 p2 = Utilities.MulT(xf.q, input.p2 - xf.p);
//Vec2 d = p2 - p1;
//Vec2 v1 = m_vertex1;
//Vec2 v2 = m_vertex2;
//Vec2 e = v2 - v1;
//Vec2 normal(e.Y, -e.X);
//normal.Normalize();
//// q = p1 + t * d
//// dot(normal, q - v1) = 0
//// dot(normal, p1 - v1) + t * dot(normal, d) = 0
//float numerator = Utilities.Dot(normal, v1 - p1);
//float denominator = Utilities.Dot(normal, d);
//if (denominator == 0.0f)
//{
// return false;
//}
//float t = numerator / denominator;
//if (t < 0.0f || input.maxFraction < t)
//{
// return false;
//}
//Vec2 q = p1 + t * d;
//// q = v1 + s * r
//// s = dot(q - v1, r) / dot(r, r)
//Vec2 r = v2 - v1;
//float rr = Utilities.Dot(r, r);
//if (rr == 0.0f)
//{
// return false;
//}
//float s = Utilities.Dot(q - v1, r) / rr;
//if (s < 0.0f || 1.0f < s)
//{
// return false;
//}
//output.fraction = t;
//if (numerator > 0.0f)
//{
// output.normal = -normal;
//}
//else
//{
// output.normal = normal;
//}
//return true;
}
