Exemple #1
0
    vecN<vecN<vecN<OutputType, 2>, 4>, 2>
    split_cubicT(c_array<const vecN<InputType, 2> > pts)
    {
      FASTUIDRAWassert(pts.size() == 4);

      vecN<vecN<vecN<OutputType, 2>, 4>, 2> return_value;
      vecN<IntermediateType, 2> p0(pts[0]), p1(pts[1]), p2(pts[2]), p3(pts[3]);
      vecN<IntermediateType, 2> p01, p23, pA, pB, pC;
      const IntermediateType two(2), three(3), four(4), eight(8);

      p01 = (p0 + p1) / two;
      p23 = (p2 + p3) / two;
      pA = (p0 + two * p1 + p2) / four;
      pB = (p1 + two * p2 + p3) / four;
      pC = (p0 + three * p1 + three * p2 + p3) / eight;

      vecN<OutputType, 2> q0(pts[0]), q01(p01), qA(pA), qC(pC);
      vecN<OutputType, 2> qB(pB), q23(p23), q3(pts[3]);

      return_value[0] = vecN<vecN<OutputType, 2>, 4>(q0, q01, qA, qC);
      return_value[1] = vecN<vecN<OutputType, 2>, 4>(qC, qB, q23, q3);

      return return_value;
    }
