Ejemplo n.º 1
0
DenseIntVectSet& DenseIntVectSet::operator|=(const DenseIntVectSet& d)
{
  if (m_domain.contains(d.m_domain))
        {
          BoxIterator bit(d.m_domain);
          int i=0;
          for (bit.begin(); bit.ok(); ++bit, ++i)
          {
                if (d.m_bits[i]) m_bits.setTrue(m_domain.index(bit()));
          }
        }
  else if (d.m_domain.contains(m_domain))
        {
          DenseIntVectSet newSet = d;
          BoxIterator bit(m_domain);
          int i=0;
          for (bit.begin(); bit.ok(); ++bit, ++i)
          {
                if (m_bits[i]) newSet.m_bits.setTrue(newSet.m_domain.index(bit()));
          }
          *this = newSet;
        }
  else
        {
          Box newDomain = minBox(m_domain, d.m_domain);
          DenseIntVectSet newSet(newDomain, false);
          newSet |= *this;
          newSet |= d;
          *this = newSet;
        }
  return *this;
}
static int MakeRandomGuassianPointCloud (NewtonMesh* const mesh, dVector* const points, int count)
{
	dVector size(0.0f);
	dMatrix matrix(dGetIdentityMatrix()); 
	NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);

	dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z));
	dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z));

	size = (maxBox - minBox).Scale (0.5f);
	dVector origin = (maxBox + minBox).Scale (0.5f);

	dFloat biasExp = 10.0f;
	dFloat r = dSqrt (size.DotProduct3(size));
	r = powf(r, 1.0f/biasExp);
	for (int i = 0; i < count; i++) {
		dVector& p = points[i];
		bool test;
		do {
			p = dVector (2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 0.0f);
			dFloat len = dSqrt (p.DotProduct3(p));
			dFloat scale = powf(len, biasExp) / len;
			p = p.Scale (scale) + origin;
			test = (p.m_x > minBox.m_x) && (p.m_x < maxBox.m_x) && (p.m_y > minBox.m_y) && (p.m_y < maxBox.m_y) && (p.m_z > minBox.m_z) && (p.m_z < maxBox.m_z);
		} while (!test);
	}

	return count;
}
Ejemplo n.º 3
0
    bool isCenterInViewport(const Frustum& frustum, const Boxf& worldBox,
                            const Viewport& viewport) const
    {
        Vector4f center = worldBox.getCenter();
        center[3] = 1.0;

        Vector4f mvpCenter = frustum.getMVPMatrix() * center;
        Vector3f mvpCenterHom = mvpCenter / mvpCenter[3];

        const bool isNegXBorder = viewport[0] == 0.0f; // left
        const bool isPosXBorder =
            viewport[0] + viewport[2] == 1.0f;         // left + width
        const bool isNegYBorder = viewport[1] == 0.0f; // top
        const bool isPosYBorder =
            viewport[1] + viewport[3] == 1.0f; // top + height

        Vector3f minBox(-1.0f);
        Vector3f maxBox(1.0f);
        if (isNegXBorder)
            minBox[0] = -infinite;
        if (isPosXBorder)
            maxBox[0] = infinite;
        if (isNegYBorder)
            minBox[1] = -infinite;
        if (isPosYBorder)
            maxBox[1] = infinite;

        minBox[2] = -infinite;
        maxBox[2] = infinite;

        const Boxf ndcCube(minBox, maxBox);
        return ndcCube.isIn(mvpCenterHom);
    }
