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);
}
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;
}
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);
}
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()));
}
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;
}
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;
}
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);
}
}
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;
}
}
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;
}
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;
}
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);
}
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);
}
}
}
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;
}
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();
}
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();
}
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;
}
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;
}