Exemple #2
0
double C2M::getHit(Vec3 p3, Vec3 n3){

	Vec3 p(p3);
	Vec3 n((n3 - Eye::get()->N*(n3*Eye::get()->N) ).normalize());
	
	double tmin = 999999999999;
	bool hit = false;
	Vec3 hitp;
	for(list<ArrCurve*>::iterator it =_curves.begin(); it!=_curves.end(); it++){

		ArrCurve*  ac = (*it);
		Vec3 p0(ac->getP(0));

		for(int i=1; i<ac->size();i++){
	
			Vec3 p1(ac->getP(i));
			Vec3 n1 = (p1 - p0);

			//double d = (n1%n).normalize()*((p0-p)%n1).normalize();
			//cout<<"dot:="<<d<<"........................"<<endl;

			double len1 = n1.norm();
			double t = getIntersectionDist(p, n, p1, n1.normalize() );

			if (t>0 && t<tmin && ( p + t*n - p1 ).norm()<len1 ){
				tmin = t;
				hit = true;
			}
			p0.set(p1);
		}
	}

	if (hit) 
		return tmin;
	return -1;
}
Exemple #3
0
void MapgenV6::addDirtGravelBlobs() {
	if (getBiome(v2s16(node_min.X, node_min.Z)) != BT_NORMAL)
		return;
	
	PseudoRandom pr(blockseed + 983);
	for (int i = 0; i < volume_nodes/10/10/10; i++) {
		bool only_fill_cave = (myrand_range(0,1) != 0);
		v3s16 size(
			pr.range(1, 8),
			pr.range(1, 8),
			pr.range(1, 8)
		);
		v3s16 p0(
			pr.range(node_min.X, node_max.X) - size.X / 2,
			pr.range(node_min.Y, node_max.Y) - size.Y / 2,
			pr.range(node_min.Z, node_max.Z) - size.Z / 2
		);
		
		MapNode n1((p0.Y > -32 && !pr.range(0, 1)) ? c_dirt : c_gravel);
		for (int z1 = 0; z1 < size.Z; z1++)
		for (int y1 = 0; y1 < size.Y; y1++)
		for (int x1 = 0; x1 < size.X; x1++) {
			v3s16 p = p0 + v3s16(x1, y1, z1);
			u32 i = vm->m_area.index(p);
			if (!vm->m_area.contains(i))
				continue;
			// Cancel if not stone and not cave air
			if (vm->m_data[i].getContent() != c_stone &&
				!(vm->m_flags[i] & VMANIP_FLAG_CAVE))
				continue;
			if (only_fill_cave && !(vm->m_flags[i] & VMANIP_FLAG_CAVE))
				continue;
			vm->m_data[i] = n1;
		}
	}
}
dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
	dgAssert (m_p0.m_w == dgFloat32 (0.0f));
	dgAssert (m_p1.m_w == dgFloat32 (0.0f));

	if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) {
		dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32));
		dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask);
		dgBigVector p0(m_p0);
		dgBigVector p0v0(v0 - p0);

		dgBigVector diff(m_diff);
		dgBigVector normal(faceNormal);
		dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar();
		if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) {
			dgBigVector p (p0 + diff.Scale (tOut));
			dgBigVector unitDir(m_unitDir);
			for (dgInt32 i = 0; i < indexCount; i++) {
				dgInt32 i2 = indexArray[i] * stride;
				dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask);

				dgBigVector edge0(p - v0);
				dgBigVector edge1(v1 - v0);
				dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar();
				if (area < dgFloat32 (0.0f)) {
					return 1.2f;
				}
				v0 = v1;
			}

			return dgFloat32(tOut);
		}
	}

	return dgFloat32 (1.2f);
}
Exemple #5
0
void object::test<1>
()
{

    LineIntersector li;
    Coordinate pt(10, 10);
    HotPixel hp(pt, 1, li);

    ensure_equals(hp.getCoordinate(), pt);

    const Envelope& env = hp.getSafeEnvelope();
    ensure_equals(env.toString(), "Env[9.25:10.75,9.25:10.75]");

    Coordinate p0(0, 10);
    Coordinate p1(20, 10);
    ensure("hp.intersects 0 10, 20 10", hp.intersects(p0, p1));

    p1.y = 11; // intersection point within 0.75 distance
    ensure("hp.intersects(0 10, 20 11)", hp.intersects(p0, p1));

    p1.y = 20;
    ensure_not("!hp.intersects(0 10, 20 20)", hp.intersects(p0, p1));

}
static void RenderBodyContactsAndTangentDiretions (NewtonBody* const body, float length)
{
	for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
		if (NewtonJointIsActive (joint)) {
			for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
				dVector point;
				dVector normal;	
				NewtonMaterial* const material = NewtonContactGetMaterial (contact);
				NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);

				dVector tangnetDir0;
				dVector tangnetDir1;
				NewtonMaterialGetContactTangentDirections(material, body, &tangnetDir0[0], &tangnetDir1[0]);

				// if we are display debug info we need to block other threads from writing the data at the same time
				dVector p1 (point + normal.Scale (length));
				//dVector p0 (point - normal.Scale (length));
				dVector p0 (point);
				glVertex3f (p0.m_x, p0.m_y, p0.m_z);
				glVertex3f (p1.m_x, p1.m_y, p1.m_z);
			}
		}
	}
}
GUIFormSpecMenu::ItemSpec GUIFormSpecMenu::getItemAtPos(v2s32 p) const
{
	core::rect<s32> imgrect(0,0,imgsize.X,imgsize.Y);
	
	for(u32 i=0; i<m_inventorylists.size(); i++)
	{
		const ListDrawSpec &s = m_inventorylists[i];

		for(s32 i=0; i<s.geom.X*s.geom.Y; i++)
		{
			s32 item_i = i + s.start_item_i;
			s32 x = (i%s.geom.X) * spacing.X;
			s32 y = (i/s.geom.X) * spacing.Y;
			v2s32 p0(x,y);
			core::rect<s32> rect = imgrect + s.pos + p0;
			if(rect.isPointInside(p))
			{
				return ItemSpec(s.inventoryloc, s.listname, item_i);
			}
		}
	}

	return ItemSpec(InventoryLocation(), "", -1);
}
Exemple #8
0
void setup(MeshType & mesh, viennagrid::quadrilateral_tag)
{
  typedef typename viennagrid::result_of::point<MeshType>::type               PointType;
  typedef typename viennagrid::result_of::vertex_handle<MeshType>::type       VertexHandleType;

  PointType p0(0.0, 0.0);
  PointType p1(1.0, 0.0);
  PointType p2(2.0, 0.0);
  PointType p3(2.0, 1.0);
  PointType p4(1.0, 1.0);
  PointType p5(0.0, 1.0);

  std::cout << "Adding vertices to segment:" << std::endl;
  VertexHandleType vh0 = viennagrid::make_vertex( mesh, p0 );
  VertexHandleType vh1 = viennagrid::make_vertex( mesh, p1 );
  VertexHandleType vh2 = viennagrid::make_vertex( mesh, p2 );
  VertexHandleType vh3 = viennagrid::make_vertex( mesh, p3 );
  VertexHandleType vh4 = viennagrid::make_vertex( mesh, p4 );
  VertexHandleType vh5 = viennagrid::make_vertex( mesh, p5 );

  viennagrid::make_quadrilateral( mesh, vh0, vh1, vh5, vh4 );
  viennagrid::make_quadrilateral( mesh, vh1, vh2, vh4, vh3 );

}
void RemoveMultiple ( const std::list<ID>& listToTreat, std::list< std::pair<ID, ID> >& finalList )
{
    ID counter = 0;
    std::list<ID> temporaryList ( listToTreat );

    //! Sort the list
    temporaryList.sort();

    //! initialize the new list
    std::pair <ID, ID> p0 ( temporaryList.front() , counter );
    finalList.push_back ( p0 );

    //! We remove the multiple occurences :
    for ( std::list<ID>::iterator it = temporaryList.begin() ; it != temporaryList.end() ; ++ it )
    {
        if ( ( *it ) != finalList.back().first )
        {
            counter ++ ;
            //! Add to the list the new value
            std::pair <ID, ID> p ( ( *it ) , counter );
            finalList.push_back ( p );
        }
    }
}
Exemple #10
0
int main(int argc,char **argv)
{
    if(argc!=2){
	fprintf(stderr,"usage: %s scanner.so\n",argv[0]);
        fprintf(stderr,"type 'make plugins' to make available plugins\n");
	exit(1);
    }

    /* Strip extension and path */
    std::string fname = argv[1];
    scanner_t *fn=0;
    std::string name = fname;
    size_t dot = name.rfind('.');
    if(dot==std::string::npos){
	fprintf(stderr,"%s: cannot strip extension\n",name.c_str());
	exit(1);
    }
    name = name.substr(0,dot);         

    /* Strip dir */
    size_t slash = name.rfind('.');
    if(slash!=std::string::npos){
        name = name.substr(slash+1);
    }

#ifdef HAVE_DLOPEN
    if(fname.find('.')==std::string::npos){
        fname = "./" + fname;               // fedora requires a complete path name
    }

#ifdef HAVE_DLOPEN_PREFLIGHT
    if(!dlopen_preflight(fname.c_str())){
	fprintf(stderr,"dlopen_preflight - cannot open %s: %s",fname.c_str(),dlerror());
        exit(1);
    }
#endif

    void *lib=dlopen(fname.c_str(), RTLD_LAZY);
    if(lib==0){
        fprintf(stderr,"fname=%s\n",fname.c_str());
        fprintf(stderr,"dlopen: %s\n",dlerror());
        exit(1);
    }

    fn=(scanner_t *)dlsym(lib, name.c_str());
    if(fn==0){
        fprintf(stderr,"dlsym: %s\n",dlerror());
        exit(1);
    }

#endif
#ifdef HAVE_LOADLIBRARY
    /* Use Win32 LoadLibrary function */
    /* See http://msdn.microsoft.com/en-us/library/ms686944(v=vs.85).aspx */
    HINSTANCE hinstLib = LoadLibrary(TEXT(fname.c_str()));
    if(hinstLib==0){
        fprintf(stderr,"LoadLibrary(%s) failed",fname.c_str());
        exit(1);
    }
    MYPROC fn = (MYPROC)GetProcAddress(hinstLib,name.c_str());
    if(fn==0){
        fprintf(stderr,"GetProcAddress(%s) failed",name.c_str());
        exit(1);
    }
#endif

    feature_recorder_set fs(0,my_hasher,feature_recorder_set::NO_INPUT,feature_recorder_set::NO_OUTDIR);
    uint8_t buf[100];
    pos0_t p0("");
    sbuf_t sbuf(p0,buf,sizeof(buf),sizeof(buf),false);
    scanner_params sp(scanner_params::PHASE_STARTUP,sbuf,fs);
    recursion_control_block rcb(0,"STAND");
    scanner_info si;
    sp.info = &si;
    (*fn)(sp,rcb);
    std::cout << "Loaded scanner '" << si.name << "' by " << si.author << "\n";
#ifdef HAVE_DLOPEN
    dlclose(lib);
#endif
    return 0;
}
Exemple #11
0
void Camera::DrawSpike(double rad, const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	glPushMatrix();

	float znear, zfar;
	Render::GetNearFarClipPlane(znear, zfar);
	double newdist = znear + 0.5f * (zfar - znear);
	double scale = newdist / viewCoords.Length();

	glTranslatef(float(scale*viewCoords.x), float(scale*viewCoords.y), float(scale*viewCoords.z));

	Render::State::UseProgram(0);
	// face the camera dammit
	vector3d zaxis = viewCoords.Normalized();
	vector3d xaxis = vector3d(0,1,0).Cross(zaxis).Normalized();
	vector3d yaxis = zaxis.Cross(xaxis);
	matrix4x4d rot = matrix4x4d::MakeInvRotMatrix(xaxis, yaxis, zaxis);
	glMultMatrixd(&rot[0]);

	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
	glEnable(GL_BLEND);

	// XXX WRONG. need to pick light from appropriate turd.
	GLfloat col[4];
	glGetLightfv(GL_LIGHT0, GL_DIFFUSE, col);
	glColor4f(col[0], col[1], col[2], 1);
	glBegin(GL_TRIANGLE_FAN);
	glVertex3f(0,0,0);
	glColor4f(col[0], col[1], col[2], 0);

	const float spikerad = float(scale*rad);

	// bezier with (0,0,0) control points
	{
		vector3f p0(0,spikerad,0), p1(spikerad,0,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	{
		vector3f p0(spikerad,0,0), p1(0,-spikerad,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	{
		vector3f p0(0,-spikerad,0), p1(-spikerad,0,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	{
		vector3f p0(-spikerad,0,0), p1(0,spikerad,0);
		float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) {
			vector3f p = (1-t)*(1-t)*p0 + t*t*p1;
			glVertex3fv(&p[0]);
		}
	}
	glEnd();
	glDisable(GL_BLEND);
	glEnable(GL_LIGHTING);
	glEnable(GL_DEPTH_TEST);
	glPopMatrix();
}
dgUnsigned32 dgSlidingConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;

	//dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
	CalculateGlobalMatrixAndAngle (matrix0, matrix1);
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
	matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);

	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

	dgVector p0 (matrix0.m_posit);
	dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));

	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

	dgVector r0 (p0 + matrix0.m_up.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector r1 (p1 + matrix1.m_up.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	dgPointParam pointDataR;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);
	InitPointParam (pointDataR, m_stiffness, r0, r1);

	CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); 
	CalculatePointDerivative (4, params, dir2, pointDataR, &m_jointForce[4]); 

	dgInt32 ret = 5;
	if (m_jointAccelFnt) {
		dgJointCallbackParam axisParam;
		axisParam.m_accel = dgFloat32 (0.0f);
		axisParam.m_timestep = params.m_timestep;
		axisParam.m_minFriction = DG_MIN_BOUND;
		axisParam.m_maxFriction = DG_MAX_BOUND;

		if (m_jointAccelFnt (*this, &axisParam)) {
			if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[5].m_low = axisParam.m_minFriction;
				params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
				params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculatePointDerivative (5, params, matrix0.m_front, pointDataP, &m_jointForce[5]); 
			//params.m_jointAccel[5] = axisParam.m_accel;
			SetMotorAcceleration (5, axisParam.m_accel, params);
			ret = 6;
		}
	}

	return dgUnsigned32 (ret);
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(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 polygonInstance = proxy.m_floatingCollision;
    dgAssert(this == polygonInstance->GetChildShape());
    dgAssert(m_count);
    dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    dgInt32 count = 0;


    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());
    dgVector savedFaceNormal(m_normal);

    dgVector savedPosit (proxy.m_matrix.m_posit);
    proxy.m_matrix.m_posit = dgVector::m_wOne;

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

    dgContact* const contactJoint = proxy.m_contactJoint;
    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
    dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
    dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
    dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));

    dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
    if (penetration < dgFloat32(0.0f)) {
        contactJoint->m_closestDistance = -penetration;
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    contactJoint->m_closestDistance = dgFloat32(0.0f);
    dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
    if (distance >= dgFloat32(0.0f)) {
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
    dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

    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));
        dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
        plane = proxy.m_matrix.TransformPlane(plane);

        //dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
        //dgFloat32 centerDist = plane.Evalue(boxOrigin);
        dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
        dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();

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

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

    const dgInt32 hullId = hull->GetUserDataID();
    if (inside & !proxy.m_intersectionTestOnly) {
        dgAssert(penetration >= dgFloat32(0.0f));
        dgVector pointsContacts[64];

        dgAssert(penetration >= 0.0f);
        dgVector point(pointInHull + normalInHull.Scale4(penetration));

        count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
        dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

        const dgMatrix& worldMatrix = hull->m_globalMatrix;
        dgContactPoint* const contactsOut = proxy.m_contacts;
        dgAssert(contactsOut);
        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(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
            m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
        }

        dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape;
        count = convexShape->CalculateConvexToConvexContact(proxy);
        dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
        if (count >= 1) {
            dgContactPoint* const contactsOut = proxy.m_contacts;
            if (m_closestFeatureType == 3) {
                for (dgInt32 i = 0; i < count; i++) {
                    //contactsOut[i].m_userId = m_faceId;
                    contactsOut[i].m_shapeId0 = hullId;
                    contactsOut[i].m_shapeId1 = m_faceId;
                }
            } else {
                dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
                if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) {
                    dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
                    dgVector n(&m_vertex[index * m_stride]);
                    if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) {
                        normal = n;
                    } else {
                        dgVector dir0(n * savedFaceNormal);
                        dgVector dir1(n * normal);
                        dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
                        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;
                    }
                }
            }
        }
    }

    proxy.m_matrix.m_posit = savedPosit;
    return count;
}
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
    dgVector normal(normalIn);
    dgInt32 count = 0;
    dgFloat32 maxDist = dgFloat32 (1.0f);
    dgFloat32 projectFactor = m_normal % normal;
    if (projectFactor < dgFloat32 (0.0f)) {
        projectFactor *= dgFloat32 (-1.0f);
        normal = normal.Scale3 (dgFloat32 (-1.0f));
    }

    if (projectFactor > dgFloat32 (0.9999f)) {
        for (dgInt32 i = 0; i < m_count; i ++) {
            contactsOut[count] = m_localPoly[i];
            count ++;
        }

#ifdef _DEBUG
        dgInt32 j = count - 1;
        for (dgInt32 i = 0; i < count; i ++) {
            dgVector error (contactsOut[i] - contactsOut[j]);
            dgAssert ((error % error) > dgFloat32 (1.0e-20f));
            j = i;
        }
#endif

    } else if (projectFactor > dgFloat32 (0.1736f)) {
        maxDist = dgFloat32 (0.0f);
        dgPlane plane (normal, - (normal % origin));

        dgVector p0 (m_localPoly[m_count - 1]);
        dgFloat32 side0 = plane.Evalue (p0);
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector p1 (m_localPoly[i]);
            dgFloat32 side1 = plane.Evalue (p1);

            if (side0 > dgFloat32 (0.0f)) {
                maxDist = dgMax (maxDist, side0);
                contactsOut[count] = p0 - plane.Scale3 (side0);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }

                if (side1 <= dgFloat32 (0.0f)) {
                    dgVector dp (p1 - p0);
                    dgFloat32 t = plane % dp;
                    dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                    if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                        t = dgSign(t) * dgFloat32 (1.0e-8f);
                    }
                    contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                    count ++;
                    if (count > 1) {
                        dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                        dgFloat32 error = edgeSegment % edgeSegment;
                        if (error < dgFloat32 (1.0e-8f)) {
                            count --;
                        }
                    }
                }
            } else if (side1 > dgFloat32 (0.0f)) {
                dgVector dp (p1 - p0);
                dgFloat32 t = plane % dp;
                dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                    t = dgSign(t) * dgFloat32 (1.0e-8f);
                }
                contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }
            }

            side0 = side1;
            p0 = p1;
        }
    } else {
        maxDist = dgFloat32 (1.0e10f);
        dgPlane plane (normal, - (normal % origin));

        dgVector p0 (m_localPoly[m_count - 1]);
        dgFloat32 side0 = plane.Evalue (p0);
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector p1 (m_localPoly[i]);
            dgFloat32 side1 = plane.Evalue (p1);

            if ((side0 * side1) < dgFloat32 (0.0f)) {
                dgVector dp (p1 - p0);
                dgFloat32 t = plane % dp;
                dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
                if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
                    t = dgSign(t) * dgFloat32 (1.0e-8f);
                }
                contactsOut[count] = p0 - dp.Scale3 (side0 / t);
                count ++;
                if (count > 1) {
                    dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
                    dgFloat32 error = edgeSegment % edgeSegment;
                    if (error < dgFloat32 (1.0e-8f)) {
                        count --;
                    }
                }
            }
            side0 = side1;
            p0 = p1;
        }
    }


    if (count > 1) {
        if (maxDist < dgFloat32 (1.0e-3f)) {
            dgVector maxPoint (contactsOut[0]);
            dgVector minPoint (contactsOut[0]);
            dgVector lineDir (m_normal * normal);

            dgFloat32 proj = contactsOut[0] % lineDir;
            dgFloat32 maxProjection = proj;
            dgFloat32 minProjection = proj;
            for (dgInt32 i = 1; i < count; i ++) {
                proj = contactsOut[i] % lineDir;
                if (proj > maxProjection) {
                    maxProjection = proj;
                    maxPoint = contactsOut[i];
                }
                if (proj < minProjection) {
                    minProjection = proj;
                    minPoint = contactsOut[i];
                }
            }

            contactsOut[0] = maxPoint;
            contactsOut[1] = minPoint;
            count = 2;
        }


        dgVector error (contactsOut[count - 1] - contactsOut[0]);
        if ((error % error) < dgFloat32 (1.0e-8f)) {
            count --;
        }
    }