Ejemplo n.º 4
0
IntVectSet& IntVectSet::operator&=(const IntVectSet& ivs)
{
  if (!(minBox().intersects(ivs.minBox())))
    {
      m_ivs.clear();
      m_isdense = true;
      m_dense = DenseIntVectSet();
      return *this;
    }
  if (m_isdense)
    {
      if (ivs.m_isdense) m_dense&=ivs.m_dense;
      else
        {
          convert();
          m_ivs &= ivs.m_ivs;
        }
    }
  else
    {
      if (ivs.m_isdense) ivs.convert();
        m_ivs &= ivs.m_ivs;
    }

  return *this;
}
dgInt32 dgBroadPhaseDefault::ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32& timeToImpact, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const
{
	dgInt32 totalCount = 0;
	if (m_rootNode) {
		dgVector boxP0;
		dgVector boxP1;
		dgAssert(matrix.TestOrthogonal());
		shape->CalcAABB(matrix, boxP0, boxP1);

		dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH];
		const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH];

		dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask);
		dgVector velocB(dgFloat32(0.0f));
		dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA);

		dgVector minBox(m_rootNode->m_minBox - boxP1);
		dgVector maxBox(m_rootNode->m_maxBox - boxP0);
		stackPool[0] = m_rootNode;
		distance[0] = ray.BoxIntersect(minBox, maxBox);

		totalCount = dgBroadPhase::ConvexCast(stackPool, distance, 1, velocA, velocB, ray, shape, matrix, target, timeToImpact, prefilter, userData, info, maxContacts, threadIndex);
	}

	return totalCount;
}
Ejemplo n.º 6
0
void LoadLumberYardMesh(DemoEntityManager* const scene, const dVector& location, int shapeid)
{
	DemoEntity* const entity = DemoEntity::LoadNGD_mesh ("lumber.ngd", scene->GetNewton(), scene->GetShaderCache());

	dTree<NewtonCollision*, DemoMesh*> filter;
	NewtonWorld* const world = scene->GetNewton();

	dFloat density = 15.0f;

	int defaultMaterialID = NewtonMaterialGetDefaultGroupID(scene->GetNewton());
	for (DemoEntity* child = entity->GetFirst(); child; child = child->GetNext()) {
		DemoMesh* const mesh = (DemoMesh*)child->GetMesh();
		if (mesh) {
			dAssert(mesh->IsType(DemoMesh::GetRttiType()));
			dTree<NewtonCollision*, DemoMesh*>::dTreeNode* node = filter.Find(mesh);
			if (!node) {
				// make a collision shape only for and instance
				dFloat* const array = mesh->m_vertex;
				dVector minBox(1.0e10f, 1.0e10f, 1.0e10f, 1.0f);
				dVector maxBox(-1.0e10f, -1.0e10f, -1.0e10f, 1.0f);

				for (int i = 0; i < mesh->m_vertexCount; i++) {
					dVector p(array[i * 3 + 0], array[i * 3 + 1], array[i * 3 + 2], 1.0f);
					minBox.m_x = dMin(p.m_x, minBox.m_x);
					minBox.m_y = dMin(p.m_y, minBox.m_y);
					minBox.m_z = dMin(p.m_z, minBox.m_z);

					maxBox.m_x = dMax(p.m_x, maxBox.m_x);
					maxBox.m_y = dMax(p.m_y, maxBox.m_y);
					maxBox.m_z = dMax(p.m_z, maxBox.m_z);
				}

				dVector size(maxBox - minBox);
				dMatrix offset(dGetIdentityMatrix());
				offset.m_posit = (maxBox + minBox).Scale(0.5f);
				NewtonCollision* const shape = NewtonCreateBox(world, size.m_x, size.m_y, size.m_z, shapeid, &offset[0][0]);
				node = filter.Insert(shape, mesh);
			}

			// create a body and add to the world
			NewtonCollision* const shape = node->GetInfo();
			dMatrix matrix(child->GetMeshMatrix() * child->CalculateGlobalMatrix());
			matrix.m_posit += location;
			dFloat mass = density * NewtonConvexCollisionCalculateVolume(shape);
			CreateSimpleSolid(scene, mesh, mass, matrix, shape, defaultMaterialID);
		}
	}

	// destroy all shapes
	while (filter.GetRoot()) {
		NewtonCollision* const shape = filter.GetRoot()->GetInfo();
		NewtonDestroyCollision(shape);
		filter.Remove(filter.GetRoot());
	}
	delete entity;
}
Ejemplo n.º 7
0
dgFloat32 dgBody::ConvexRayCast (const dgFastRayTest& ray, const dgCollisionInstance* const convexShape, const dgVector& shapeMinBox, const dgVector& shapeMaxBox, const dgMatrix& origin, const dgVector& shapeVeloc, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 minT, dgInt32 threadId) const
{
	dgVector minBox (m_minAABB - shapeMaxBox);
	dgVector maxBox (m_maxAABB - shapeMinBox);
	if (ray.BoxTest (minBox, maxBox)) {
		dgContactPoint contactOut;
		dgFloat32 t = m_collision->ConvexRayCast (convexShape, origin, shapeVeloc, minT, contactOut, preFilter, this, userData, threadId);
		if (t < minT) {
			dgAssert (t >= dgFloat32 (0.0f));
			dgAssert (t <= dgFloat32 (1.1f));
			minT = filter (this, contactOut.m_collision0, contactOut.m_point, contactOut.m_normal, contactOut.m_shapeId0, userData, t);
		}
	} 
	return minT;
}
	static int PlaneCollisionAABBOverlapTest (void* userData, const dFloat* const box0, const dFloat* const box1)
	{
		const dInfinitePlane* const me = (dInfinitePlane*) userData;

		dVector minBox (box0[0], box0[1], box0[2], 0.0f);
		dFloat test = me->m_plane.DotProduct3(minBox) +  me->m_plane.m_w;
		if (test > 0.0f) {
			return 0;
		}

		dVector maxBox (box1[0], box1[1], box1[2], 0.0f);
		test = me->m_plane.DotProduct3(maxBox) +  me->m_plane.m_w;
		if (test < 0.0f) {
			return 0;
		}
		
		return 1;
	}
