コード例 #1
0
ファイル: btDbvtBroadphase.cpp プロジェクト: stolk/bullet3
		void update(btScalar speed, btScalar amplitude, btBroadphaseInterface* pbi)
		{
			time += speed;
			center[0] = btCos(time * (btScalar)2.17) * amplitude +
						btSin(time) * amplitude / 2;
			center[1] = btCos(time * (btScalar)1.38) * amplitude +
						btSin(time) * amplitude;
			center[2] = btSin(time * (btScalar)0.777) * amplitude;
			pbi->setAabb(proxy, center - extents, center + extents, 0);
		}
コード例 #2
0
ファイル: tire.cpp プロジェクト: Timo6/vdrift
btScalar Tire::PacejkaFy(
	btScalar alpha,
	btScalar gamma,
	btScalar Fz,
	btScalar dFz,
	btScalar friction_coeff,
	btScalar & Dy,
	btScalar & BCy,
	btScalar & Shf) const
{
	const btScalar * p = coefficients;
	btScalar Fz0 = nominal_load;

	// vertical shift
	btScalar Sv = Fz * (p[PVY1] + p[PVY2] * dFz + (p[PVY3] + p[PVY4] * dFz) * gamma);

	// horizontal shift
	btScalar Sh = p[PHY1] + p[PHY2] * dFz + p[PHY3] * gamma;

	// composite slip angle
	btScalar A = alpha + Sh;

	// slope at origin
	btScalar K = p[PKY1] * Fz0 * btSin(2 * btAtan(Fz / (p[PKY2] * Fz0))) * (1 - p[PKY3] * btFabs(gamma));

	// curvature factor
	btScalar E = (p[PEY1] + p[PEY2] * dFz) * (1 - (p[PEY3] + p[PEY4] * gamma) * sgn(A));

	// peak factor
	btScalar D = Fz * (p[PDY1] + p[PDY2] * dFz) * (1 - p[PDY3] * gamma * gamma);

	// shape factor
	btScalar C = p[PCY1];

	// stiffness factor
	btScalar B = K / (C * D);

	// force
	btScalar F = D * btSin(C * btAtan(B * A - E * (B * A - btAtan(B * A)))) + Sv;

	// scale by surface friction
	F *= friction_coeff;

	// aligning torque params
	Dy = D;
	BCy = B * C;
	Shf = Sh + Sv / K;

	return F;
}
コード例 #3
0
btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
{
	// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
	btScalar xEllipse = btCos(fAngleInRadians);
	btScalar yEllipse = btSin(fAngleInRadians);

	// Use the slope of the vector (using x/yEllipse) and find the length
	// of the line that intersects the ellipse:
	//  x^2   y^2
	//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
	//  a^2   b^2
	// Do the math and it should be clear.

	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
	if (fabs(xEllipse) > SIMD_EPSILON)
	{
		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
		swingLimit = sqrt(swingLimit2);
	}

	// convert into point in constraint space:
	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
	btQuaternion qSwing(vSwingAxis, swingLimit);
	btVector3 vPointInConstraintSpace(fLength,0,0);
	return quatRotate(qSwing, vPointInConstraintSpace);
}
コード例 #4
0
btSoftBody*		btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
												   const btVector3& radius,
												   int res)
{
	struct	Hammersley
	{
		static void	Generate(btVector3* x,int n)
		{
			for(int i=0;i<n;i++)
			{
				btScalar	p=0.5,t=0;
				for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
				btScalar	w=2*t-1;
				btScalar	a=(SIMD_PI+2*i*SIMD_PI)/n;
				btScalar	s=btSqrt(1-w*w);
				*x++=btVector3(s*btCos(a),s*btSin(a),w);
			}
		}
	};
	btAlignedObjectArray<btVector3>	vtx;
	vtx.resize(3+res);
	Hammersley::Generate(&vtx[0],vtx.size());
	for(int i=0;i<vtx.size();++i)
	{
		vtx[i]=vtx[i]*radius+center;
	}
	return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
}
コード例 #5
0
int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
{
	int m=-1;
	while(m==-1)
	{
		m = maxdirfiltered(p,count,dir,allow);
		if(allow[m]==3) return m;
		T u = orth(dir);
		T v = btCross(u,dir);
		int ma=-1;
		for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
		{
			btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
			btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
			int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
			if(ma==m && mb==m)
			{
				allow[m]=3;
				return m;
			}
			if(ma!=-1 && ma!=mb)  // Yuck - this is really ugly
			{
				int mc = ma;
				for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
				{
					btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
					btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
					int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
					if(mc==m && md==m)
					{
						allow[m]=3;
						return m;
					}
					mc=md;
				}
			}
			ma=mb;
		}
		allow[m]=0;
		m=-1;
	}
	btAssert(0);
	return m;
} 
コード例 #6
0
ファイル: tire.cpp プロジェクト: Timo6/vdrift
btScalar Tire::PacejkaSvy(
	btScalar sigma,
	btScalar alpha,
	btScalar gamma,
	btScalar dFz,
	btScalar Dy)
{
	const btScalar * p = coefficients;
	btScalar Dv = Dy * (p[RVY1] + p[RVY2] * dFz + p[RVY3] * gamma) * btCos(btAtan(p[RVY4] * alpha));
	btScalar Sv = Dv * btSin(p[RVY5] * btAtan(p[RVY6] * sigma));
	return Sv;
}
コード例 #7
0
void fillWithRandomNumbers(proto::BigBirdConstructionData* info)
{
    for (int ii = 0 ; ii < kNumSamplesInWingbeat; ++ii) {
        btScalar req_angle = 0.9f * info->wingflaphingelimit() * btSin((float)ii/kNumSamplesInWingbeat * SIMD_2_PI * kFreq);
        btScalar feather_angle = 0.9f * info->featheraoahingelimit() * btCos((float)ii/kNumSamplesInWingbeat * SIMD_2_PI * kFreq);
        proto::WingbeatSample* sample = info->mutable_wingbeatdata()->add_sample();
        sample->set_wing(req_angle);
        sample->set_feather(feather_angle);
        //sample->set_wing(-info->wingflaphingelimit() + (((double)rand())/RAND_MAX)*(2*info->wingflaphingelimit()));
        //sample->set_feather(-info->featheraoahingelimit() + (((double)rand())/RAND_MAX)*(2*info->featheraoahingelimit()));
        //sample->set_feather(0);
    }
}
コード例 #8
0
ファイル: Tutorial.cpp プロジェクト: 20-sim/bullet3
	void	integrateVelocity(double deltaTime)
	{
		LWPose newPose;
		
		newPose.m_position = m_worldPose.m_position + m_linearVelocity*deltaTime;
		
		if (m_flags & LWFLAG_USE_QUATERNION_DERIVATIVE)
		{
			newPose.m_orientation = m_worldPose.m_orientation;
			newPose.m_orientation += (m_angularVelocity * newPose.m_orientation) * (deltaTime * btScalar(0.5));
			newPose.m_orientation.normalize();
			m_worldPose = newPose;
		} else
		{
			//Exponential map
			//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
			
			//btQuaternion q_w = [ sin(|w|*dt/2) * w/|w| , cos(|w|*dt/2)]
			//btQuaternion q_new =  q_w * q_old;
			
			b3Vector3 axis;
			b3Scalar	fAngle = m_angularVelocity.length();
			//limit the angular motion
			const btScalar angularMotionThreshold =  btScalar(0.5)*SIMD_HALF_PI;
			
			if (fAngle*deltaTime > angularMotionThreshold)
			{
				fAngle = angularMotionThreshold / deltaTime;
			}
			
			if ( fAngle < btScalar(0.001) )
			{
				// use Taylor's expansions of sync function
				axis   = m_angularVelocity*( btScalar(0.5)*deltaTime-(deltaTime*deltaTime*deltaTime)*(btScalar(0.020833333333))*fAngle*fAngle );
			}
			else
			{
				// sync(fAngle) = sin(c*fAngle)/t
				axis   = m_angularVelocity*( btSin(btScalar(0.5)*fAngle*deltaTime)/fAngle );
			}
			b3Quaternion dorn (axis.x,axis.y,axis.z,btCos( fAngle*deltaTime*b3Scalar(0.5) ));
			b3Quaternion orn0 = m_worldPose.m_orientation;
			
			b3Quaternion predictedOrn = dorn * orn0;
			predictedOrn.normalize();
			m_worldPose.m_orientation = predictedOrn;
		}
		
	}