#ifdef _DEBUG
    if (count > 1) {
        dgInt32 j = count - 1;
        for (dgInt32 i = 0; i < count; i ++) {
            dgVector error (contactsOut[i] - contactsOut[j]);
            dgAssert ((error % error) > dgFloat32 (1.0e-20f));
            j = i;
        }

        if (count >= 3) {
            dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
            dgVector e0 (contactsOut[1] - contactsOut[0]);
            for (dgInt32 i = 2; i < count; i ++) {
                dgVector e1 (contactsOut[i] - contactsOut[0]);
                n += e0 * e1;
                e0 = e1;
            }
            n = n.Scale3 (dgRsqrt(n % n));
            dgFloat32 val = n % normal;
            dgAssert (val > dgFloat32 (0.9f));
        }
    }
#endif
    return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
	dgInt32 count = 0;

	dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
	dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
	dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());

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

	const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
	dgContact* const contactJoint = proxy.m_contactJoint;
	const dgCollisionInstance* const hull = proxy.m_instance0;

	dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
	dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
	dgVector p0(hullMatrix.TransformVector(pointInHull));

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

	dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
	contactJoint->m_closestDistance = dgFloat32(0.0f);
	dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
	if (distance >= dgFloat32(0.0f)) {
		return 0;
	}

	dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
	dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

	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 edgeBoundaryNormal(m_normal * e);
		dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
		plane = hullMatrix.TransformPlane(plane);

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

		if ((centerDist + supportDist) < dgFloat32(0.0f)) {
			return 0;
		}

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