Ejemplo n.º 9
0
void Car::initPhysics() {

    NewtonWorld * nWorld = this->controller->getWorld();
    NewtonCollision* collision;
    float mass = 900.0f;

    vector3df v1 = this->carNode->getBoundingBox().MinEdge;
    vector3df v2 = this->carNode->getBoundingBox().MaxEdge;

    dVector minBox(v1.X, v1.Y, v1.Z);
    dVector maxBox(v2.X, v2.Y, v2.Z);

    dVector size(maxBox - minBox);
    dVector origin((maxBox + minBox).Scale(0.5f));

    size.m_w = 1.0f;
    origin.m_w = 1.0f;

    dMatrix offset(GetIdentityMatrix());
    offset.m_posit = origin;

    collision = NewtonCreateBox(nWorld, size.m_x, size.m_y, size.m_z, 0, &offset[0][0]);

    dVector inertia;

    matrix4 m = this->carNode->getRelativeTransformation();
    NewtonConvexHullModifierSetMatrix(collision, m.pointer());
    NewtonBody * body = NewtonCreateBody(nWorld, collision, m.pointer());
    NewtonBodySetUserData(body, this);
    NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
    NewtonBodySetMassMatrix(body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
    NewtonBodySetCentreOfMass(body, &origin[0]);
    NewtonBodySetForceAndTorqueCallback(body, applyCarMoveForce);
    NewtonBodySetTransformCallback(body, applyCarTransform);
    int matId = NewtonMaterialGetDefaultGroupID(nWorld);
    NewtonMaterialSetCollisionCallback(nWorld, matId, matId, this, 0, applyCarCollisionForce);

    NewtonReleaseCollision(nWorld, collision);
    this->setCarBodyAndGravity(body, dVector(0,-10,0,0));
    this->setLocalCoordinates(this->createChassisMatrix());
}
static int MakeRandomPoisonPointCloud(NewtonMesh* const mesh, dVector* const points)
{
	dVector size(0.0f);
	dMatrix matrix(dGetIdentityMatrix()); 
	NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);

	dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z));
	dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z));

	size = maxBox - minBox;
	int xCount = int (size.m_x / POINT_DENSITY_PER_METERS) + 1;
	int yCount = int (size.m_y / POINT_DENSITY_PER_METERS) + 1;
	int zCount = int (size.m_z / POINT_DENSITY_PER_METERS) + 1;

	int count = 0;
	dFloat z0 = minBox.m_z;
	for (int iz = 0; (iz < zCount) && (count < MAX_POINT_CLOUD_SIZE); iz ++) {
		dFloat y0 = minBox.m_y;
		for (int iy = 0; (iy < yCount) && (count < MAX_POINT_CLOUD_SIZE); iy ++) {
			dFloat x0 = minBox.m_x;
			for (int ix = 0; (ix < xCount) && (count < MAX_POINT_CLOUD_SIZE); ix ++) {

				dFloat x = x0;
				dFloat y = y0;
				dFloat z = z0;
				x += RandomVariable(POISON_VARIANCE);
				y += RandomVariable(POISON_VARIANCE);
				z += RandomVariable(POISON_VARIANCE);
				points[count] = dVector (x, y, z);
				count ++;
				x0 += POINT_DENSITY_PER_METERS;
			}
			y0 += POINT_DENSITY_PER_METERS;
		}
		z0 += POINT_DENSITY_PER_METERS;
	}

	return count;
}
void dgBroadPhaseDefault::ConvexRayCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData, dgInt32 threadId) const
{
	if (filter && m_rootNode && shape->IsType(dgCollision::dgCollisionConvexShape_RTTI)) {
		dgVector boxP0;
		dgVector boxP1;
		shape->CalcAABB(shape->GetLocalMatrix() * matrix, boxP0, boxP1);

		//dgInt32 stack = 1;
		dgFloat32 distance[DG_COMPOUND_STACK_DEPTH];
		const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH];

		dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask);
		dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA);

		dgVector minBox(m_rootNode->m_minBox - boxP1);
		dgVector maxBox(m_rootNode->m_maxBox - boxP0);
		stackPool[0] = m_rootNode;
		distance[0] = ray.BoxIntersect(minBox, maxBox);

		dgBroadPhase::ConvexRayCast(stackPool, distance, 1, velocA, ray, shape, matrix, target, filter, prefilter, userData, threadId);
	}
}
Ejemplo n.º 12
0
IntVectSet& IntVectSet::operator-=(const IntVectSet& ivs)
{
  if (!(minBox().intersects(ivs.minBox()))) return *this;
  if (m_isdense)
    {
      if (ivs.m_isdense) m_dense-=ivs.m_dense;
      else
        {
          for (TreeIntVectSetIterator it(ivs.m_ivs); it.ok(); ++it) m_dense -= it();
        }
    }
  else
    {
      if (ivs.m_isdense)
        {
          for (DenseIntVectSetIterator it(ivs.m_dense);it.ok(); ++it) m_ivs -= it();
        }
      else
        m_ivs -= ivs.m_ivs;
    }

  return *this;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
    dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
    dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));

    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgAssert (this == proxy.m_floatingCollision->GetChildShape());
    dgAssert (m_count);
    dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    const dgBody* const floatingBody = proxy.m_floatingBody;
    const dgBody* const referenceBody = proxy.m_referenceBody;

    dgContact* const contactJoint = proxy.m_contactJoint;
    contactJoint->m_closestDistance = dgFloat32 (1.0e10f);

    m_normal = m_normal.CompProduct4(polyInstanceInvScale);
    dgAssert (m_normal.m_w == dgFloat32 (0.0f));
    m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
    const dgVector savedFaceNormal (m_normal);

    for (dgInt32 i = 0; i < m_count; i ++) {
        m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
        dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
    }

    dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
    hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;

    dgMatrix polygonMatrix;
    polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
    polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
    polygonMatrix[1] = m_normal;
    polygonMatrix[2] = polygonMatrix[0] * m_normal;
    polygonMatrix[3] = hullOrigin;
    dgAssert (polygonMatrix.TestOrthogonal());

    dgMatrix savedProxyMatrix (proxy.m_matrix);
    proxy.m_matrix = polygonMatrix * proxy.m_matrix;

    dgVector floatingVeloc (floatingBody->m_veloc);
    dgVector referenceVeloc (referenceBody->m_veloc);
    const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
    dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
    dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));

    dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
    dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
    m_normal = polygonMatrix.UnrotateVector(m_normal);

    if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
        proxy.m_matrix = savedProxyMatrix;
        return 0;
    }
    for (dgInt32 i = 0; i < m_count; i ++) {
        m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
        dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
        polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
        polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
    }
    dgInt32 count = 0;


    dgVector hullBoxP0;
    dgVector hullBoxP1;
    hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
    dgVector minBox (polyBoxP0 - hullBoxP1);
    dgVector maxBox (polyBoxP1 - hullBoxP0);
    dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
    dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);

    if (distance < dgFloat32 (1.0f)) {

        dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
//		dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
//		boxOrigin += polyRelativeVeloc.Scale4 (distance);

        dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
        dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
        dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
        dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
        dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
        dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));

        bool inside = true;
        dgInt32 i0 = m_count - 1;
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector e (m_localPoly[i] - m_localPoly[i0]);
            dgVector n (m_normal * e);
            dgPlane plane (n, - (m_localPoly[i0] % n));

            dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
            dgFloat32 centerDist = plane.Evalue(boxOrigin);

            if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
                proxy.m_matrix = savedProxyMatrix;
                return 0;
            }

            if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
                inside = false;
            }
            i0 = i;
        }

