public PlaneBody(Vector3 normal, Vector3 p)
: base(p, Vector3.Zero, 0f, 0, 0f)
{
this.normal = Vector3.Normalize(normal);
this.p = p;
//A line extends infinitely, so make our AABB as big as possible
motionBounds = new AABB3D(Vector3.Zero, new Vector3(float.MaxValue, float.MaxValue, float.MaxValue));
}
public override void GenerateMotionAABB(float dt)
{
Vector3 predictedPos = Pos + (Vel * dt);
Vector3 center = (Pos + predictedPos) / 2f;
Vector3 halfExtents = new Vector3((Math.Abs(predictedPos.X - Pos.X) * 0.5f) + radius, (Math.Abs(predictedPos.Y - Pos.Y) * 0.5f) + radius, (Math.Abs(predictedPos.Z - Pos.Z) * 0.5f) + radius);
motionBounds = new AABB3D(center, halfExtents);
}
/// <summary>
///
/// </summary>
/// <param name="pVer"></param>
/// <param name="verNum"></param>
/// <param name="pOut"></param>
public virtual void VGetBoundingSphereFromVertex(IntPtr pVer, int verNum, out Sphere s)
{
AABB3D aabb = new AABB3D();
aabb.InitMinMax();
VGetBoundingAABBFromVertex(pVer, verNum, out aabb);
s.c = (aabb.vMin + aabb.vMax) * 0.5f;
s.r = (aabb.vMin - s.c).Length();
}
public GridIntersectableMesh(Mesh <V> mesh)
{
this.mesh = mesh;
resolution = 20;
triangleHash = new List <int> [resolution, resolution, resolution];
box = mesh.ComputeAABB();
float3 dim = box.Maximum - box.Minimum;
float maxDim = max(dim.x, max(dim.y, dim.z));
box = new AABB3D {
Minimum = box.Minimum - 0.1f, Maximum = box.Minimum + maxDim + 0.1f
}; // extend to grant all mesh-intersections are inside the box.
for (int i = 0; i < mesh.Indices.Length / 3; i++)
{
V v1 = mesh.Vertices[mesh.Indices[i * 3 + 0]];
V v2 = mesh.Vertices[mesh.Indices[i * 3 + 1]];
V v3 = mesh.Vertices[mesh.Indices[i * 3 + 2]];
Triangle3D tri = new Triangle3D(v1.Position, v2.Position, v3.Position);
float3 minimum = min(v1.Position, min(v2.Position, v3.Position));
float3 maximum = max(v1.Position, max(v2.Position, v3.Position));
int3 corner1 = (int3)((minimum - box.Minimum) * resolution / (box.Maximum - box.Minimum));
int3 corner2 = (int3)((maximum - box.Minimum) * resolution / (box.Maximum - box.Minimum));
for (int z = corner1.z; z <= corner2.z; z++)
{
for (int y = corner1.y; y <= corner2.y; y++)
{
for (int x = corner1.x; x <= corner2.x; x++)
{
if (triangleHash[z, y, x] == null)
{
triangleHash[z, y, x] = new List <int>();
}
triangleHash[z, y, x].Add(i); // add the triangle to the list.
}
}
}
}
}
// Update is called once per frame
void Update()
{
// Get the length of the cable
float length = getLength();
// Realign particle if it is outside of boundaries
if (length > maxLength)
{
float penetration = length - maxLength;
Vector3 collisionNormal = new Vector3(0, parent.transform.position.y - transform.position.y, 0);
collisionNormal = collisionNormal.normalized;
AABB3D parentHull = parent.GetComponent <AABB3D>();
//CollisionManager3D.CollisionInfo newCollision = new CollisionManager3D.CollisionInfo(GetComponent<AABB3D>(), parent.GetComponent<AABB3D>(), penetration, collisionNormal, Vector3.zero, CollisionResolution3D.CalculateSeparatingVelocity(a, b, a.GetPosition(), b.GetPosition()));
//CollisionResolution3D.ResolvePenetration(newCollision);
//CollisionResolution3D.ResolveVelocities(newCollision, Time.deltaTime);
}
}
/// <summary> /// 視錐台とAABBによる当たり判定！ /// </summary> /// <param name="aabb">aabb</param> /// <returns>錐台内に点がある場合はtrueを返す</returns> public abstract bool CollisionAABB(AABB3D aabb);
public override void GenerateContacts(ref List <RigidBody3D> bodies, ref List <Contact3D> contacts, float dt)
{
//Clear all cells
for (int i = 0; i < cells.Length; i++)
{
cells[i].Clear();
}
//Insert bodies into all cells they overlap
for (int i = 0; i < bodies.Count; i++)
{
AABB3D bounds = bodies[i].MotionBounds;
Vector3 min = bounds.GetMin();
Vector3 max = bounds.GetMax();
int startRow = (int)(min.X * invWidth * rows);
startRow = (int)Math.Max(startRow, 0);
int startCol = (int)(min.Y * invHeight * cols);
startCol = (int)Math.Max(startCol, 0);
int startStack = (int)(min.Z * invDepth * stacks);
startStack = (int)Math.Max(startStack, 0);
int endRow = (int)(max.X * invWidth * rows);
endRow = (int)Math.Min(endRow, rows - 1);
int endCol = (int)(max.Y * invHeight * cols);
endCol = (int)Math.Min(endCol, cols - 1);
int endStack = (int)(max.Z * invDepth * stacks);
endStack = (int)Math.Min(endStack, stacks - 1);
for (int j = startRow; j <= endRow; j++)
{
for (int k = startCol; k <= endCol; k++)
{
for (int l = startStack; l <= endStack; l++)
{
cells[(j * cols) + (k * rows) + l].AddObject(i);
}
}
}
}
//Loop through each cell, add speculative contacts to each object in the cell
for (int i = 0; i < cells.Length; i++)
{
if (cells[i].Indices.Count <= 0)
{
continue;
}
Cell3D currentCell = cells[i];
foreach (int index1 in currentCell.Indices)
{
foreach (int index2 in currentCell.Indices)
{
if (index1 == index2)
{
continue;
}
RigidBody3D a = bodies[index1];
RigidBody3D b = bodies[index2];
if (a.InvMass != 0 || b.InvMass != 0)
{
//Add a speculative contact
contacts.Add(a.GenerateContact(b, dt));
}
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="pVer"></param>
/// <param name="verNum"></param>
/// <param name="pOut"></param>
public virtual void VGetBoundingAABBFromVertex(IntPtr pVer, int verNum, out AABB3D aabb)
{
aabb = new AABB3D();
}
/// <summary>
/// 視錐台とAABBによる当たり判定！ 最適化されてないので今後の課題・・・
/// 注意：今は使わない方が良い
/// </summary>
/// <param name=""></param>
/// <param name=""></param>
/// <returns></returns>
public override bool CollisionAABB(AABB3D aabb)
{
MCVector3 vP;
int nOutOfLeft = 0, nOutOfRight = 0, nOutOfFar = 0, nOutOfNear = 0, nOutOfTop = 0, nOutOfBottom = 0;
bool bIsInRightTest, bIsInUpTest, bIsInFrontTest;
MCVector3[] avCorners = new MCVector3[2];
avCorners[0] = aabb.vMin - m_eye;
avCorners[1] = aabb.vMax - m_eye;
for (int i = 0; i < 8; ++i)
{
bIsInRightTest = bIsInUpTest = bIsInFrontTest = false;
vP.X = avCorners[i & 1].X;
vP.Y = avCorners[(i >> 2) & 1].Y;
vP.Z = avCorners[(i >> 1) & 1].Z;
float r = m_viewRight.X * vP.X + m_viewRight.Y * vP.Y + m_viewRight.Z * vP.Z;
float u = m_viewUp.X * vP.X + m_viewUp.Y * vP.Y + m_viewUp.Z * vP.Z;
float f = m_viewForward.X * vP.X + m_viewForward.Y * vP.Y + m_viewForward.Z * vP.Z;
if (r < -m_rightFactor * f)
{
++nOutOfLeft;
}
else if (r > m_rightFactor * f)
{
++nOutOfRight;
}
else
{
bIsInRightTest = true;
}
if (u < -m_upFactor * f)
{
++nOutOfBottom;
}
else if (u > m_upFactor * f)
{
++nOutOfTop;
}
else
{
bIsInUpTest = true;
}
if (f < m_nearPlane)
{
++nOutOfNear;
}
else if (f > m_farPlane)
{
++nOutOfFar;
}
else
{
bIsInFrontTest = true;
}
if (bIsInRightTest && bIsInFrontTest && bIsInUpTest)
{
return(true);
}
}
if (nOutOfLeft == 8 || nOutOfRight == 8 || nOutOfFar == 8 || nOutOfNear == 8 || nOutOfTop == 8 || nOutOfBottom == 8)
{
return(false);
}
return(true);
}