//inside = false;
	dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
	if (m_faceClipSize > convexSphapeUmbra) {
		BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
		m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
	}

	const dgInt32 hullId = hull->GetUserDataID();
	if (inside & !proxy.m_intersectionTestOnly) {
		dgAssert(penetration >= dgFloat32(0.0f));
		dgVector contactPoints[64];

		dgAssert(penetration >= 0.0f);
		dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));

		count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
		dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

		dgContactPoint* const contactsOut = proxy.m_contacts;
		dgAssert(contactsOut);
		for (dgInt32 i = 0; i < count; i++) {
			contactsOut[i].m_point = hullMatrix.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);
		dgAssert(proxy.m_intersectionTestOnly || (count >= 0));

		if (count >= 1) {
			dgContactPoint* const contactsOut = proxy.m_contacts;
			if (m_closestFeatureType == 3) {
				for (dgInt32 i = 0; i < count; i++) {
					//contactsOut[i].m_userId = m_faceId;
					contactsOut[i].m_shapeId0 = hullId;
					contactsOut[i].m_shapeId1 = m_faceId;
				}
			} else {
				dgVector normal (contactsOut[0].m_normal);

				if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) {
					dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
					dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride])));
					if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) {
						normal = adjacentNormal;
					} else {
						dgVector dir0(adjacentNormal * m_normal);
						dgVector dir1(adjacentNormal * normal);
						dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
						if (projection <= dgFloat32(0.0f)) {
							normal = adjacentNormal;
						}
					}
					normal = polygonInstance->m_globalMatrix.RotateVector(normal);

					for (dgInt32 i = 0; i < count; i++) {
						contactsOut[i].m_normal = normal;
						contactsOut[i].m_shapeId0 = hullId;
						contactsOut[i].m_shapeId1 = m_faceId;
					}
				} else {
					for (dgInt32 i = 0; i < count; i++) {
						contactsOut[i].m_shapeId0 = hullId;
						contactsOut[i].m_shapeId1 = m_faceId;
					}
				}
			}
		}
	}
	return count;
}
Exemple #16
0
void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator)
{
	m_rtti |= dgCollisionSphere_RTTI;
	m_radius = radius;

	m_edgeCount = DG_SPHERE_EDGE_COUNT;
	m_vertexCount = DG_SPHERE_VERTEX_COUNT;
	dgCollisionConvex::m_vertex = m_vertex;

	if (!m_shapeRefCount) {

		dgInt32 indexList[256];
		dgVector tmpVectex[256];

		dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
		dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
		dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
		dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
		dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));

		dgInt32 i = 1;
		dgInt32 count = 0;
		TesselateTriangle (i, p4, p0, p2, count, tmpVectex);
		TesselateTriangle (i, p4, p2, p1, count, tmpVectex);
		TesselateTriangle (i, p4, p1, p3, count, tmpVectex);
		TesselateTriangle (i, p4, p3, p0, count, tmpVectex);
		TesselateTriangle (i, p5, p2, p0, count, tmpVectex);
		TesselateTriangle (i, p5, p1, p2, count, tmpVectex);
		TesselateTriangle (i, p5, p3, p1, count, tmpVectex);
		TesselateTriangle (i, p5, p0, p3, count, tmpVectex);

		//dgAssert (count == EDGE_COUNT);
		dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f); 

		dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT);
		for (dgInt32 i = 0; i < vertexCount; i ++) {
			m_unitSphere[i] = tmpVectex[i];
		}
		dgPolyhedra polyhedra(m_allocator);

		polyhedra.BeginFace();
		for (dgInt32 i = 0; i < count; i += 3) {
#ifdef _DEBUG
			dgEdge* const edge = polyhedra.AddFace (indexList[i],  indexList[i + 1], indexList[i + 2]);
			dgAssert (edge);
#else 
			polyhedra.AddFace (indexList[i],  indexList[i + 1], indexList[i + 2]);
#endif
		}
		polyhedra.EndFace();

		dgUnsigned64 i1 = 0;
		dgPolyhedra::Iterator iter (polyhedra);
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);
			edge->m_userData = i1;
			i1 ++;
		}

		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);

			dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];

			ptr->m_vertex = edge->m_incidentVertex;
			ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
			ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
			ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
		}
	}

	for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) {
		m_vertex[i] = m_unitSphere[i].Scale4 (m_radius);
	}

	m_shapeRefCount ++;
	dgCollisionConvex::m_simplex = m_edgeArray;
	SetVolumeAndCG ();
}
Exemple #17
0
PyObject *
_PyImport_LoadDynamicModuleWithSpec(PyObject *spec, FILE *fp)
{
#ifndef MS_WINDOWS
    PyObject *pathbytes = NULL;
#endif
    PyObject *name_unicode = NULL, *name = NULL, *path = NULL, *m = NULL;
    const char *name_buf, *hook_prefix;
    const char *oldcontext;
    dl_funcptr exportfunc;
    PyModuleDef *def;
    PyObject *(*p0)(void);

    name_unicode = PyObject_GetAttrString(spec, "name");
    if (name_unicode == NULL) {
        return NULL;
    }

    name = get_encoded_name(name_unicode, &hook_prefix);
    if (name == NULL) {
        goto error;
    }
    name_buf = PyBytes_AS_STRING(name);

    path = PyObject_GetAttrString(spec, "origin");
    if (path == NULL)
        goto error;

#ifdef MS_WINDOWS
    exportfunc = _PyImport_FindSharedFuncptrWindows(hook_prefix, name_buf,
                                                    path, fp);
#else
    pathbytes = PyUnicode_EncodeFSDefault(path);
    if (pathbytes == NULL)
        goto error;
    exportfunc = _PyImport_FindSharedFuncptr(hook_prefix, name_buf,
                                             PyBytes_AS_STRING(pathbytes),
                                             fp);
    Py_DECREF(pathbytes);
#endif

    if (exportfunc == NULL) {
        if (!PyErr_Occurred()) {
            PyObject *msg;
            msg = PyUnicode_FromFormat(
                "dynamic module does not define "
                "module export function (%s_%s)",
                hook_prefix, name_buf);
            if (msg == NULL)
                goto error;
            PyErr_SetImportError(msg, name_unicode, path);
            Py_DECREF(msg);
        }
        goto error;
    }

    p0 = (PyObject *(*)(void))exportfunc;

    /* Package context is needed for single-phase init */
    oldcontext = _Py_PackageContext;
    _Py_PackageContext = PyUnicode_AsUTF8(name_unicode);
    if (_Py_PackageContext == NULL) {
        _Py_PackageContext = oldcontext;
        goto error;
    }
    m = p0();
    _Py_PackageContext = oldcontext;

    if (m == NULL) {
        if (!PyErr_Occurred()) {
            PyErr_Format(
                PyExc_SystemError,
                "initialization of %s failed without raising an exception",
                name_buf);
        }
        goto error;
    } else if (PyErr_Occurred()) {
        PyErr_Clear();
        PyErr_Format(
            PyExc_SystemError,
            "initialization of %s raised unreported exception",
            name_buf);
        m = NULL;
        goto error;
    }
    if (Py_TYPE(m) == NULL) {
        /* This can happen when a PyModuleDef is returned without calling
         * PyModuleDef_Init on it
         */
        PyErr_Format(PyExc_SystemError,
                     "init function of %s returned uninitialized object",
                     name_buf);
        m = NULL; /* prevent segfault in DECREF */
        goto error;
    }
    if (PyObject_TypeCheck(m, &PyModuleDef_Type)) {
        Py_DECREF(name_unicode);
        Py_DECREF(name);
        Py_DECREF(path);
        return PyModule_FromDefAndSpec((PyModuleDef*)m, spec);
    }

    /* Fall back to single-phase init mechanism */

    if (hook_prefix == nonascii_prefix) {
        /* don't allow legacy init for non-ASCII module names */
        PyErr_Format(
            PyExc_SystemError,
            "initialization of * did not return PyModuleDef",
            name_buf);
        goto error;
    }

    /* Remember pointer to module init function. */
    def = PyModule_GetDef(m);
    if (def == NULL) {
        PyErr_Format(PyExc_SystemError,
                     "initialization of %s did not return an extension "
                     "module", name_buf);
        goto error;
    }
    def->m_base.m_init = p0;

    /* Remember the filename as the __file__ attribute */
    if (PyModule_AddObject(m, "__file__", path) < 0)
        PyErr_Clear(); /* Not important enough to report */
    else
        Py_INCREF(path);

    if (_PyImport_FixupExtensionObject(m, name_unicode, path) < 0)
        goto error;

    Py_DECREF(name_unicode);
    Py_DECREF(name);
    Py_DECREF(path);

    return m;

error:
    Py_DECREF(name_unicode);
    Py_XDECREF(name);
    Py_XDECREF(path);
    Py_XDECREF(m);
    return NULL;
}
void dgCollisionTaperedCapsule::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
	m_rtti |= dgCollisionTaperedCapsule_RTTI;

	m_radio0 = dgAbsf (radio0);
	m_radio1 = dgAbsf (radio1);
	m_height = dgAbsf (height * 0.5f);

	m_clip1 = dgFloat32 (0.0f);
	m_clip0 = dgFloat32 (0.0f);
	dgFloat32 angle0 = dgFloat32 (-3.141592f / 2.0f);
	dgFloat32 angle1 = dgFloat32 ( 3.141592f / 2.0f);
	do {
		dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f);
		dgVector dir (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f));

		dgVector p0(dir.Scale3 (m_radio0));
		dgVector p1(dir.Scale3 (m_radio1));
		p0.m_x += m_height;
		p1.m_x -= m_height;
		dgFloat32 dir0 = p0 % dir;
		dgFloat32 dir1 = p1 % dir;
		if (dir0 > dir1) {
			angle1 = angle;
			m_clip0 = p0.m_x - m_height;
		} else {
			angle0 = angle;
			m_clip1 = p1.m_x + m_height;
		}
	} while ((angle1 - angle0) > dgFloat32 (0.001f * 3.141592f/180.0f));

	dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f);
	m_sideNormal = dgVector (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f));

	m_clipRadio0 = dgSqrt (m_radio0 * m_radio0 - m_clip0 * m_clip0);
	m_clipRadio1 = dgSqrt (m_radio1 * m_radio1 - m_clip1 * m_clip1);

	dgInt32 i1 = 0;
	dgInt32 i0 = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;

	dgFloat32 dx0 = (m_clip0 - m_radio0) / DG_CAP_SEGMENTS;
	dgFloat32 x0 =  m_radio0 + dx0;
	
	dgFloat32 dx1 = (m_clip1 + m_radio1) / DG_CAP_SEGMENTS;
	dgFloat32 x1 =  -m_radio1 + dx1;
	for (dgInt32 j = 0; j < DG_CAP_SEGMENTS; j ++) {
		dgFloat32 angle = dgFloat32 (0.0f);
		dgFloat32 r0 = dgSqrt (m_radio0 * m_radio0 - x0 * x0);
		dgFloat32 r1 = dgSqrt (m_radio1 * m_radio1 - x1 * x1);

		i0 -= DG_CAPSULE_SEGMENTS;
		for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
			dgFloat32 z = dgSin (angle);
			dgFloat32 y = dgCos (angle);
			m_vertex[i0] = dgVector ( m_height + x0, y * r0, z * r0, dgFloat32 (0.0f));
			m_vertex[i1] = dgVector (-m_height + x1, y * r1, z * r1, dgFloat32 (0.0f));
			i0 ++;
			i1 ++;
			angle += dgPI2 / DG_CAPSULE_SEGMENTS;
		}
		x0 += dx0;
		x1 += dx1;
		i0 -= DG_CAPSULE_SEGMENTS;
	}

	m_vertexCount = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;
	m_edgeCount = DG_CAPSULE_SEGMENTS * (6 + 8 * (DG_CAP_SEGMENTS - 1));
	dgCollisionConvex::m_vertex = m_vertex;

	if (!m_shapeRefCount) {
		dgPolyhedra polyhedra(m_allocator);
		dgInt32 wireframe[DG_CAPSULE_SEGMENTS + 10];

		i1 = 0;
		i0 = DG_CAPSULE_SEGMENTS - 1;
		polyhedra.BeginFace ();
		for (dgInt32 j = 0; j < DG_CAP_SEGMENTS * 2 - 1; j ++) {
			for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { 
				wireframe[0] = i0;
				wireframe[1] = i1;
				wireframe[2] = i1 + DG_CAPSULE_SEGMENTS;
				wireframe[3] = i0 + DG_CAPSULE_SEGMENTS;
				i0 = i1;
				i1 ++;
				polyhedra.AddFace (4, wireframe);
			}
			i0 = i1 + DG_CAPSULE_SEGMENTS - 1;
		}

		for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { 
			wireframe[i] = DG_CAPSULE_SEGMENTS - 1 - i;
		}
		polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe);

		for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { 
			wireframe[i] = i + DG_CAPSULE_SEGMENTS * (DG_CAP_SEGMENTS * 2 - 1);
		}
		polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe);
		polyhedra.EndFace ();

		dgAssert (SanityCheck (polyhedra));

		dgUnsigned64 i = 0;
		dgPolyhedra::Iterator iter (polyhedra);
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);
			edge->m_userData = i;
			i ++;
		}

		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);

			dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];

			ptr->m_vertex = edge->m_incidentVertex;
			ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
			ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
			ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
		}
	}

	m_shapeRefCount ++;
	dgCollisionConvex::m_simplex = m_edgeArray;
	SetVolumeAndCG ();
}
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;
}
Exemple #20
0
// 随机模型1
Model randomModel(double length, 
				  double width, 
				  double margin, 
				  double sMaxH,
				  double sMinH,
				  double sMaxW, 
				  double sMinW, 
				  double surfaceInterval, 
				  double vRange, 
				  int dFlag) {
	Direction* tdp;
	Direction* rdp;
    switch(dFlag) {
	case 1:
		tdp = new Direction(0, 1, 0);
		rdp = new Direction(0, 1, 0);
		break;
	case 2:
		tdp = new Direction(0, -1, 0);
		rdp = new Direction(0, -1, 0);
		break;
	case 3:
		tdp = new Direction(0, 1, 0);
		rdp = new Direction(0, -1, 0);
		break;
	case 4:
		tdp = new Direction(0, -1, 0);
		rdp = new Direction(0, 1, 0);
		break;
	default:
		double ty = plusOrMinus();
		double ry = plusOrMinus();
		Direction td(0, ty, 0);
		Direction rd(0, ry, 0);
		break;
	}

	Vehicle tx = randomVehicle(0, width, *tdp, vRange);
	Vehicle rx = randomVehicle(length, width, *rdp, vRange);

	vector<Material> materials = materialList();
	vector<Surface> surfaces;
	double totalLength = 0;
	srand((int)time(0));
	while (totalLength <= length) {
	    double x0 = (width / 2) + random((int)margin);
	    double y0 = totalLength;
	    double z0 = sMinH + random(int(sMaxH - sMinH));
	    Point p0(x0, y0, z0);

	    double sWidth = sMinW + random((int)(sMaxW - sMinW));
		double x1 = x0;
		double y1 = y0 + sWidth;
		double z1 = 0;
		Point p1(x1, y1, z1);

	    Surface s(p0, p1);
	    surfaces.push_back(s);

		totalLength = y0 + sWidth + random((int)surfaceInterval);
	}
	totalLength = 0;
	while (totalLength <= length) {
	    double x0 = -((width / 2) + random((int)margin));
	    double y0 = totalLength;
	    double z0 = sMinH + random(int(sMaxH - sMinH));
	    Point p0(x0, y0, z0);

	    double sWidth = sMinW + random((int)(sMaxW - sMinW));
		double x1 = x0;
		double y1 = y0 + sWidth;
		double z1 = 0;
		Point p1(x1, y1, z1);

	    Surface s(p0, p1);
	    surfaces.push_back(s);

		totalLength = y0 + sWidth + random((int)surfaceInterval);
	}

	int sizeM = materials.size();
	for (int i = 0; i < (int)surfaces.size(); i++) {
	    int indexM = random(sizeM);
		surfaces[i].setMaterial(materials[indexM]);
		surfaces[i].init();
	}

	Direction tad(0,0,1);
	Direction tarp(0,0,0);
	Direction taPolar(sqrt(double(2))/2, 0, sqrt(double(2))/2);
	Antenna ta(tad, tarp, taPolar, 10);
	vector<Antenna> tas;
	tas.push_back(ta);

	Direction rad(0,0,1);
	Direction rarp(0,0,0);
	Direction raVPolar(1, 0, 0);
	Direction raHPolar(0, 0, 1);
	Antenna ra(rad, rarp, raVPolar, raHPolar);
	vector<Antenna> ras;
	ras.push_back(ra);

	Model model(tx, rx, surfaces, tas, ras);
	return model;
}
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;
}
void Path::addArcTo(const FloatPoint& p1, const FloatPoint& p2, float radius)
{
    FloatPoint p0(m_path.currentPosition());

    if ((p1.x() == p0.x() && p1.y() == p0.y()) || (p1.x() == p2.x() && p1.y() == p2.y()) || radius == 0.f) {
        m_path.lineTo(p1);
        return;
    }

    FloatPoint p1p0((p0.x() - p1.x()), (p0.y() - p1.y()));
    FloatPoint p1p2((p2.x() - p1.x()), (p2.y() - p1.y()));
    float p1p0_length = sqrtf(p1p0.x() * p1p0.x() + p1p0.y() * p1p0.y());
    float p1p2_length = sqrtf(p1p2.x() * p1p2.x() + p1p2.y() * p1p2.y());

    double cos_phi = (p1p0.x() * p1p2.x() + p1p0.y() * p1p2.y()) / (p1p0_length * p1p2_length);

    // The points p0, p1, and p2 are on the same straight line (HTML5, 4.8.11.1.8)
    // We could have used areCollinear() here, but since we're reusing
    // the variables computed above later on we keep this logic.
    if (qFuzzyCompare(qAbs(cos_phi), 1.0)) {
        m_path.lineTo(p1);
        return;
    }

    float tangent = radius / tan(acos(cos_phi) / 2);
    float factor_p1p0 = tangent / p1p0_length;
    FloatPoint t_p1p0((p1.x() + factor_p1p0 * p1p0.x()), (p1.y() + factor_p1p0 * p1p0.y()));

    FloatPoint orth_p1p0(p1p0.y(), -p1p0.x());
    float orth_p1p0_length = sqrt(orth_p1p0.x() * orth_p1p0.x() + orth_p1p0.y() * orth_p1p0.y());
    float factor_ra = radius / orth_p1p0_length;

    // angle between orth_p1p0 and p1p2 to get the right vector orthographic to p1p0
    double cos_alpha = (orth_p1p0.x() * p1p2.x() + orth_p1p0.y() * p1p2.y()) / (orth_p1p0_length * p1p2_length);
    if (cos_alpha < 0.f)
        orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());

    FloatPoint p((t_p1p0.x() + factor_ra * orth_p1p0.x()), (t_p1p0.y() + factor_ra * orth_p1p0.y()));

    // calculate angles for addArc
    orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
    float sa = acos(orth_p1p0.x() / orth_p1p0_length);
    if (orth_p1p0.y() < 0.f)
        sa = 2 * piDouble - sa;

    // anticlockwise logic
    bool anticlockwise = false;

    float factor_p1p2 = tangent / p1p2_length;
    FloatPoint t_p1p2((p1.x() + factor_p1p2 * p1p2.x()), (p1.y() + factor_p1p2 * p1p2.y()));
    FloatPoint orth_p1p2((t_p1p2.x() - p.x()), (t_p1p2.y() - p.y()));
    float orth_p1p2_length = sqrtf(orth_p1p2.x() * orth_p1p2.x() + orth_p1p2.y() * orth_p1p2.y());
    float ea = acos(orth_p1p2.x() / orth_p1p2_length);
    if (orth_p1p2.y() < 0)
        ea = 2 * piDouble - ea;
    if ((sa > ea) && ((sa - ea) < piDouble))
        anticlockwise = true;
    if ((sa < ea) && ((ea - sa) > piDouble))
        anticlockwise = true;

    m_path.lineTo(t_p1p0);

    addArc(p, radius, sa, ea, anticlockwise);
}
int main()
{
  Triangulation t;
  Face_handle fh;

  // Check the empty triangulation
  fh = test_point_location(t, Point(0.5, 0.5), Triangulation::EMPTY);
  CGAL_assertion(fh == Face_handle());

  // Insert the first point
  Point p0(0.5, 0.5);
  Vertex_handle vh0 = t.insert(p0);
  CGAL_assertion(t.is_valid(true));
  CGAL_USE(vh0);

  fh = test_point_location(t, p0, Triangulation::VERTEX);
  CGAL_assertion(fh->has_vertex(vh0));

  fh = test_point_location(t, p0 + Vector(0.1, 0.1), Triangulation::EDGE);
  CGAL_assertion(fh->has_vertex(vh0));

  fh = test_point_location(t, p0 + Vector(-0.1, -0.1), Triangulation::EDGE);
  CGAL_assertion(fh->has_vertex(vh0));

  fh = test_point_location(t, p0 + Vector(-0.2, -0.3), Triangulation::FACE);
  CGAL_assertion(fh->has_vertex(vh0));

  CGAL_assertion(t.is_valid(true));

  // Insert the second point on an edge
  Point p1(0.7, 0.7);
  Vertex_handle vh1 = t.insert(p1);
  CGAL_USE(vh1);
  CGAL_assertion(t.is_valid(true));

  fh = test_point_location(t, p0, Triangulation::VERTEX);
  CGAL_assertion(fh->has_vertex(vh0));

  fh = test_point_location(t, p1, Triangulation::VERTEX);
  CGAL_assertion(fh->has_vertex(vh1));

  fh = test_point_location(t, p0 + Vector(0.1, 0.1), Triangulation::EDGE);
  CGAL_assertion(fh->has_vertex(vh0));
  CGAL_assertion(fh->has_vertex(vh1));

  fh = test_point_location(t, p0 + Vector(-0.1, -0.1), Triangulation::EDGE);
  CGAL_assertion(fh->has_vertex(vh0));
  CGAL_assertion(!fh->has_vertex(vh1));

  fh = test_point_location(t, p1 + Vector(0.1, 0.1), Triangulation::EDGE);
  CGAL_assertion(!fh->has_vertex(vh0));
  CGAL_assertion(fh->has_vertex(vh1));

  fh = test_point_location(t, p0 + Vector(-0.02, -0.03), Triangulation::FACE);
  CGAL_assertion(fh->has_vertex(vh0));

  fh = test_point_location(t, p1 + Vector(-0.02, -0.03), Triangulation::FACE);
  CGAL_assertion(fh->has_vertex(vh1));

  CGAL_assertion(t.is_valid(true));

  // Insert the third point in a face
  Point p2(0.8, 0.6);
  Vertex_handle vh2 = t.insert(p2);
  CGAL_USE(vh2);
  CGAL_assertion(t.is_valid(true));

  fh = test_point_location(t, p0, Triangulation::VERTEX);
  CGAL_assertion(fh->has_vertex(vh0));
  fh = test_point_location(t, p1, Triangulation::VERTEX);
  CGAL_assertion(fh->has_vertex(vh1));
  fh = test_point_location(t, p2, Triangulation::VERTEX);
  CGAL_assertion(fh->has_vertex(vh2));

  fh = test_point_location(t, Point(0.6, 0.6), Triangulation::EDGE);
  CGAL_assertion(fh->has_vertex(vh0));
  CGAL_assertion(fh->has_vertex(vh1));

  test_point_location(t, Point(0.7, 0.6), Triangulation::FACE);
  test_point_location(t, p0 + Vector(-0.02, -0.03), Triangulation::FACE);
  test_point_location(t, p0 + Vector(0.02, -0.03), Triangulation::FACE);
  test_point_location(t, p0 + Vector(-0.02, 0.03), Triangulation::FACE);
  test_point_location(t, p0 + Vector(0.02, 0.03), Triangulation::FACE);

  return 0;
}
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params)
{
  dgInt32 ret;
  dgFloat32 relVelocErr;
  dgFloat32 penetrationErr;
  dgMatrix matrix0;
  dgMatrix matrix1;

  if (m_jointUserCallback)
  {
    m_jointUserCallback(*this, params.m_timestep);
  }

  dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1));
  m_angles = angle.Scale(-dgFloat32(1.0f));

  const dgVector& dir0 = matrix0.m_front;
  const dgVector& dir1 = matrix0.m_up;
  const dgVector& dir2 = matrix0.m_right;
  const dgVector& p0 = matrix0.m_posit;
  const dgVector& p1 = matrix1.m_posit;

  dgPointParam pointData;
  InitPointParam(pointData, m_stiffness, p0, p1);
  CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]);
  CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]);
  CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]);
  ret = 3;

  if (m_twistLimit)
  {
    if (angle.m_x > m_twistAngle)
    {
      dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);

      const dgVector& dir = matrix0.m_right;
      CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      relVelocErr = velocError % dir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
    else if (angle.m_x < -m_twistAngle)
    {
      dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);
      dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f)));
      CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      relVelocErr = velocError % dir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
  }

  if (m_coneLimit)
  {

    dgFloat32 coneCos;
    coneCos = matrix0.m_front % matrix1.m_front;
    if (coneCos < m_coneAngleCos)
    {
      dgVector p0(
          matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
      InitPointParam(pointData, m_stiffness, p0, p0);

      dgVector tangentDir(matrix0.m_front * matrix1.m_front);
      tangentDir = tangentDir.Scale(
          dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f));
      CalculatePointDerivative(ret, params, tangentDir, pointData,
          &m_jointForce[ret]);
      ret++;

      dgVector normalDir(tangentDir * matrix0.m_front);

      dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
      //restitution = contact.m_restitution;
      relVelocErr = velocError % normalDir;
      if (relVelocErr > dgFloat32(1.0e-3f))
      {
        relVelocErr *= dgFloat32(1.1f);
      }

      penetrationErr = MIN_JOINT_PIN_LENGTH
          * (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
      _ASSERTE(penetrationErr >= dgFloat32 (0.0f));

      CalculatePointDerivative(ret, params, normalDir, pointData,
          &m_jointForce[ret]);
      params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
      params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
      params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
//			params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
      SetMotorAcceleration(ret,
          (relVelocErr + penetrationErr) * params.m_invTimestep, params);
      ret++;
    }
  }

  return dgUnsigned32(ret);
}
Exemple #25
0
	void Framework::update(){
		sleep( 16 );
		if ( gFirstFrame ){
			gFirstFrame = false;
			gImage = new Image( "background.dds" ); 
			gImageWidth = gImage->width();
			gImageHeight = gImage->height();
			createTexture( 
				&gTexture,
				gImageWidth, 
				gImageHeight, 
				gImage->data(), 
				gImageWidth, 
				gImageHeight );
			SAFE_DELETE( gImage );
		}
		setTexture( gTexture );
		
		double rotation = static_cast< double >( gCount );
		Vector2 scale( sin( rotation ) + 0.5, cos( rotation ) + 0.5 );

		//行列作る
		Matrix23 m;
		m.setTranslation( Vector2( gImageWidth/2, gImageHeight/2 ) );
		if ( gScaleFirst ){
			m.rotate( rotation );
			m.scale( scale );
		}else{
			m.scale( scale );
			m.rotate( rotation );
		}
		m.translate( Vector2( -gImageWidth/2, -gImageHeight/2 ) );

		Vector2 p0( 0.0, 0.0 );
		Vector2 p1( 100.0, 0.0 );
		Vector2 p2( 0.0, 100.0 );
		Vector2 p3( 100.0, 100.0 );
		double t0[ 2 ] = { 0.0, 0.0 };
		double t1[ 2 ] = { 1.0, 0.0 };
		double t2[ 2 ] = { 0.0, 1.0 };
		double t3[ 2 ] = { 1.0, 1.0 };
		//行列乗算
		m.multiply( &p0, p0 );
		m.multiply( &p1, p1 );
		m.multiply( &p2, p2 );
		m.multiply( &p3, p3 );
		//描画
		//Vector2はそのままは渡せません。
		drawTriangle2D( &p0.x, &p1.x, &p2.x, t0, t1, t2 ); //012
		drawTriangle2D( &p3.x, &p1.x, &p2.x, t3, t1, t2 ); //312

		//スペースで切り替え
		if ( Input::Manager::instance().keyboard().isTriggered( ' ' ) ){
			gScaleFirst = !gScaleFirst;
		}
		++gCount;

		drawDebugString( 0, 0, "press SPACE to swap ROTATION and SCALING" );
		//終了処理してみようか
		if ( isEndRequested() ){
			destroyTexture( &gTexture );
		}
	}