// for the time being for the minkousky contact calculation
        inside = false;
        const dgInt32 hullId = hull->GetUserDataID();
        if (inside) {
            dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
            dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
            dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));

            dgFloat32 timetoImpact = dgFloat32 (0.0f);
            //dgFloat32 closestDistance = dgFloat32 (0.0f);
            dgAssert (0);
//			dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
            dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
            if (penetration < dgFloat32 (0.0f)) {
                timetoImpact = penetration / (polyRelativeVeloc % m_normal);
                dgAssert (timetoImpact >= dgFloat32 (0.0f));
//				closestDistance = -penetration;
            }

            if (timetoImpact <= proxy.m_timestep) {
                dgVector pointsContacts[64];

                contactJoint->m_closestDistance = penetration;
                dgAssert (0);
//				dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
                dgVector point (pointInHull);

                count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
                dgAssert (0);
//				dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
                dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));

                penetration = dgMax (penetration, dgFloat32 (0.0f));
                const dgMatrix& worldMatrix = hull->m_globalMatrix;
                dgContactPoint* const contactsOut = proxy.m_contacts;
                dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
                for (dgInt32 i = 0; i < count; i ++) {
                    contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
                    contactsOut[i].m_normal = globalNormal;
                    contactsOut[i].m_shapeId0 = hullId;
                    contactsOut[i].m_shapeId1 = m_faceId;
                    contactsOut[i].m_penetration = penetration;
                }
            }
        } else {
            dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
            if (m_faceClipSize > convexSphapeUmbra) {
                BeamClipping (boxOrigin, convexSphapeUmbra);
                m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
            }

            dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
            count = convexShape->CalculateConvexCastContacts (proxy);

//			dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
            if (count >= 1) {
                dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
                if (m_closestFeatureType == 3) {
                    for (dgInt32 i = 0; i < count; i ++) {
                        contactsOut[i].m_shapeId0 = hullId;
                        contactsOut[i].m_shapeId1 = m_faceId;
                    }
                } else {
                    dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
                    if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
                        dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
                        dgVector n (&m_vertex[index * m_stride]);
                        dgVector dir0 (n * savedFaceNormal);
                        dgVector dir1 (n * normal);
                        dgFloat32 projection = dir0 % dir1;
                        if (projection <= dgFloat32 (0.0f)) {
                            normal = n;
                        }
                        normal = polygonInstance->m_globalMatrix.RotateVector(normal);
                        for (dgInt32 i = 0; i < count; i ++) {
                            contactsOut[i].m_normal = normal;
                            //contactsOut[i].m_userId = m_faceId;
                            contactsOut[i].m_shapeId0 = hullId;
                            contactsOut[i].m_shapeId1 = m_faceId;
                        }
                    } else {
                        for (dgInt32 i = 0; i < count; i ++) {
                            //contactsOut[i].m_userId = m_faceId;
                            contactsOut[i].m_shapeId0 = hullId;
                            contactsOut[i].m_shapeId1 = m_faceId;
                        }
                    }
                }
#endif

                for (dgInt32 i = 0; i < count; i ++) {
                    contactsOut[i].m_shapeId0 = hullId;
                    contactsOut[i].m_shapeId1 = m_faceId;
                }
            }
        }
    }

    proxy.m_matrix = savedProxyMatrix;
    return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
	dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
	dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));

	dgAssert(this == proxy.m_instance1->GetChildShape());
	dgAssert(m_count);
	dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

	const dgBody* const body0 = proxy.m_body0;
	const dgBody* const body1 = proxy.m_body1;

	dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());

	dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
	if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
		return 0;
	}
	dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
	if (den > dgFloat32 (1.0e-5f)) {
		// this can actually happens
		dgAssert(0);
		return 0;
	}

	dgContact* const contactJoint = proxy.m_contactJoint;
	contactJoint->m_closestDistance = dgFloat32(1.0e10f);

	dgMatrix polygonMatrix;
	dgVector right (m_localPoly[1] - m_localPoly[0]);
	polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
	polygonMatrix[1] = m_normal;
	polygonMatrix[2] = polygonMatrix[0] * m_normal;
	polygonMatrix[3] = dgVector::m_wOne;
	dgAssert (polygonMatrix.TestOrthogonal());

	dgVector polyBoxP0(dgFloat32(1.0e15f));
	dgVector polyBoxP1(dgFloat32(-1.0e15f));
	for (dgInt32 i = 0; i < m_count; i++) {
		dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
		polyBoxP0 = polyBoxP0.GetMin(point);
		polyBoxP1 = polyBoxP1.GetMax(point);
	}

	dgVector hullBoxP0;
	dgVector hullBoxP1;
	dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
	proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
	dgVector minBox(polyBoxP0 - hullBoxP1);
	dgVector maxBox(polyBoxP1 - hullBoxP0);
	dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
	dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
 	dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);

	dgInt32 count = 0;
	if (distance < dgFloat32(1.0f)) {
		bool inside = false;

		dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
		dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
		boxSize = sphereMag2.Sqrt();

		dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
		dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;

		dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
		dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;

		dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
		dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
		dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half)); 
		dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half))); 
		sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
		boxSize = sphereMag2.Sqrt();

		dgAssert (boxOrigin.m_w == 0.0f);
		boxOrigin = boxOrigin | dgVector::m_wOne;
		
		if (!proxy.m_intersectionTestOnly) {
			inside = true;
			dgInt32 i0 = m_count - 1;

			for (dgInt32 i = 0; i < m_count; i++) {
				dgVector e(m_localPoly[i] - m_localPoly[i0]);
				dgVector n(m_normal * e & dgVector::m_triplexMask);
				dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
				dgPlane plane(n, -(m_localPoly[i0] % n));

				dgVector p0 (boxOrigin + n.Scale4 (param));
				dgVector p1 (boxOrigin - n.Scale4 (param));

				dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
				dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();

				if ((size0 < 0.0f) && (size1 < 0.0f)) {
					return 0;
				}

				if ((size0 * size1) < 0.0f) {
					inside = false;
					break;
				}
				i0 = i;
			}
		}

		dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
		if (m_faceClipSize > convexSphapeUmbra) {
			BeamClipping(boxOrigin, convexSphapeUmbra);
			m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
		}

		const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
		if (inside & !proxy.m_intersectionTestOnly) {
			const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
			dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
			dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
			dgVector p0 (matrixInstance0.TransformVector(pointInHull));

			dgFloat32 timetoImpact = dgFloat32(0.0f);
			dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
			if (penetration < dgFloat32(0.0f)) {
				timetoImpact = penetration / (relativeVelocity % m_normal);
				dgAssert(timetoImpact >= dgFloat32(0.0f));
			}

			if (timetoImpact <= proxy.m_timestep) {
				dgVector contactPoints[64];
				contactJoint->m_closestDistance = penetration;
				proxy.m_timestep = timetoImpact;
				proxy.m_normal = m_normal;
				proxy.m_closestPointBody0 = p0;
				proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);

				if (!proxy.m_intersectionTestOnly) {
					pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
					count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);

					dgVector step(relativeVelocity.Scale4(timetoImpact));
					penetration = dgMax(penetration, dgFloat32(0.0f));
					dgContactPoint* const contactsOut = proxy.m_contacts;
					for (dgInt32 i = 0; i < count; i++) {
						contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
						contactsOut[i].m_normal = m_normal;
						contactsOut[i].m_shapeId0 = hullId;
						contactsOut[i].m_shapeId1 = m_faceId;
						contactsOut[i].m_penetration = penetration;
					}
				}
			}
		} else {
			m_vertexCount = dgUnsigned16 (m_count);
			count = world->CalculateConvexToConvexContacts(proxy);
			if (count >= 1) {
				dgContactPoint* const contactsOut = proxy.m_contacts;
				for (dgInt32 i = 0; i < count; i++) {
					contactsOut[i].m_shapeId0 = hullId;
					contactsOut[i].m_shapeId1 = m_faceId;
				}
			}
		}
	}

	return count;
}
void dgCollisionDeformableSolidMesh::ResolvePositionsConstraints (dgFloat32 timestep)
{
	dgAssert (m_myBody);

	dgInt32 strideInBytes = sizeof (dgVector) * m_clustersCount + sizeof (dgMatrix) * m_clustersCount + sizeof (dgMatrix) * m_particles.m_count;
	m_world->m_solverMatrixMemory.ExpandCapacityIfNeessesary (1, strideInBytes);
	dgVector* const regionCom = (dgVector*)&m_world->m_solverMatrixMemory[0];
	dgMatrix* const sumQiPi = (dgMatrix*) &regionCom[m_clustersCount];
	dgMatrix* const covarianceMatrix = (dgMatrix*) &sumQiPi[m_clustersCount];

	const dgFloat32* const masses = m_particles.m_unitMass;
	dgVector zero (dgFloat32 (0.0f));
	for (dgInt32 i = 0; i < m_clustersCount; i ++) {
		regionCom[i] = zero;
	}

	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		dgVector mass (masses[i]);
		const dgVector& r = m_posit[i];
		const dgVector& r0 = m_shapePosit[i];
		dgVector mr (r.Scale4(masses[i]));
		covarianceMatrix[i] = dgMatrix (r0, mr);
	
		const dgInt32 start = m_clusterPositStart[i];
		const dgInt32 count = m_clusterPositStart[i + 1] - start;
		for (dgInt32 j = 0; j < count; j ++) {
			dgInt32 index = m_clusterPosit[start + j];
			regionCom[index] += mr;
		}
	}

	for (dgInt32 i = 0; i < m_clustersCount; i ++) {
		dgVector mcr (regionCom[i]);
		regionCom[i] = mcr.Scale4 (dgFloat32 (1.0f) / m_clusterMass[i]);
		const dgVector& cr0 = m_clusterCom0[i];
		sumQiPi[i] = dgMatrix (cr0, mcr.CompProduct4(dgVector::m_negOne));
	}

	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		const dgInt32 start = m_clusterPositStart[i];
		const dgInt32 count = m_clusterPositStart[i + 1] - start;
		const dgMatrix& covariance = covarianceMatrix[i];
		for (dgInt32 j = 0; j < count; j ++) {
			dgInt32 index = m_clusterPosit[start + j];
			dgMatrix& QiPi = sumQiPi[index];
			QiPi.m_front += covariance.m_front;
			QiPi.m_up += covariance.m_up;
			QiPi.m_right += covariance.m_right;
		}
	}