コード例 #9
0
ファイル: wheel.cpp プロジェクト: logzero/vdrift
bool Wheel::updateContact(btScalar raylen)
{
	// update wheel transform
	transform.setOrigin(body->getCenterOfMassOffset() + suspension.getPosition());
	transform.setRotation(suspension.getOrientation());
	transform = body->getCenterOfMassTransform() * transform;

	// wheel contact
	btVector3 wheelPos = transform.getOrigin();
	btVector3 rayDir = -transform.getBasis().getColumn(2); // down
	btScalar rayLen = radius + raylen;
	btVector3 rayStart = wheelPos - rayDir * radius;
	ray.set(rayStart, rayDir, rayLen);
	ray.m_exclude = body;
	world->rayTest(ray.m_rayFrom, ray.m_rayTo, ray);

	// surface bumpiness
	btScalar bump = 0;
	const Surface * surface = ray.getSurface();
	if (surface)
	{
		btScalar posx = ray.getPoint()[0];
		btScalar posz = ray.getPoint()[2];
		btScalar phase = 2 * M_PI * (posx + posz) / surface->bumpWaveLength;
		btScalar shift = 2 * btSin(phase * M_PI_2);
		btScalar amplitude = 0.25 * surface->bumpAmplitude;
		bump = amplitude * (btSin(phase + shift) + btSin(M_PI_2 * phase) - 2.0f);
	}

	// update suspension
	btScalar relDisplacement = 2 * radius - ray.getDepth() + bump;
	btScalar displacement = suspension.getDisplacement() + relDisplacement;
	suspension.setDisplacement(displacement);

	return displacement >= 0;
}
コード例 #10
0
ファイル: tire.cpp プロジェクト: Timo6/vdrift
btScalar Tire::PacejkaFx(
	btScalar sigma,
	btScalar Fz,
	btScalar dFz,
	btScalar friction_coeff) const
{
	const btScalar * p = coefficients;

	// vertical shift
	btScalar Sv = Fz * (p[PVX1] + p[PVX2] * dFz);

	// horizontal shift
	btScalar Sh = p[PHX1] + p[PHX2] * dFz;

	// composite slip
	btScalar S = sigma + Sh;

	// slope at origin
	btScalar K = Fz * (p[PKX1] + p[PKX2] * dFz) * btExp(-p[PKX3] * dFz);

	// curvature factor
	btScalar E = (p[PEX1] + p[PEX2] * dFz + p[PEX3] * dFz * dFz) * (1 - p[PEX4] * sgn(S));

	// peak factor
	btScalar D = Fz * (p[PDX1] + p[PDX2] * dFz);

	// shape factor
	btScalar C = p[PCX1];

	// stiffness factor
	btScalar B =  K / (C * D);

	// force
	btScalar F = D * btSin(C * btAtan(B * S - E * (B * S - btAtan(B * S)))) + Sv;

	// scale by surface friction
	F *= friction_coeff;

	return F;
}
コード例 #11
0
Ragdoll::Ragdoll(btDiscreteDynamicsWorld * world, btScalar heightOffset)
{
	this->world = world;

	btScalar bodyMass = (btScalar)70.0;

	// feet definition
	btScalar footLength = (btScalar)0.24, footHeight = (btScalar)0.08, footWidth = (btScalar)0.15;
	btScalar footTop = footHeight + heightOffset;
	btScalar footXOffset = (btScalar)0.04, footZOffset = (btScalar)0.167;
	btScalar footMass = bodyMass * (btScalar)1.38/100;

	// left foot
	btCollisionShape *bpShape = new btBoxShape(btVector3(footLength/2, footHeight/2, footWidth/2));
	btQuaternion bpRotation(0, 0, 0, 1);
	btVector3 bpTranslation(footXOffset, footTop-footHeight/2, -footZOffset);
	btDefaultMotionState *bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	btVector3 bpInertia(0, 0, 0);
	btScalar bpMass = footMass;
	bpShape->calculateLocalInertia(bpMass, bpInertia);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(bpMass, bpMotionState, bpShape, bpInertia);
	btRigidBody *body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_LEFT_FOOT] = new GrBulletObject(body);
	addBoxLinker(footLength / 2, footHeight / 2, footWidth / 2, bodyParts[BODYPART_LEFT_FOOT]);

	// right foot				 
	bpShape = new btBoxShape(btVector3(footLength / 2, footHeight / 2, footWidth / 2));
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(footXOffset, footTop - footHeight / 2, footZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = footMass;
	bpShape->calculateLocalInertia(bpMass, bpInertia);
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_RIGHT_FOOT] = new GrBulletObject(body);
	addBoxLinker(footLength / 2, footHeight / 2, footWidth / 2, bodyParts[BODYPART_RIGHT_FOOT]);

	// legs definition
	btScalar legRadius = (btScalar)0.055, legHeight = (btScalar) 0.34;
	btScalar legTop = footTop + legHeight;
	btScalar legMass = bodyMass * (btScalar)5.05/100;

	// left leg
	bpShape = new btCapsuleShape(legRadius, legHeight - 2 * legRadius);
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, legTop - legHeight / 2, -footZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = legMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_LEFT_LEG] = new GrBulletObject(body);
	addCylinderLinker(legRadius, legHeight, bodyParts[BODYPART_LEFT_LEG]);

	// right leg											 			  
	bpShape = new btCapsuleShape(legRadius, legHeight - 2 * legRadius);
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, legTop - legHeight / 2, footZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = legMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_RIGHT_LEG] = new GrBulletObject(body);
	addCylinderLinker(legRadius, legHeight, bodyParts[BODYPART_RIGHT_LEG]);

	// thighs definition
	btScalar thighRadius = (btScalar)0.07, thighHeight = (btScalar)0.32;
	btScalar thighAngle = (btScalar) (12 * M_PI / 180);
	btScalar thighAngledHeight = thighHeight * btCos(thighAngle);
	btScalar thighTop = legTop + thighAngledHeight;
	btScalar thighZOffset = (btScalar)0.1136;
	btScalar thighMass = bodyMass * (btScalar)11.125/100;

	// left thigh  h=0.316						 
	bpShape = new btCapsuleShape(thighRadius, thighHeight - 2 * thighRadius);
	bpRotation = btQuaternion(1 * btSin(thighAngle / 2), 0, 0, btCos(thighAngle / 2));
	bpTranslation = btVector3(0, thighTop - thighAngledHeight/2, -thighZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = thighMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_LEFT_THIGH] = new GrBulletObject(body);
	addCylinderLinker(thighRadius, thighHeight, bodyParts[BODYPART_LEFT_THIGH]);

	// right thigh  h=0.316	, ends at 0.706				  							
	bpShape = new btCapsuleShape(thighRadius, thighHeight - 2 * thighRadius);
	bpRotation = btQuaternion(-1 * btSin(thighAngle / 2), 0, 0, btCos(thighAngle / 2));
	bpTranslation = btVector3(0, thighTop - thighAngledHeight / 2, thighZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = thighMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_RIGHT_THIGH] = new GrBulletObject(body);
	addCylinderLinker(thighRadius, thighHeight, bodyParts[BODYPART_RIGHT_THIGH]);

	// pelvis definition
	btScalar pelvisLength = (btScalar)0.19, pelvisHeight = (btScalar)0.15, pelvisWidth = (btScalar)0.35;
	btScalar pelvisTop = thighTop + pelvisHeight;
	btScalar pelvisMass = bodyMass * (btScalar)14.81/100;

	// pelvis 
	bpShape = new btBoxShape(btVector3(pelvisLength/2, pelvisHeight/2, pelvisWidth/2));
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, pelvisTop - pelvisHeight/2, 0);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = pelvisMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_PELVIS] = new GrBulletObject(body);
	addBoxLinker(pelvisLength / 2, pelvisHeight / 2, pelvisWidth / 2, bodyParts[BODYPART_PELVIS]);

	// abdomen definition
	btScalar abdomenLength = (btScalar)0.13, abdomenHeight = (btScalar)0.113, abdomenWidth = (btScalar)0.268;
	btScalar abdomenTop = pelvisTop + abdomenHeight;
	btScalar abdomenMass = bodyMass * (btScalar)12.65 / 100;

	// abdomen
	bpShape = new btBoxShape(btVector3(abdomenLength/2, abdomenHeight/2, abdomenWidth/2));
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, abdomenTop - abdomenHeight/2, 0);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = abdomenMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_ABDOMEN] = new GrBulletObject(body);
	addBoxLinker(abdomenLength / 2, abdomenHeight / 2, abdomenWidth / 2, bodyParts[BODYPART_ABDOMEN]);

	// thorax definition
	btScalar thoraxLength = (btScalar)0.2, thoraxHeight = (btScalar)0.338, thoraxWidth = (btScalar)0.34;
	btScalar thoraxTop = abdomenTop + thoraxHeight;
	btScalar thoraxMass = bodyMass * (btScalar)18.56/100;

	// thorax 
	bpShape = new btBoxShape(btVector3(thoraxLength/2, thoraxHeight/2, thoraxWidth/2));
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, thoraxTop-thoraxHeight/2, 0);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = thoraxMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_THORAX] = new GrBulletObject(body);
	addBoxLinker(thoraxLength / 2, thoraxHeight / 2, thoraxWidth / 2, bodyParts[BODYPART_THORAX]);

	// upper arms definition
	btScalar upperArmRadius = (btScalar)0.04, upperArmHeight = (btScalar)0.25;
	btScalar upperArmAngle = (btScalar)(25 * M_PI / 180);
	btScalar upperArmAngledHeight = upperArmHeight * btCos(upperArmAngle);
	btScalar upperArmBottom = thoraxTop - upperArmAngledHeight;
	btScalar upperArmZOffset = (btScalar)0.223;
	btScalar upperArmMass = bodyMass * (btScalar)3.075 / 100;

	// left upper arm  			
	bpShape = new btCapsuleShape(upperArmRadius, upperArmHeight - 2 * upperArmRadius);
	bpShape = new btBoxShape(btVector3(upperArmRadius, upperArmHeight/2, upperArmRadius));
	bpRotation = btQuaternion(1 * btSin(upperArmAngle/2), 0, 0, btCos(upperArmAngle/2));
	bpTranslation = btVector3(0, upperArmBottom + upperArmAngledHeight/2, -upperArmZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = upperArmMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_LEFT_UPPER_ARM] = new GrBulletObject(body);
	addCylinderLinker(upperArmRadius, upperArmHeight, bodyParts[BODYPART_LEFT_UPPER_ARM]);

	// right upper arm  									 									
	bpShape = new btCapsuleShape(upperArmRadius, upperArmHeight - 2 * upperArmRadius);
	bpRotation = btQuaternion(-1 * btSin(upperArmAngle / 2), 0, 0, btCos(upperArmAngle / 2));
	bpTranslation = btVector3(0, upperArmBottom + upperArmAngledHeight / 2, upperArmZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = upperArmMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_RIGHT_UPPER_ARM] = new GrBulletObject(body);
	addCylinderLinker(upperArmRadius, upperArmHeight, bodyParts[BODYPART_RIGHT_UPPER_ARM]);

	// lower arms definition
	btScalar lowerArmRadius = (btScalar)0.035, lowerArmHeight = (btScalar)0.28;
	btScalar lowerArmTop = upperArmBottom;
	btScalar lowerArmZOffset = (btScalar)0.276;
	btScalar lowerArmMass = bodyMass * (btScalar)1.72 / 100;

	// left lower arm  
	bpShape = new btCapsuleShape(lowerArmRadius, lowerArmHeight - 2 * lowerArmRadius);
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, lowerArmTop - lowerArmHeight/2, -lowerArmZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = lowerArmMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_LEFT_LOWER_ARM] = new GrBulletObject(body);
	addCylinderLinker(lowerArmRadius, lowerArmHeight, bodyParts[BODYPART_LEFT_LOWER_ARM]);

	// right lower arm     																	  
	bpShape = new btCapsuleShape(lowerArmRadius, lowerArmHeight - 2 * lowerArmRadius);
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, lowerArmTop - lowerArmHeight / 2, lowerArmZOffset);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = lowerArmMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_RIGHT_LOWER_ARM] = new GrBulletObject(body);
	addCylinderLinker(lowerArmRadius, lowerArmHeight, bodyParts[BODYPART_RIGHT_LOWER_ARM]);

	// neck definition
	btScalar neckRadius = (btScalar)0.05, neckHeight = (btScalar)0.04;
	btScalar neckTop = thoraxTop + neckHeight;
	btScalar neckMass = (btScalar)0.5;
	
	// neck	  
	bpShape = new btBoxShape(btVector3(neckRadius, neckHeight/2, neckRadius));
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, neckTop - neckHeight/2, 0);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = neckMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_NECK] = new GrBulletObject(body);
	addCylinderLinker(neckRadius, neckHeight, bodyParts[BODYPART_NECK]);

	// head definition
	btScalar headRadius = (btScalar)0.1, headHeight = (btScalar)0.283;
	btScalar headTop = neckTop + headHeight;
	btScalar headMass = (btScalar)5.0;

	// head	 																  
	bpShape = new btCapsuleShape(headRadius, headHeight-2*headRadius);
	bpRotation = btQuaternion(0, 0, 0, 1);
	bpTranslation = btVector3(0, headTop - headHeight/2, 0);
	bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
	bpInertia = btVector3(0, 0, 0);
	bpMass = headMass;
	rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
	body = new btRigidBody(rbInfo);
	world->addRigidBody(body);
	bodyParts[BODYPART_HEAD] = new GrBulletObject(body);
	addSphereLinker(headHeight/2, bodyParts[BODYPART_HEAD]);

	// create joints
	btConeTwistConstraint *coneConstraint;
	btHingeConstraint * hingeConstraint;

	// head-neck
	btTransform bodyA, bodyB;
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, 0, M_PI_2);
	bodyA.setOrigin(btVector3(0, -(headHeight/2 + 0.02), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, 0, M_PI_2);
	bodyB.setOrigin(btVector3(0, neckHeight/2 + 0.01, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_HEAD]->getRigidBody(), *bodyParts[BODYPART_NECK]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, M_PI_2);
	joints[JOINT_HEAD_NECK] = coneConstraint;
	world->addConstraint(joints[JOINT_HEAD_NECK], false);

	// neck-thorax
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, 0, M_PI_2);
	bodyA.setOrigin(btVector3(0, -neckHeight/2 - 0.02, 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, 0, M_PI_2);
	bodyB.setOrigin(btVector3(0, thoraxHeight / 2 + 0.02, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_NECK]->getRigidBody(), *bodyParts[BODYPART_THORAX]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_NECK_THORAX] = coneConstraint;
	world->addConstraint(joints[JOINT_NECK_THORAX], true);

	// thorax-leftupperarm																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, thoraxHeight / 2 - upperArmRadius, -(thoraxWidth / 2 + upperArmRadius)));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, upperArmHeight / 2 + upperArmRadius, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_THORAX]->getRigidBody(), *bodyParts[BODYPART_LEFT_UPPER_ARM]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_2, M_PI_2, 0);
	joints[JOINT_THORAX_LEFT_UPPER_ARM] = coneConstraint;
	world->addConstraint(joints[JOINT_THORAX_LEFT_UPPER_ARM], true);

	// left upper-lower arm	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(upperArmHeight / 2 + upperArmRadius / 2), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, lowerArmHeight / 2 + lowerArmRadius / 2, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_LEFT_UPPER_ARM]->getRigidBody(), *bodyParts[BODYPART_LEFT_LOWER_ARM]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_2, M_PI_2, M_PI_4);
	joints[JOINT_LEFT_ARM_UPPER_LOWER] = coneConstraint;
	world->addConstraint(joints[JOINT_LEFT_ARM_UPPER_LOWER], true);

	// thorax-rightupperarm																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, -M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, thoraxHeight / 2 - upperArmRadius, (thoraxWidth / 2 + upperArmRadius)));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, -M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, upperArmHeight / 2 + upperArmRadius, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_THORAX]->getRigidBody(), *bodyParts[BODYPART_RIGHT_UPPER_ARM]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_2, M_PI_2, 0);
	joints[JOINT_THORAX_RIGHT_UPPER_ARM] = coneConstraint;
	world->addConstraint(joints[JOINT_THORAX_RIGHT_UPPER_ARM], true);

	// right upper-lower arm	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(upperArmHeight / 2 + upperArmRadius / 2), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, lowerArmHeight / 2 + lowerArmRadius / 2, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_RIGHT_UPPER_ARM]->getRigidBody(), *bodyParts[BODYPART_RIGHT_LOWER_ARM]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_2, M_PI_2, M_PI_4);
	joints[JOINT_RIGHT_ARM_UPPER_LOWER] = coneConstraint;
	world->addConstraint(joints[JOINT_RIGHT_ARM_UPPER_LOWER], true);

	// thorax-abdomen  	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(thoraxHeight/2 - 0.05), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, abdomenHeight/2 + 0.05, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_THORAX]->getRigidBody(), *bodyParts[BODYPART_ABDOMEN]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_THORAX_ADBOMEN] = coneConstraint;
	world->addConstraint(joints[JOINT_THORAX_ADBOMEN], true);

	// abdomen-pelvis   	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(abdomenHeight/2 - 0.05), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, pelvisHeight/2 + 0.05, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_ABDOMEN]->getRigidBody(), *bodyParts[BODYPART_PELVIS]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_ABDOMEN_PELVIS] = coneConstraint;
	world->addConstraint(joints[JOINT_ABDOMEN_PELVIS], true);

	// pelvis-leftthigh	   	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(pelvisHeight / 2), -(pelvisWidth/2 - thighRadius)));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, thighHeight/2 + thighRadius, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_PELVIS]->getRigidBody(), *bodyParts[BODYPART_LEFT_THIGH]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_PELVIS_LEFT_THIGH] = coneConstraint;
	world->addConstraint(joints[JOINT_PELVIS_LEFT_THIGH], true);

	// left thigh-leg 	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(thighHeight / 2 + thighRadius), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, legHeight / 2 + legRadius, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_LEFT_THIGH]->getRigidBody(), *bodyParts[BODYPART_LEFT_LEG]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_LEFT_THIGH_LEG] = coneConstraint;
	world->addConstraint(joints[JOINT_LEFT_THIGH_LEG], true);

	// left leg-foot    																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(legHeight / 2 + legRadius), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(-footXOffset, footHeight + 0.03, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_LEFT_LEG]->getRigidBody(), *bodyParts[BODYPART_LEFT_FOOT]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_LEFT_LEG_FOOT] = coneConstraint;
	world->addConstraint(joints[JOINT_LEFT_LEG_FOOT], true);


	// pelvis-rightthigh	   	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(pelvisHeight / 2), (pelvisWidth / 2 - thighRadius)));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, thighHeight / 2 + thighRadius, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_PELVIS]->getRigidBody(), *bodyParts[BODYPART_RIGHT_THIGH]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_PELVIS_RIGHT_THIGH] = coneConstraint;
	world->addConstraint(joints[JOINT_PELVIS_RIGHT_THIGH], true);

	// RIGHT thigh-leg 	 																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(thighHeight / 2 + thighRadius), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(0, legHeight / 2 + legRadius, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_RIGHT_THIGH]->getRigidBody(), *bodyParts[BODYPART_RIGHT_LEG]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_RIGHT_THIGH_LEG] = coneConstraint;
	world->addConstraint(joints[JOINT_RIGHT_THIGH_LEG], true);

	// RIGHT leg-foot    																		  
	bodyA.setIdentity();
	bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyA.setOrigin(btVector3(0, -(legHeight / 2 + legRadius), 0));
	bodyB.setIdentity();
	bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
	bodyB.setOrigin(btVector3(-footXOffset, footHeight + 0.03, 0));
	coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_RIGHT_LEG]->getRigidBody(), *bodyParts[BODYPART_RIGHT_FOOT]->getRigidBody(), bodyA, bodyB);
	coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
	joints[JOINT_RIGHT_LEG_FOOT] = coneConstraint;
	world->addConstraint(joints[JOINT_RIGHT_LEG_FOOT], true);

}
コード例 #12
0
void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
{
	m_swingCorrection = btScalar(0.);
	m_twistLimitSign = btScalar(0.);
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;
	// compute rotation of A wrt B (in constraint space)
	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
	{	// it is assumed that setMotorTarget() was alredy called 
		// and motor target m_qTarget is within constraint limits
		// TODO : split rotation to pure swing and pure twist
		// compute desired transforms in world
		btTransform trPose(m_qTarget);
		btTransform trA = transA * m_rbAFrame;
		btTransform trB = transB * m_rbBFrame;
		btTransform trDeltaAB = trB * trPose * trA.inverse();
		btQuaternion qDeltaAB = trDeltaAB.getRotation();
		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
		float swingAxisLen2 = swingAxis.length2();
		if(btFuzzyZero(swingAxisLen2))
		{
		   return;
		}
		m_swingAxis = swingAxis;
		m_swingAxis.normalize();
		m_swingCorrection = qDeltaAB.getAngle();
		if(!btFuzzyZero(m_swingCorrection))
		{
			m_solveSwingLimit = true;
		}
		return;
	}


	{
		// compute rotation of A wrt B (in constraint space)
		btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
		btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
		btQuaternion qAB = qB.inverse() * qA;
		// split rotation into cone and twist
		// (all this is done from B's perspective. Maybe I should be averaging axes...)
		btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
		btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
		btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();

		if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
		{
			btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
			computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);

			if (swingAngle > swingLimit * m_limitSoftness)
			{
				m_solveSwingLimit = true;

				// compute limit ratio: 0->1, where
				// 0 == beginning of soft limit
				// 1 == hard/real limit
				m_swingLimitRatio = 1.f;
				if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
				{
					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
										(swingLimit - swingLimit * m_limitSoftness);
				}				

				// swing correction tries to get back to soft limit
				m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);

				// adjustment of swing axis (based on ellipse normal)
				adjustSwingAxisToUseEllipseNormal(swingAxis);

				// Calculate necessary axis & factors		
				m_swingAxis = quatRotate(qB, -swingAxis);

				m_twistAxisA.setValue(0,0,0);

				m_kSwing =  btScalar(1.) /
					(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
					 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
			}
		}
		else
		{
			// you haven't set any limits;
			// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
			// anyway, we have either hinge or fixed joint
			btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
			btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
			btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
			btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
			btVector3 target;
			btScalar x = ivB.dot(ivA);
			btScalar y = ivB.dot(jvA);
			btScalar z = ivB.dot(kvA);
			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
			{ // fixed. We'll need to add one more row to constraint
				if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
				{
					m_solveSwingLimit = true;
					m_swingAxis = -ivB.cross(ivA);
				}
			}
			else
			{
				if(m_swingSpan1 < m_fixThresh)
				{ // hinge around Y axis
					if(!(btFuzzyZero(y)))
					{
						m_solveSwingLimit = true;
						if(m_swingSpan2 >= m_fixThresh)
						{
							y = btScalar(0.f);
							btScalar span2 = btAtan2(z, x);
							if(span2 > m_swingSpan2)
							{
								x = btCos(m_swingSpan2);
								z = btSin(m_swingSpan2);
							}
							else if(span2 < -m_swingSpan2)
							{
								x =  btCos(m_swingSpan2);
								z = -btSin(m_swingSpan2);
							}
						}
					}
				}
				else
				{ // hinge around Z axis
					if(!btFuzzyZero(z))
					{
						m_solveSwingLimit = true;
						if(m_swingSpan1 >= m_fixThresh)
						{
							z = btScalar(0.f);
							btScalar span1 = btAtan2(y, x);
							if(span1 > m_swingSpan1)
							{
								x = btCos(m_swingSpan1);
								y = btSin(m_swingSpan1);
							}
							else if(span1 < -m_swingSpan1)
							{
								x =  btCos(m_swingSpan1);
								y = -btSin(m_swingSpan1);
							}
						}
					}
				}
				target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
				target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
				target.normalize();
				m_swingAxis = -ivB.cross(target);
				m_swingCorrection = m_swingAxis.length();
				m_swingAxis.normalize();
			}
		}

		if (m_twistSpan >= btScalar(0.f))
		{
			btVector3 twistAxis;
			computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);

			if (m_twistAngle > m_twistSpan*m_limitSoftness)
			{
				m_solveTwistLimit = true;

				m_twistLimitRatio = 1.f;
				if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
				{
					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
										(m_twistSpan  - m_twistSpan * m_limitSoftness);
				}

				// twist correction tries to get back to soft limit
				m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);

				m_twistAxis = quatRotate(qB, -twistAxis);

				m_kTwist = btScalar(1.) /
					(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
					 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
			}

			if (m_solveSwingLimit)
				m_twistAxisA = quatRotate(qA, -twistAxis);
		}
		else
		{
			m_twistAngle = btScalar(0.f);
		}
	}
}
コード例 #13
0
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
	bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
	btScalar dbgDrawSize = constraint->getDbgDrawSize();
	if(dbgDrawSize <= btScalar(0.f))
	{
		return;
	}

	switch(constraint->getConstraintType())
	{
		case POINT2POINT_CONSTRAINT_TYPE:
			{
				btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
				btTransform tr;
				tr.setIdentity();
				btVector3 pivot = p2pC->getPivotInA();
				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
				tr.setOrigin(pivot);
				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				// that ideally should draw the same frame	
				pivot = p2pC->getPivotInB();
				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
				tr.setOrigin(pivot);
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
			}
			break;
		case HINGE_CONSTRAINT_TYPE:
			{
				btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
				btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				btScalar minAng = pHinge->getLowerLimit();
				btScalar maxAng = pHinge->getUpperLimit();
				if(minAng == maxAng)
				{
					break;
				}
				bool drawSect = true;
				if(minAng > maxAng)
				{
					minAng = btScalar(0.f);
					maxAng = SIMD_2_PI;
					drawSect = false;
				}
				if(drawLimits) 
				{
					btVector3& center = tr.getOrigin();
					btVector3 normal = tr.getBasis().getColumn(2);
					btVector3 axis = tr.getBasis().getColumn(0);
					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
				}
			}
			break;
		case CONETWIST_CONSTRAINT_TYPE:
			{
				btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
				btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				if(drawLimits)
				{
					//const btScalar length = btScalar(5);
					const btScalar length = dbgDrawSize;
					static int nSegments = 8*4;
					btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
					btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
					pPrev = tr * pPrev;
					for (int i=0; i<nSegments; i++)
					{
						fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
						btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
						pCur = tr * pCur;
						getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));

						if (i%(nSegments/8) == 0)
							getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));

						pPrev = pCur;
					}						
					btScalar tws = pCT->getTwistSpan();
					btScalar twa = pCT->getTwistAngle();
					bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
					if(useFrameB)
					{
						tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
					}
					else
					{
						tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
					}
					btVector3 pivot = tr.getOrigin();
					btVector3 normal = tr.getBasis().getColumn(0);
					btVector3 axis1 = tr.getBasis().getColumn(1);
					getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);

				}
			}
			break;
		case D6_CONSTRAINT_TYPE:
			{
				btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
				btTransform tr = p6DOF->getCalculatedTransformA();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				tr = p6DOF->getCalculatedTransformB();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				if(drawLimits) 
				{
					tr = p6DOF->getCalculatedTransformA();
					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
					btVector3 up = tr.getBasis().getColumn(2);
					btVector3 axis = tr.getBasis().getColumn(0);
					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
					axis = tr.getBasis().getColumn(1);
					btScalar ay = p6DOF->getAngle(1);
					btScalar az = p6DOF->getAngle(2);
					btScalar cy = btCos(ay);
					btScalar sy = btSin(ay);
					btScalar cz = btCos(az);
					btScalar sz = btSin(az);
					btVector3 ref;
					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
					ref[1] = -sz*axis[0] + cz*axis[1];
					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
					tr = p6DOF->getCalculatedTransformB();
					btVector3 normal = -tr.getBasis().getColumn(0);
					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
					if(minFi > maxFi)
					{
						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
					}
					else if(minFi < maxFi)
					{
						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
					}
					tr = p6DOF->getCalculatedTransformA();
					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
				}
			}
			break;
		case SLIDER_CONSTRAINT_TYPE:
			{
				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
				btTransform tr = pSlider->getCalculatedTransformA();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				tr = pSlider->getCalculatedTransformB();
				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
				if(drawLimits)
				{
					btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
					btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
					btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
					getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
					btVector3 normal = tr.getBasis().getColumn(0);
					btVector3 axis = tr.getBasis().getColumn(1);
					btScalar a_min = pSlider->getLowerAngLimit();
					btScalar a_max = pSlider->getUpperAngLimit();
					const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
				}
			}
			break;
		default : 
			break;
	}
	return;
}
コード例 #14
0
ファイル: GraspTest.cpp プロジェクト: benkehoe/gnrr
void GraspTest::clientMoveAndDisplay()
{
	
 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 	float dt = float(getDeltaTimeMicroseconds()) * 0.000001f;
	//printf("dt = %f: ",dt);

	// drive cone-twist motor
	m_Time += 0.03f;
	if (s_bTestConeTwistMotor)
	{  // this works only for obsolete constraint solver for now
		// build cone target
		btScalar t = 1.25f*m_Time;
		btVector3 axis(0,sin(t),cos(t));
		axis.normalize();
		btQuaternion q1(axis, 0.75f*SIMD_PI);

		// build twist target
		//btQuaternion q2(0,0,0);
		//btQuaternion q2(btVehictor3(1,0,0), -0.3*sin(m_Time));
		btQuaternion q2(btVector3(1,0,0), -1.49f*btSin(1.5f*m_Time));

		// compose cone + twist and set target
		q1 = q1 * q2;
		m_ctc->enableMotor(true);
		m_ctc->setMotorTargetInConstraintSpace(q1);
	}
	
	if (c1w) {
		c1w->setLinearLowerLimit(btVector3(0., 0.5 * sin(m_Time), 0.));
		c1w->setLinearUpperLimit(btVector3(0., 0.5 * sin(m_Time), 0.));
	}
	
	if (p2p) {
		btVector3 pt = finger22->getCenterOfMassTransform() * btVector3(scale*0.058,0,0);
		//printf("%f %f %f\n",pt.getX(), pt.getY(), pt.getZ());
		//printf("%f %f %f\n",p2p->getRelativePivotPosition(0),p2p->getRelativePivotPosition(1),p2p->getRelativePivotPosition(2));
	}
	
	bool mimic = false;
	
	if (mimic) {	
		if (j_00_01_jf1 && j_01_02_jf1mimic && true) {
			j_00_01_jf1->calculateTransforms();
			btTransform tfA = j_00_01_jf1->getCalculatedTransformA();
			btTransform tfB = j_00_01_jf1->getCalculatedTransformB();
			btTransform tfAB = tfB.inverseTimes(tfA);
			btScalar angle = tfAB.getRotation().getAngle();
		
			btScalar mimicAngle = angle / 3. + 0.8727;
		
			j_01_02_jf1mimic->setLimit(5,mimicAngle,mimicAngle);
		}
	
		if (j_10_11_jf2 && j_11_12_jf2mimic && true) {
			j_10_11_jf2->calculateTransforms();
			btTransform tfA = j_10_11_jf2->getCalculatedTransformA();
			btTransform tfB = j_10_11_jf2->getCalculatedTransformB();
			btTransform tfAB = tfB.inverseTimes(tfA);
			btScalar angle = tfAB.getRotation().getAngle();
		
			btScalar mimicAngle = angle / 3. + 0.8727;
		
			j_11_12_jf2mimic->setLimit(5,mimicAngle,mimicAngle);
		}
	
		if (j_20_21_jf3 && j_21_22_jf3mimic && true) {
			j_20_21_jf3->calculateTransforms();
			btTransform tfA = j_20_21_jf3->getCalculatedTransformA();
			btTransform tfB = j_20_21_jf3->getCalculatedTransformB();
			btTransform tfAB = tfB.inverseTimes(tfA);
			btScalar angle = tfAB.getRotation().getAngle();
		
			btScalar mimicAngle = angle / 3. + 0.8727;
		
			j_21_22_jf3mimic->setLimit(5,mimicAngle,mimicAngle);
		}
		
		if (j_hb_00_jf4 && j_hb_10_jf4mimic && false) {
			j_hb_00_jf4->calculateTransforms();
			btTransform tfA = j_hb_00_jf4->getCalculatedTransformA();
			btTransform tfB = j_hb_00_jf4->getCalculatedTransformB();
			btTransform tfAB = tfB.inverseTimes(tfA);
			btScalar angle = tfAB.getRotation().getAngle();
		
			btScalar mimicAngle = SIMD_PI - angle;
		
			j_hb_10_jf4mimic->setLimit(5,mimicAngle,mimicAngle);
		}
	}

	{
		static bool once = true;
		if ( m_dynamicsWorld->getDebugDrawer() && once)
		{
			m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
			once=false;
		}
	}

	
	{
	 	//during idle mode, just run 1 simulation step maximum
		int maxSimSubSteps = m_idle ? 1 : 1;
		if (m_idle)
			dt = 1.0f/420.f / 10;


		int numSimSteps = 1;
		if (m_Time > 10) {
			numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
		}

		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	
		bool verbose = false;
		if (verbose)
		{
			if (!numSimSteps)
				printf("Interpolated transforms\n");
			else
			{
				if (numSimSteps > maxSimSubSteps)
				{
					//detect dropping frames
					printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
				} else
				{
					printf("Simulated (%i) steps\n",numSimSteps);
				}
			}
		}
	}
	renderme();

//	drawLimit();

    glFlush();
    swapBuffers();
}
コード例 #15
0
void ConstraintDemo::clientMoveAndDisplay()
{
	
 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

 	float dt = float(getDeltaTimeMicroseconds()) * 0.000001f;
	//printf("dt = %f: ",dt);

	// drive cone-twist motor
	m_Time += 0.03f;
	if (s_bTestConeTwistMotor)
	{  // this works only for obsolete constraint solver for now
		// build cone target
		btScalar t = 1.25f*m_Time;
		btVector3 axis(0,sin(t),cos(t));
		axis.normalize();
		btQuaternion q1(axis, 0.75f*SIMD_PI);

		// build twist target
		//btQuaternion q2(0,0,0);
		//btQuaternion q2(btVehictor3(1,0,0), -0.3*sin(m_Time));
		btQuaternion q2(btVector3(1,0,0), -1.49f*btSin(1.5f*m_Time));

		// compose cone + twist and set target
		q1 = q1 * q2;
		m_ctc->enableMotor(true);
		m_ctc->setMotorTargetInConstraintSpace(q1);
	}

	{
		static bool once = true;
		if ( m_dynamicsWorld->getDebugDrawer() && once)
		{
			m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
			once=false;
		}
	}

	
	{
	 	//during idle mode, just run 1 simulation step maximum
		int maxSimSubSteps = m_idle ? 1 : 1;
		if (m_idle)
			dt = 1.0f/420.f;

		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);

		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	
		bool verbose = false;
		if (verbose)
		{
			if (!numSimSteps)
				printf("Interpolated transforms\n");
			else
			{
				if (numSimSteps > maxSimSubSteps)
				{
					//detect dropping frames
					printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
				} else
				{
					printf("Simulated (%i) steps\n",numSimSteps);
				}
			}
		}
	}
	renderme();