void manifold(Mesh<Vertex3d> &mesh)
{
    const int N = 11;
    const int M = N - 1;
    const double L = ((double)N) / 2.0;

    Vertex3d vertices[N * N];
    int indices[M * M * 6];

    int index = 0;

    for (int ix = 0; ix < N; ++ix)
    {
        for (int iy = 0; iy < N; ++iy)
        {
            double x = (1.0 * ix - L + 0.5) / L;
            double y = (1.0 * iy - L + 0.5) / L;
            double z = manifoldFunction(x,y);


            double xdx = x + 0.01;
            double ydy = y + 0.01;
            double zdx = manifoldFunction(xdx, y);
            double zdy = manifoldFunction(x, ydy);

            GAL::P3d p0(x,y,z);
            GAL::P3d pdx(xdx, y, zdx);
            GAL::P3d pdy(x, ydy, zdy);

            GAL::P3d nor = GAL::Cross(pdx - p0, pdy - p0);

            vertices[index].position = p0;
            vertices[index].normal = nor / GAL::Len(nor);
            ++index;
        }
    }

    index = 0;

    for (int n = 0; n < M; ++n)
    {
        for (int m = 0; m < M; ++m)
        {
            int i0 = n * N + m;
            int i1 = i0 + 1;
            int i2 = i0 + N;
            int i3 = i0 + N + 1;

            indices[index++] = i0;
            indices[index++] = i2;
            indices[index++] = i1;

            indices[index++] = i1;
            indices[index++] = i2;
            indices[index++] = i3;
        }
    }

    mesh.addVertices(vertices, sizeof(vertices) / sizeof(GAL::P3d));
    mesh.addIndices(indices, sizeof(indices) / sizeof(int));
}
		void Render(DemoEntityManager* const scene)
		{
			NewtonCollision* const deformableCollision = NewtonBodyGetCollision(m_body);
			dAssert((NewtonCollisionGetType(deformableCollision) == SERIALIZE_ID_CLOTH_PATCH) || (NewtonCollisionGetType(deformableCollision) == SERIALIZE_ID_DEFORMABLE_SOLID));

			const dFloat* const particles = NewtonDeformableMeshGetParticleArray(deformableCollision);
			int stride = NewtonDeformableMeshGetParticleStrideInBytes(deformableCollision) / sizeof (dFloat);

			// calculate vertex skinning
			for (int i = 0; i < m_vertexCount; i++) {
				int index = m_indexMap[i] * stride;
				m_vertex[i * 3 + 0] = particles[index + 0];
				m_vertex[i * 3 + 1] = particles[index + 1];
				m_vertex[i * 3 + 2] = particles[index + 2];

				// clear the normal for next loop
				m_normal[i * 3 + 0] = 0.0f;
				m_normal[i * 3 + 1] = 0.0f;
				m_normal[i * 3 + 2] = 0.0f;
			}

			// calculate vertex normals 
			int normalStride = 3;
			for (DemoMesh::dListNode* segmentNode = GetFirst(); segmentNode; segmentNode = segmentNode->GetNext()) {
				const DemoSubMesh& subSegment = segmentNode->GetInfo();
				for (int i = 0; i < subSegment.m_indexCount; i += 3) {
					int i0 = subSegment.m_indexes[i + 0] * normalStride;
					int i1 = subSegment.m_indexes[i + 1] * normalStride;
					int i2 = subSegment.m_indexes[i + 2] * normalStride;
					dVector p0(m_vertex[i0], m_vertex[i0 + 1], m_vertex[i0 + 2], 0.0f);
					dVector p1(m_vertex[i1], m_vertex[i1 + 1], m_vertex[i1 + 2], 0.0f);
					dVector p2(m_vertex[i2], m_vertex[i2 + 1], m_vertex[i2 + 2], 0.0f);
					dVector p10(p1 - p0);
					dVector p20(p2 - p0);
					dVector normal(p10.CrossProduct(p20));
					normal = normal.Scale(1.0f / dSqrt(normal.DotProduct3(normal)));

					m_normal[i0 + 0] += normal.m_x;
					m_normal[i0 + 1] += normal.m_y;
					m_normal[i0 + 2] += normal.m_z;

					m_normal[i1 + 0] += normal.m_x;
					m_normal[i1 + 1] += normal.m_y;
					m_normal[i1 + 2] += normal.m_z;

					m_normal[i2 + 0] += normal.m_x;
					m_normal[i2 + 1] += normal.m_y;
					m_normal[i2 + 2] += normal.m_z;
				}
			}

			// normalize all the normals
			for (int i = 0; i < m_vertexCount; i++) {
				dVector n(m_normal[i * 3 + 0], m_normal[i * 3 + 1], m_normal[i * 3 + 2], 0.0f);
				n = n.Scale(1.0f / dSqrt(n.DotProduct3(n)));
				m_normal[i * 3 + 0] = n.m_x;
				m_normal[i * 3 + 1] = n.m_y;
				m_normal[i * 3 + 2] = n.m_z;
			}

			glDisable(GL_CULL_FACE);
			DemoMesh::Render(scene);
			glEnable(GL_CULL_FACE);
		}
	static void Statistics (dgSphere &sphere, dgVector &eigenValues, dgVector &scaleVector, const hacd::HaF32 vertex[], const hacd::HaI32 faceIndex[], hacd::HaI32 indexCount, hacd::HaI32 stride)
	{
		dgVector var (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
		dgVector cov (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
		dgVector centre (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
		dgVector massCenter (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));

		hacd::HaF64 totalArea = hacd::HaF32 (0.0f);
		const hacd::HaF32* const ptr = vertex;
		for (hacd::HaI32 i = 0; i < indexCount; i += 3) {
			hacd::HaI32 index = faceIndex[i] * stride;
			dgVector p0 (&ptr[index]);
			p0 = p0.CompProduct (scaleVector);

			index = faceIndex[i + 1] * stride;;
			dgVector p1 (&ptr[index]);
			p1 = p1.CompProduct (scaleVector);

			index = faceIndex[i + 2] * stride;;
			dgVector p2 (&ptr[index]);
			p2 = p2.CompProduct (scaleVector);

			dgVector normal ((p1 - p0) * (p2 - p0));

			hacd::HaF64 area = hacd::HaF32 (0.5f) * sqrt (normal % normal);

			centre = p0 + p1 + p2;
			centre = centre.Scale (hacd::HaF32  (1.0f / 3.0f));

			// Inertia of each point in the triangle
			hacd::HaF64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;	
			hacd::HaF64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;	
			hacd::HaF64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;	

			hacd::HaF64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;	
			hacd::HaF64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;	
			hacd::HaF64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;	

			if (area > dgEPSILON * 10.0) {
				hacd::HaF64 K = area / hacd::HaF64 (12.0);
				//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
				Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
				Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
				Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);

				Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
				Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
				Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
				centre = centre.Scale ((hacd::HaF32)area);
			} 

			totalArea += area;
			massCenter += centre;
			var += dgVector ((hacd::HaF32)Ixx, (hacd::HaF32)Iyy, (hacd::HaF32)Izz, hacd::HaF32 (0.0f));
			cov += dgVector ((hacd::HaF32)Ixy, (hacd::HaF32)Ixz, (hacd::HaF32)Iyz, hacd::HaF32 (0.0f));
		}

		if (totalArea > dgEPSILON * 10.0) {
			hacd::HaF64 K = hacd::HaF64 (1.0) / totalArea; 
			var = var.Scale ((hacd::HaF32)K);
			cov = cov.Scale ((hacd::HaF32)K);
			massCenter = massCenter.Scale ((hacd::HaF32)K);
		}

		hacd::HaF64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
		hacd::HaF64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
		hacd::HaF64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;

		hacd::HaF64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
		hacd::HaF64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
		hacd::HaF64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;

		sphere.m_front = dgVector ((hacd::HaF32)Ixx, (hacd::HaF32)Ixy, (hacd::HaF32)Ixz, hacd::HaF32 (0.0f));
		sphere.m_up    = dgVector ((hacd::HaF32)Ixy, (hacd::HaF32)Iyy, (hacd::HaF32)Iyz, hacd::HaF32 (0.0f));
		sphere.m_right = dgVector ((hacd::HaF32)Ixz, (hacd::HaF32)Iyz, (hacd::HaF32)Izz, hacd::HaF32 (0.0f));
		sphere.EigenVectors(eigenValues);
	}
Exemple #29
0
void GfxMesh::addCube(float _size, Vec3f _pos, Vec4f _color, bool _front, bool _back, bool _right, bool _left, bool _top, bool _bottom)
{
	_size /= 2.0f;

	Vec3f p0(_pos.x - _size, _pos.y - _size, _pos.z + _size);
	Vec3f p1(_pos.x + _size, _pos.y - _size, _pos.z + _size);
	Vec3f p2(_pos.x + _size, _pos.y + _size, _pos.z + _size);
	Vec3f p3(_pos.x - _size, _pos.y + _size, _pos.z + _size);
	Vec3f p4(_pos.x + _size, _pos.y - _size, _pos.z - _size);
	Vec3f p5(_pos.x - _size, _pos.y - _size, _pos.z - _size);
	Vec3f p6(_pos.x - _size, _pos.y + _size, _pos.z - _size);
	Vec3f p7(_pos.x + _size, _pos.y + _size, _pos.z - _size);

	Vec3f n;

	unsigned int v0, v1, v2, v3, v4, v5, v6, v7;

	// Front
	if (_front)
	{
		n = Vec3f(0.0f, 0.0f, 1.0f);

		v0 = addVertex(p0, n, _color);
		v1 = addVertex(p1, n, _color);
		v2 = addVertex(p2, n, _color);
		v3 = addVertex(p3, n, _color);

		addTriangle(v0, v1, v2);
		addTriangle(v0, v2, v3);
	}

	// Back
	if (_back)
	{
		n = Vec3f(0.0f, 0.0f, -1.0f);

		v4 = addVertex(p4, n, _color);
		v5 = addVertex(p5, n, _color);
		v6 = addVertex(p6, n, _color);
		v7 = addVertex(p7, n, _color);

		addTriangle(v4, v5, v6);
		addTriangle(v4, v6, v7);
	}

	// Right
	if (_right)
	{
		n = Vec3f(1.0f, 0.0f, 0.0f);

		v1 = addVertex(p1, n, _color);
		v4 = addVertex(p4, n, _color);
		v7 = addVertex(p7, n, _color);
		v2 = addVertex(p2, n, _color);

		addTriangle(v1, v4, v7);
		addTriangle(v1, v7, v2);
	}

	// Left
	if (_left)
	{
		n = Vec3f(-1.0f, 0.0f, 0.0f);

		v5 = addVertex(p5, n, _color);
		v0 = addVertex(p0, n, _color);
		v3 = addVertex(p3, n, _color);
		v6 = addVertex(p6, n, _color);

		addTriangle(v5, v0, v3);
		addTriangle(v5, v3, v6);
	}

	// Top
	if (_top)
	{
		n = Vec3f(0.0f, 1.0f, 0.0f);

		v3 = addVertex(p3, n, _color);
		v2 = addVertex(p2, n, _color);
		v7 = addVertex(p7, n, _color);
		v6 = addVertex(p6, n, _color);

		addTriangle(v3, v2, v7);
		addTriangle(v3, v7, v6);
	}

	// Bottom
	if (_bottom)
	{
		n = Vec3f(0.0f, -1.0f, 0.0f);

		v5 = addVertex(p5, n, _color);
		v4 = addVertex(p4, n, _color);
		v1 = addVertex(p1, n, _color);
		v0 = addVertex(p0, n, _color);

		addTriangle(v5, v4, v1);
		addTriangle(v5, v1, v0);
	}
}
void CreateHeightFieldMesh (NewtonCollision* collision, Entity* ent)
{
	int width;
	int height;
	dFloat hScale;
	dFloat vScale;
	unsigned short* elevations;
	NewtonCollisionInfoRecord collisionInfo;

	// keep the compiler happy
	memset (&collisionInfo, 0, sizeof (NewtonCollisionInfoRecord));
	NewtonCollisionGetInfo (collision, &collisionInfo);

	// get the info from the collision mesh and create a visual mesh
	width = collisionInfo.m_heightField.m_width;
	height = collisionInfo.m_heightField.m_height;
	elevations = collisionInfo.m_heightField.m_elevation;
	vScale = collisionInfo.m_heightField.m_verticalScale;
	hScale = collisionInfo.m_heightField.m_horizonalScale;

	// allocate space to store vertex data
	ent->m_vertexCount = width * height;
	ent->m_vertex = (dFloat*) malloc (3 * width * height * sizeof (dFloat));
	ent->m_normal = (dFloat*) malloc (3 * width * height * sizeof (dFloat));
	ent->m_uv = (dFloat*) malloc (2 * width * height * sizeof (dFloat));



	// scan the height field and convert every cell into two triangles
	for (int z = 0; z < height; z ++) {
		int z0;
		int z1;
		z0 = ((z - 1) < 0) ? 0 : z - 1;
		z1 = ((z + 1) > (height - 1)) ? height - 1 : z + 1 ;
		for (int x = 0; x < width; x ++) {
			int x0;
			int x1;

			x0 = ((x - 1) < 0) ? 0 : x - 1;
			x1 = ((x + 1) > (width - 1)) ? width - 1 : x + 1 ;

			dVector p0 (hScale * x0, elevations[z * width + x1] * vScale, hScale * z);
			dVector p1 (hScale * x1, elevations[z * width + x0] * vScale, hScale * z);
			dVector x10 (p1 - p0);

			dVector q0 (hScale * x, elevations[z0 * width + x] * vScale, hScale * z0);
			dVector q1 (hScale * x, elevations[z1 * width + x] * vScale, hScale * z1);
			dVector z10 (q1 - q0);

			dVector normal (z10 * x10);
			normal = normal.Scale (dSqrt (1.0f / (normal % normal)));
			dVector point (hScale * x, elevations[z * width + x] * vScale, hScale * z);

			ent->m_vertex[(z * width + x) * 3 + 0] = point.m_x;
			ent->m_vertex[(z * width + x) * 3 + 1] = point.m_y;
			ent->m_vertex[(z * width + x) * 3 + 2] = point.m_z;

			ent->m_normal[(z * width + x) * 3 + 0] = normal.m_x;
			ent->m_normal[(z * width + x) * 3 + 1] = normal.m_y;
			ent->m_normal[(z * width + x) * 3 + 2] = normal.m_z;

			ent->m_uv[(z * width + x) * 2 + 0] = x * TEXTURE_SCALE;
			ent->m_uv[(z * width + x) * 2 + 1] = z * TEXTURE_SCALE;
		}
	}

	
	// since the bitmap sample is 256 x 256, i fix into a single 16 bit index vertex array with
	ent->m_subMeshCount = 1;
	ent->m_subMeshes = (Entity::SubMesh*) malloc (sizeof (Entity::SubMesh));

	// allocate space to the index list
	ent->m_subMeshes[0].m_textureHandle = LoadTexture ("grassAndDirt.tga");
	ent->m_subMeshes[0].m_indexCount = (width - 1) * (height - 1) * 6;
	ent->m_subMeshes[0].m_indexArray = (unsigned short*) malloc (ent->m_subMeshes[0].m_indexCount * sizeof (unsigned short));

	// now following the grid pattern and create and index list
	int index;
	int vertexIndex;

	index = 0;
	vertexIndex = 0;
	for (int z = 0; z < height - 1; z ++) {
		vertexIndex = z * width;
		for (int x = 0; x < width - 1; x ++) {

			ent->m_subMeshes[0].m_indexArray[index + 0] = GLushort (vertexIndex);
			ent->m_subMeshes[0].m_indexArray[index + 1] = GLushort (vertexIndex + width);
			ent->m_subMeshes[0].m_indexArray[index + 2] = GLushort (vertexIndex + 1);
			index += 3;

			ent->m_subMeshes[0].m_indexArray[index + 0] = GLushort (vertexIndex + 1);
			ent->m_subMeshes[0].m_indexArray[index + 1] = GLushort (vertexIndex + width);
			ent->m_subMeshes[0].m_indexArray[index + 2] = GLushort (vertexIndex + width + 1);
			index += 3;
			vertexIndex ++;
		}
	}

	// Optimize the mesh for hardware rendering if possible
	ent->OptimizeMesh();

/*
	dVector boxP0; 
	dVector boxP1; 
	// get the position of the aabb of this geometry
	dMatrix matrix (ent->m_curRotation, ent->m_curPosition);
	NewtonCollisionCalculateAABB (collision, &matrix[0][0], &boxP0.m_x, &boxP1.m_x); 

	// place the origin of the visual mesh at the center of the height field
	matrix.m_posit = (boxP0 + boxP1).Scale (-0.5f);
	matrix.m_posit.m_w = 1.0f;
	ent->m_curPosition = matrix.m_posit;
	ent->m_prevPosition = matrix.m_posit;


	// create the level rigid body
	body = NewtonCreateBody(world, collision);

	// release the collision tree (this way the application does not have to do book keeping of Newton objects
	NewtonReleaseCollision (world, collision);


	// save the pointer to the graphic object with the body.
	NewtonBodySetUserData (body, ent);

	// set the global position of this body
	NewtonBodySetMatrix (body, &matrix[0][0]); 


	// set the destructor for this object
//	NewtonBodySetDestructorCallback (body, Destructor);

	// get the position of the aabb of this geometry
	NewtonCollisionCalculateAABB (collision, &matrix[0][0], &boxP0.m_x, &boxP1.m_x); 

	// add some extra padding the world size
	boxP0.m_x -=  10.0f;
	boxP0.m_y -=  10.0f;
	boxP0.m_z -=  10.0f;
	boxP1.m_x +=  10.0f;
	boxP1.m_y += 400.0f;
	boxP1.m_z +=  10.0f;

	// set the world size
	NewtonSetWorldSize (world, &boxP0.m_x, &boxP1.m_x); 
	return body;
*/
}