dgVector beta0 (dgFloat32 (0.93f));
//dgVector beta0 (dgFloat32 (0.0f));
dgVector beta1 (dgVector::m_one - beta0);
	dgFloat32 stiffness = dgFloat32 (0.3f);

	for (dgInt32 i = 0; i < m_clustersCount; i ++) {
		dgMatrix& QiPi = sumQiPi[i];

		dgMatrix S (QiPi * QiPi.Transpose4X4());
		dgVector eigenValues;

		S.EigenVectors (eigenValues, m_clusterRotationInitialGuess[i]);
		m_clusterRotationInitialGuess[i] = S;

#ifdef _DEBUG_____ 
		dgMatrix P0 (QiPi * QiPi.Transpose4X4());
		dgMatrix D (dgGetIdentityMatrix());
		D[0][0] = eigenValues[0];
		D[1][1] = eigenValues[1];
		D[2][2] = eigenValues[2];
		dgMatrix P1 (S.Transpose4X4() * D * S);
		dgAssert (P0.TestSymetric3x3());
		dgAssert (P1.TestSymetric3x3());

		dgMatrix xx (P1 * P0.Symetric3by3Inverse());
		dgAssert (dgAbsf (xx[0][0] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[1][1] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[2][2] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[0][1]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[0][2]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[1][0]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[1][2]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[2][0]) < dgFloat32 (1.0e-3f));
		dgAssert (dgAbsf (xx[2][1]) < dgFloat32 (1.0e-3f));
#endif

		eigenValues = eigenValues.InvSqrt();

		dgMatrix m;
		m.m_front = S.m_front.CompProduct4(eigenValues.BroadcastX());
		m.m_up    = S.m_up.CompProduct4(eigenValues.BroadcastY());
		m.m_right = S.m_right.CompProduct4(eigenValues.BroadcastZ());
		m.m_posit = dgVector::m_wOne;
		dgMatrix invS (S.Transpose4X4() * m);
		dgMatrix R (invS * QiPi);
		dgMatrix A (m_clusterAqqInv[i] * QiPi);

		QiPi.m_front = A.m_front.CompProduct4(beta0) + R.m_front.CompProduct4(beta1);
		QiPi.m_up = A.m_up.CompProduct4(beta0) + R.m_up.CompProduct4(beta1);
		QiPi.m_right = A.m_right.CompProduct4(beta0) + R.m_right.CompProduct4(beta1);
	}

	dgVector invTimeScale (stiffness / timestep);
	dgVector* const veloc = m_particles.m_veloc;

	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		const dgInt32 start = m_clusterPositStart[i];
		const dgInt32 count = m_clusterPositStart[i + 1] - start;

		dgVector v (dgFloat32 (0.0f));
		const dgVector& p = m_posit[i];
		const dgVector& p0 = m_shapePosit[i];
		for (dgInt32 j = 0; j < count; j ++) {
			dgInt32 index = m_clusterPosit[start + j];
			const dgMatrix& matrix = sumQiPi[index];
			dgVector gi (matrix.RotateVector(p0 - m_clusterCom0[index]) + regionCom[index]);
			v += ((gi - p).CompProduct4(invTimeScale).Scale4 (m_clusterWeight[index]));
		}
		veloc[i] += v;
	}