//	drawLimit();

    glFlush();
    swapBuffers();
}
コード例 #16
0
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
{

	
	GLInstanceGraphicsShape* glmesh = 0;

	btConvexShape* convexColShape = 0;

	switch (visual->m_geometry.m_type)
	{
		case URDF_GEOM_CYLINDER:
		{
			btAlignedObjectArray<btVector3> vertices;
		
			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
			int numSteps = 32;
			for (int i = 0; i<numSteps; i++)
			{

				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
				btScalar cylLength = visual->m_geometry.m_cylinderLength;
				
				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
				vertices.push_back(vert);
				vert[2] = -cylLength / 2.;
				vertices.push_back(vert);
			}

			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
			cylZShape->setMargin(0.001);
			convexColShape = cylZShape;
			break;
		}
		case URDF_GEOM_BOX:
		{
			
			btVector3 extents = visual->m_geometry.m_boxSize;
			
			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
			convexColShape = boxShape;
			convexColShape->setMargin(0.001);
			break;
		}
		case URDF_GEOM_SPHERE:
		{
			btScalar radius = visual->m_geometry.m_sphereRadius;
			btSphereShape* sphereShape = new btSphereShape(radius);
			convexColShape = sphereShape;
			convexColShape->setMargin(0.001);
			break;

			break;
		}
		case URDF_GEOM_MESH:
		{
			if (visual->m_name.length())
			{
				//b3Printf("visual->name=%s\n", visual->m_name.c_str());
			}
			if (1)//visual->m_geometry)
			{
				if (visual->m_geometry.m_meshFileName.length())
				{
					const char* filename = visual->m_geometry.m_meshFileName.c_str();
					//b3Printf("mesh->filename=%s\n", filename);
					char fullPath[1024];
					int fileType = 0;
                    
                    char tmpPathPrefix[1024];
                    std::string xml_string;
                    int maxPathLen = 1024;
                    b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
                   
                    char visualPathPrefix[1024];
                    sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
                    
                    
					sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
					b3FileUtils::toLower(fullPath);
					if (strstr(fullPath, ".dae"))
					{
						fileType = MY_FILE_COLLADA;
					}
					if (strstr(fullPath, ".stl"))
					{
						fileType = MY_FILE_STL;
					}
                    if (strstr(fullPath,".obj"))
                    {
                        fileType = MY_FILE_OBJ;
                    }


					sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
					FILE* f = fopen(fullPath, "rb");
					if (f)
					{
						fclose(f);
						


						switch (fileType)
						{
                            case MY_FILE_OBJ:
                            {
                                //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
								b3ImportMeshData meshData;
								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
								{
									
									if (meshData.m_textureImage)
									{
										MyTexture2 texData;
										texData.m_width = meshData.m_textureWidth;
										texData.m_height = meshData.m_textureHeight;
										texData.textureData = meshData.m_textureImage;
										texturesOut.push_back(texData);
									}
									glmesh = meshData.m_gfxShape;
								}

								
                                break;
                            }
                           
						case MY_FILE_STL:
						{
							glmesh = LoadMeshFromSTL(fullPath);
							break;
						}
						case MY_FILE_COLLADA:
						{

							btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
							btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
							btTransform upAxisTrans; upAxisTrans.setIdentity();
							float unitMeterScaling = 1;
							int upAxis = 2;

							LoadMeshFromCollada(fullPath,
								visualShapes,
								visualShapeInstances,
								upAxisTrans,
								unitMeterScaling,
												upAxis);

							glmesh = new GLInstanceGraphicsShape;
					//		int index = 0;
							glmesh->m_indices = new b3AlignedObjectArray<int>();
							glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

							for (int i = 0; i<visualShapeInstances.size(); i++)
							{
								ColladaGraphicsInstance* instance = &visualShapeInstances[i];
								GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];

								b3AlignedObjectArray<GLInstanceVertex> verts;
								verts.resize(gfxShape->m_vertices->size());

								int baseIndex = glmesh->m_vertices->size();

								for (int i = 0; i<gfxShape->m_vertices->size(); i++)
								{
									verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
									verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
									verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
									verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
									verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
									verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
									verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
									verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
									verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];

								}

								int curNumIndices = glmesh->m_indices->size();
								int additionalIndices = gfxShape->m_indices->size();
								glmesh->m_indices->resize(curNumIndices + additionalIndices);
								for (int k = 0; k<additionalIndices; k++)
								{
									glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
								}

								//compensate upAxisTrans and unitMeterScaling here
								btMatrix4x4 upAxisMat;
								upAxisMat.setIdentity();
//								upAxisMat.setPureRotation(upAxisTrans.getRotation());
								btMatrix4x4 unitMeterScalingMat;
								unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
								btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
								//btMatrix4x4 worldMat = instance->m_worldTransform;
								int curNumVertices = glmesh->m_vertices->size();
								int additionalVertices = verts.size();
								glmesh->m_vertices->reserve(curNumVertices + additionalVertices);

								for (int v = 0; v<verts.size(); v++)
								{
									btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
									pos = worldMat*pos;
									verts[v].xyzw[0] = float(pos[0]);
									verts[v].xyzw[1] = float(pos[1]);
									verts[v].xyzw[2] = float(pos[2]);
									glmesh->m_vertices->push_back(verts[v]);
								}
							}
							glmesh->m_numIndices = glmesh->m_indices->size();
							glmesh->m_numvertices = glmesh->m_vertices->size();
							//glmesh = LoadMeshFromCollada(fullPath);

							break;
						}
						default:
						{
                            b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
                            btAssert(0);
						}
						}


						if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
						{
						    //apply the geometry scaling
						    for (int i=0;i<glmesh->m_vertices->size();i++)
                            {
                                glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
                                glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
                                glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
                            }
						    
						}
						else
						{
							b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
						}

					}
					else
					{
						b3Warning("mesh geometry not found %s\n", fullPath);
					}


				}
			}


			break;
		}
		default:
		{
			b3Warning("Error: unknown visual geometry type\n");
		}
	}

	//if we have a convex, tesselate into localVertices/localIndices
	if ((glmesh==0) && convexColShape)
	{
		btShapeHull* hull = new btShapeHull(convexColShape);
		hull->buildHull(0.0);
		{
			//	int strideInBytes = 9*sizeof(float);
			int numVertices = hull->numVertices();
			int numIndices = hull->numIndices();

			
			glmesh = new GLInstanceGraphicsShape;
		//	int index = 0;
			glmesh->m_indices = new b3AlignedObjectArray<int>();
			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();


			for (int i = 0; i < numVertices; i++)
			{
				GLInstanceVertex vtx;
				btVector3 pos = hull->getVertexPointer()[i];
				vtx.xyzw[0] = pos.x();
				vtx.xyzw[1] = pos.y();
				vtx.xyzw[2] = pos.z();
				vtx.xyzw[3] = 1.f;
				pos.normalize();
				vtx.normal[0] = pos.x();
				vtx.normal[1] = pos.y();
				vtx.normal[2] = pos.z();
				vtx.uv[0] = 0.5f;
				vtx.uv[1] = 0.5f;
				glmesh->m_vertices->push_back(vtx);
			}

			btAlignedObjectArray<int> indices;
			for (int i = 0; i < numIndices; i++)
			{
				glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
			}
			
			glmesh->m_numvertices = glmesh->m_vertices->size();
			glmesh->m_numIndices = glmesh->m_indices->size();
		}
        delete hull;
		delete convexColShape;
		convexColShape = 0;

	}
	
	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
	{

		int baseIndex = verticesOut.size();



		for (int i = 0; i < glmesh->m_indices->size(); i++)
		{
			indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
		}

		for (int i = 0; i < glmesh->m_vertices->size(); i++)
		{
			GLInstanceVertex& v = glmesh->m_vertices->at(i);
			btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
			btVector3 vt = visualTransform*vert;
			v.xyzw[0] = vt[0];
			v.xyzw[1] = vt[1];
			v.xyzw[2] = vt[2];
			btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
			triNormal = visualTransform.getBasis()*triNormal;
			v.normal[0] = triNormal[0];
			v.normal[1] = triNormal[1];
			v.normal[2] = triNormal[2];
			verticesOut.push_back(v);
		}
	}
    delete glmesh;
    
}
コード例 #17
0
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
{
	btCollisionShape* shape = 0;

    switch (collision->m_geometry.m_type)
    {
        case URDF_GEOM_CYLINDER:
        {
			btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
			btScalar cylLength = collision->m_geometry.m_cylinderLength;
			
            btAlignedObjectArray<btVector3> vertices;
            //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
            int numSteps = 32;
            for (int i=0;i<numSteps;i++)
            {

                btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.);
                vertices.push_back(vert);
                vert[2] = -cylLength/2.;
                vertices.push_back(vert);

            }
            btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
            cylZShape->setMargin(0.001);
			cylZShape->initializePolyhedralFeatures();
			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
            
            //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
            //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
            

            shape = cylZShape;
            break;
        }
        case URDF_GEOM_BOX:
        {
			btVector3 extents = collision->m_geometry.m_boxSize;
			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
            shape = boxShape;
			shape ->setMargin(0.001);
            break;
        }
        case URDF_GEOM_SPHERE:
        {
            
			btScalar radius = collision->m_geometry.m_sphereRadius;
			btSphereShape* sphereShape = new btSphereShape(radius);
            shape = sphereShape;
			shape ->setMargin(0.001);
            break;

            break;
        }
        case URDF_GEOM_MESH:
        {
			if (collision->m_name.length())
			{
				//b3Printf("collision->name=%s\n",collision->m_name.c_str());
			}
			if (1)
			{
				if (collision->m_geometry.m_meshFileName.length())
				{
					const char* filename = collision->m_geometry.m_meshFileName.c_str();
					//b3Printf("mesh->filename=%s\n",filename);
					char fullPath[1024];
					int fileType = 0;
					sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
					b3FileUtils::toLower(fullPath);
                    char tmpPathPrefix[1024];
                    int maxPathLen = 1024;
                    b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
                    
                    char collisionPathPrefix[1024];
                    sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
                    
                    
                    
					if (strstr(fullPath,".dae"))
					{
						fileType = FILE_COLLADA;
					}
					if (strstr(fullPath,".stl"))
					{
						fileType = FILE_STL;
					}
                    if (strstr(fullPath,".obj"))
                   {
                       fileType = FILE_OBJ;
                   }

					sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
					FILE* f = fopen(fullPath,"rb");
					if (f)
					{
						fclose(f);
						GLInstanceGraphicsShape* glmesh = 0;
						
						
						switch (fileType)
						{
                            case FILE_OBJ:
                            {
                                glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
                                break;
                            }
						case FILE_STL:
							{
								glmesh = LoadMeshFromSTL(fullPath);
							break;
							}
						case FILE_COLLADA:
							{
								
								btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
								btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
								btTransform upAxisTrans;upAxisTrans.setIdentity();
								float unitMeterScaling=1;
								int upAxis = 2;
								LoadMeshFromCollada(fullPath,
													visualShapes, 
													visualShapeInstances,
													upAxisTrans,
													unitMeterScaling,
													upAxis );
								
								glmesh = new GLInstanceGraphicsShape;
						//		int index = 0;
								glmesh->m_indices = new b3AlignedObjectArray<int>();
								glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

								for (int i=0;i<visualShapeInstances.size();i++)
								{
									ColladaGraphicsInstance* instance = &visualShapeInstances[i];
									GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
		
									b3AlignedObjectArray<GLInstanceVertex> verts;
									verts.resize(gfxShape->m_vertices->size());

									int baseIndex = glmesh->m_vertices->size();

									for (int i=0;i<gfxShape->m_vertices->size();i++)
									{
										verts[i].normal[0] = 	gfxShape->m_vertices->at(i).normal[0];
										verts[i].normal[1] = 	gfxShape->m_vertices->at(i).normal[1];
										verts[i].normal[2] = 	gfxShape->m_vertices->at(i).normal[2];
										verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
										verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
										verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
										verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
										verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
										verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];

									}

									int curNumIndices = glmesh->m_indices->size();
									int additionalIndices = gfxShape->m_indices->size();
									glmesh->m_indices->resize(curNumIndices+additionalIndices);
									for (int k=0;k<additionalIndices;k++)
									{
										glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
									}
			
									//compensate upAxisTrans and unitMeterScaling here
									btMatrix4x4 upAxisMat;
                                    upAxisMat.setIdentity();
									//upAxisMat.setPureRotation(upAxisTrans.getRotation());
									btMatrix4x4 unitMeterScalingMat;
									unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
									btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
									//btMatrix4x4 worldMat = instance->m_worldTransform;
									int curNumVertices = glmesh->m_vertices->size();
									int additionalVertices = verts.size();
									glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
									
									for(int v=0;v<verts.size();v++)
									{
										btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
										pos = worldMat*pos;
										verts[v].xyzw[0] = float(pos[0]);
										verts[v].xyzw[1] = float(pos[1]);
										verts[v].xyzw[2] = float(pos[2]);
										glmesh->m_vertices->push_back(verts[v]);
									}
								}
								glmesh->m_numIndices = glmesh->m_indices->size();
								glmesh->m_numvertices = glmesh->m_vertices->size();
								//glmesh = LoadMeshFromCollada(fullPath);

								break;
							}
						default:
							{
                                b3Warning("Unsupported file type in Collision: %s\n",fullPath);
                                btAssert(0);
							}
						}
					

						if (glmesh && (glmesh->m_numvertices>0))
						{
							//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
							//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
							//convex->setUserIndex(shapeId);
							btAlignedObjectArray<btVector3> convertedVerts;
							convertedVerts.reserve(glmesh->m_numvertices);
							for (int i=0;i<glmesh->m_numvertices;i++)
							{
								convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
							}
							//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
							btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
							//cylZShape->initializePolyhedralFeatures();
							//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
							//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
							cylZShape->setMargin(0.001);
							shape = cylZShape;
						} else
						{
							b3Warning("issue extracting mesh from STL file %s\n", fullPath);
						}

                        delete glmesh;
                       
					} else
					{
						b3Warning("mesh geometry not found %s\n",fullPath);
					}
							
				}
			}

					
            break;
        }
        default:
        {
            b3Warning("Error: unknown visual geometry type\n");
        }
    }
	return shape;
}