public static bool IntersectSphereTriangle(Sphere sphere, Triangle trig, ref Geometry.Geometric_Queries.Distance_Query_Result result)
{
var mink_sph = new Minkowski_Description();
mink_sph.HalfAxis = new Vector4(sphere.Radius, sphere.Radius, sphere.Radius, 1);
mink_sph.Position_LM = sphere.Position - trig.Bary_center;
var sphere_from_trig = trig.What_Faces(mink_sph);
if (sphere_from_trig.Signed_Dist > 0.0f)
{
return(false);
}
result = new Geometry.Geometric_Queries.Distance_Query_Result();
result.first_has_normal = false;
result.Normal_W = trig.Normals[sphere_from_trig.Normal_Code];
var Extreme_Point_In_Normal_Of_Body_1 = trig.Points[sphere_from_trig.Normal_Code] - Intersection_Grid.current_pair_element_2.Rigid.Position;
var Extreme_Point_In_Normal_Of_Body_2 = -result.Normal_W * sphere.Radius;
result.signed_distance = sphere_from_trig.Signed_Dist;
result.double_contact = false;
result.Point_W_0 = Extreme_Point_In_Normal_Of_Body_2 + sphere.Position;
return(true);
}
public static Minkowski_Description Minkowskize_OBB_In_OBB(OBB obb1, OBB obb2)
{
var ret = new Minkowski_Description();
var tmp = obb1.Position - obb2.Position;
var res = Vector3.Transform(tmp, obb2.TOrientation);
var res2 = new Vector4();
var axisxinnewcords = Vector3.Transform(obb1.Axis(0), obb2.TOrientation);
var axisyinnewcords = Vector3.Transform(obb1.Axis(1), obb2.TOrientation);
var xradius = obb1.HalfAxisWidth.X * Math.Abs(axisxinnewcords.X) + obb1.HalfAxisWidth.Y * Math.Abs(axisyinnewcords.X);
var yradius = obb1.HalfAxisWidth.X * Math.Abs(axisxinnewcords.Y) + obb1.HalfAxisWidth.Y * Math.Abs(axisyinnewcords.Y);
res2.X = xradius;
res2.Y = yradius;
res2.Z = 0;
ret.HalfAxis = res2;
ret.Position_LM = res;
return(ret);
}
public static Minkowski_Description Minkowskize_Sphere_In_OBB(Sphere sph, OBB obb)
{
var ret = new Minkowski_Description();
ret.HalfAxis.X = sph.Radius;
ret.Position_LM = obb.Point_W_To_L(sph.Position);
return(ret);
}
public static Minkowski_Description Minkowskize_Sphere_In_OBB(Sphere sph, OBB obb)
{
var ret = new Minkowski_Description();
ret.HalfAxis.X = sph.Radius;
ret.Position_LM = obb.Point_W_To_L(sph.Position);
return ret;
}
public static Minkowski_Description Minkowskize_OBB_In_Triangle(OBB obb, Triangle trig)
{
var ret = new Minkowski_Description();
ret.HalfAxis.X = Math.Abs(obb.Extreme_In(trig.Normals[0]).dist_in_dir_w);
ret.HalfAxis.Y = Math.Abs(obb.Extreme_In(trig.Normals[1]).dist_in_dir_w);
ret.HalfAxis.Z = Math.Abs(obb.Extreme_In(trig.Normals[2]).dist_in_dir_w);
ret.Position_LM = obb.Position - trig.Bary_center;
return ret;
}
public static Minkowski_Description Minkowskize_OBB_In_Triangle(OBB obb, Triangle trig)
{
var ret = new Minkowski_Description();
ret.HalfAxis.X = Math.Abs(obb.Extreme_In(trig.Normals[0]).dist_in_dir_w);
ret.HalfAxis.Y = Math.Abs(obb.Extreme_In(trig.Normals[1]).dist_in_dir_w);
ret.HalfAxis.Z = Math.Abs(obb.Extreme_In(trig.Normals[2]).dist_in_dir_w);
ret.Position_LM = obb.Position - trig.Bary_center;
return(ret);
}
public static Minkowski_Description Minkowskize_Triangle_In_OBB(Triangle trig, OBB obb)
{
var ret = new Minkowski_Description();
ret.HalfAxis.X = trig.Extreme_In(obb.Normal(0)).dist_in_dir_w;
ret.HalfAxis.Y = trig.Extreme_In(obb.Normal(1)).dist_in_dir_w;
ret.HalfAxis.Z = trig.Extreme_In(obb.Normal(2)).dist_in_dir_w;
ret.HalfAxis.W = trig.Extreme_In(obb.Normal(3)).dist_in_dir_w;
ret.Position_LM = obb.Point_W_To_L(trig.Bary_center);
return ret;
}
public static Minkowski_Description Minkowskize_Triangle_In_OBB(Triangle trig, OBB obb)
{
var ret = new Minkowski_Description();
ret.HalfAxis.X = trig.Extreme_In(obb.Normal(0)).dist_in_dir_w;
ret.HalfAxis.Y = trig.Extreme_In(obb.Normal(1)).dist_in_dir_w;
ret.HalfAxis.Z = trig.Extreme_In(obb.Normal(2)).dist_in_dir_w;
ret.HalfAxis.W = trig.Extreme_In(obb.Normal(3)).dist_in_dir_w;
ret.Position_LM = obb.Point_W_To_L(trig.Bary_center);
return(ret);
}
public static Minkowski_Description Minkowskize_OBB_In_OBB(OBB obb1, OBB obb2)
{
var ret = new Minkowski_Description();
var tmp = obb1.Position - obb2.Position;
var res = Vector3.Transform(tmp, obb2.TOrientation);
var res2 = new Vector4();
var axisxinnewcords = Vector3.Transform(obb1.Axis(0), obb2.TOrientation);
var axisyinnewcords = Vector3.Transform(obb1.Axis(1), obb2.TOrientation);
var xradius = obb1.HalfAxisWidth.X * Math.Abs(axisxinnewcords.X) + obb1.HalfAxisWidth.Y * Math.Abs(axisyinnewcords.X);
var yradius = obb1.HalfAxisWidth.X * Math.Abs(axisxinnewcords.Y) + obb1.HalfAxisWidth.Y * Math.Abs(axisyinnewcords.Y);
res2.X = xradius;
res2.Y = yradius;
res2.Z = 0;
ret.HalfAxis = res2;
ret.Position_LM = res;
return ret;
}
public static bool IntersectSphereTriangle(Sphere sphere, Triangle trig, ref Geometry.Geometric_Queries.Distance_Query_Result result)
{
var mink_sph = new Minkowski_Description();
mink_sph.HalfAxis = new Vector4(sphere.Radius, sphere.Radius, sphere.Radius,1);
mink_sph.Position_LM = sphere.Position - trig.Bary_center;
var sphere_from_trig = trig.What_Faces(mink_sph);
if (sphere_from_trig.Signed_Dist > 0.0f) return false;
result = new Geometry.Geometric_Queries.Distance_Query_Result();
result.first_has_normal = false;
result.Normal_W = trig.Normals[sphere_from_trig.Normal_Code];
var Extreme_Point_In_Normal_Of_Body_1 = trig.Points[sphere_from_trig.Normal_Code] - Intersection_Grid.current_pair_element_2.Rigid.Position;
var Extreme_Point_In_Normal_Of_Body_2 = -result.Normal_W * sphere.Radius;
result.signed_distance = sphere_from_trig.Signed_Dist;
result.double_contact=false;
result.Point_W_0 = Extreme_Point_In_Normal_Of_Body_2 + sphere.Position;
return true;
}