// resolve collisions here
//for now just a hack a collision plane until I get the engine up an running
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
	dgVector p (m_basePosit + m_posit[i].m_y);
	if (p.m_y < dgFloat32 (0.0f)) {
		m_posit[i].m_y = -m_basePosit.m_y;
		veloc[i].m_y = dgFloat32 (0.0f);
	}
}

	dgVector time (timestep);
	dgVector minBase(dgFloat32 (1.0e10f));
    dgVector minBox (dgFloat32 (1.0e10f));
    dgVector maxBox (dgFloat32 (-1.0e10f));
    dgMatrix matrix (m_myBody->GetCollision()->GetGlobalMatrix().Inverse());
	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		m_posit[i] += veloc[i].CompProduct4 (time);
        m_particles.m_posit[i] = matrix.TransformVector(m_posit[i] + m_basePosit);
		minBase = minBase.GetMin (m_posit[i]);
        minBox = minBox.GetMin(m_particles.m_posit[i]);
        maxBox = maxBox.GetMax(m_particles.m_posit[i]);
	}

	minBase = minBase.Floor();
	dgVector mask ((minBase < dgVector (dgFloat32 (0.0f))) | (minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))));
	dgInt32 test = mask.GetSignMask();
	if (test & 0x07) {
		dgVector offset (((minBase < dgVector (dgFloat32 (0.0f))) & dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE/2))) + 
			             ((minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))) & dgVector (dgFloat32 (-DG_SOFTBODY_BASE_SIZE/2))));
		m_basePosit -= offset;
		for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
			m_posit[i] += offset;
		}
	}

	// integrate each particle by the deformation velocity, also calculate the new com
	// calculate the new body average velocity
//	if (m_myBody->m_matrixUpdate) {
//		myBody->m_matrixUpdate (*myBody, myBody->m_matrix, threadIndex);
//	}
	// the collision changed shape, need to update spatial structure 
//	UpdateCollision ();
//	SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
    SetCollisionBBox (minBox, maxBox);
}
Ejemplo n.º 16
0
void DemoMesh::SpliteSegment(dListNode* const node, int maxIndexCount)
{
	const DemoSubMesh& segment = node->GetInfo(); 
	if (segment.m_indexCount > maxIndexCount) {
		dVector minBox (1.0e10f, 1.0e10f, 1.0e10f, 0.0f);
		dVector maxBox (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
		for (int i = 0; i < segment.m_indexCount; i ++) {
			int index = segment.m_indexes[i];
			for (int j = 0; j < 3; j ++) {
				minBox[j] = (m_vertex[index * 3 + j] < minBox[j]) ? m_vertex[index * 3 + j] : minBox[j];
				maxBox[j] = (m_vertex[index * 3 + j] > maxBox[j]) ? m_vertex[index * 3 + j] : maxBox[j];
			}
		}

		int index = 0;
		dFloat maxExtend = -1.0e10f;
		for (int j = 0; j < 3; j ++) {
			dFloat ext = maxBox[j] - minBox[j];
			if (ext > maxExtend ) {
				index = j;
				maxExtend = ext;
			}
		}

		int leftCount = 0;
		int rightCount = 0;
		dFloat spliteDist = (maxBox[index ] + minBox[index]) * 0.5f;
		for (int i = 0; i < segment.m_indexCount; i += 3) {
			bool isleft = true;
			for (int j = 0; j < 3; j ++) {
				int vertexIndex = segment.m_indexes[i + j];
				isleft &= (m_vertex[vertexIndex * 3 + index] < spliteDist);
			}
			if (isleft) {
				leftCount += 3;
			} else {
				rightCount += 3;
			}
		}
		dAssert (leftCount);
		dAssert (rightCount);

		dListNode* const leftNode = Append();
		dListNode* const rightNode = Append();
		DemoSubMesh* const leftSubMesh = &leftNode->GetInfo();
		DemoSubMesh* const rightSubMesh = &rightNode->GetInfo();
		leftSubMesh->AllocIndexData (leftCount);
		rightSubMesh->AllocIndexData (rightCount);

		leftSubMesh->m_textureHandle = AddTextureRef (segment.m_textureHandle);
		rightSubMesh->m_textureHandle = AddTextureRef (segment.m_textureHandle);
		leftSubMesh->m_textureName = segment.m_textureName;
		rightSubMesh->m_textureName = segment.m_textureName;

		leftCount = 0;
		rightCount = 0;
		for (int i = 0; i < segment.m_indexCount; i += 3) {
			bool isleft = true;
			for (int j = 0; j < 3; j ++) {
				int vertexIndex = segment.m_indexes[i + j];
				isleft &= (m_vertex[vertexIndex * 3 + index] < spliteDist);
			}
			if (isleft) {
				leftSubMesh->m_indexes[leftCount + 0] = segment.m_indexes[i + 0];
				leftSubMesh->m_indexes[leftCount + 1] = segment.m_indexes[i + 1];
				leftSubMesh->m_indexes[leftCount + 2] = segment.m_indexes[i + 2];
				leftCount += 3;
			} else {
				rightSubMesh->m_indexes[rightCount + 0] = segment.m_indexes[i + 0];
				rightSubMesh->m_indexes[rightCount + 1] = segment.m_indexes[i + 1];
				rightSubMesh->m_indexes[rightCount + 2] = segment.m_indexes[i + 2];
				rightCount += 3;
			}
		}

		SpliteSegment(leftNode, maxIndexCount);
		SpliteSegment(rightNode, maxIndexCount);
		Remove(node);
	}
}