RagdollDemo::RagdollDemo(hkDemoEnvironment* env)
: hkDefaultPhysicsDemo(env)
{
//
// Setup the camera
//
{
hkVector4 from(5.0f, 2.0f, 5.0f);
hkVector4 to(0.0f, 0.0f, 0.0f);
hkVector4 up(0.0f, 1.0f, 0.0f);
setupDefaultCameras( env, from, to, up );
}
//
// Create the world
//
{
hkpWorldCinfo info;
info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
info.m_enableDeactivation = false;
m_world = new hkpWorld( info );
m_world->lock();
//
// Create constraint viewer
//
m_debugViewerNames.pushBack( hkpConstraintViewer::getName() );
hkpConstraintViewer::m_scale = 1.0f;
setupGraphics();
// Register the single agent required (a hkpBoxBoxAgent)
hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
}
hkVector4 pivot(0.0f, 0.0f, 0.0f);
hkVector4 halfSize(1.0f, 0.25f, 0.5f);
// Create Box Shapes
//
hkpBoxShape* boxShape;
{
boxShape = new hkpBoxShape( halfSize , 0 );
}
//
// Create fixed rigid body
//
hkpRigidBody* fixedBody;
{
hkpRigidBodyCinfo info;
info.m_position.set(-halfSize(0) - 1.0f, 0.0f, 0.0f);
info.m_shape = boxShape;
info.m_motionType = hkpMotion::MOTION_FIXED;
fixedBody = new hkpRigidBody(info);
m_world->addEntity(fixedBody);
fixedBody->removeReference();
}
//
// Create movable rigid body
//
hkpRigidBody* moveableBody;
{
hkpRigidBodyCinfo info;
info.m_position.set(halfSize(0) + 1.0f, 0.0f, 0.0f);
info.m_shape = boxShape;
// Compute the box inertia tensor
hkpMassProperties massProperties;
info.m_mass = 10.0f;
hkpInertiaTensorComputer::computeBoxVolumeMassProperties(halfSize, info.m_mass, massProperties);
info.m_inertiaTensor = massProperties.m_inertiaTensor;
info.m_centerOfMass = massProperties.m_centerOfMass;
info.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
moveableBody = new hkpRigidBody(info);
m_world->addEntity(moveableBody);
moveableBody->removeReference();
}
//
// Cleanup shape references
//
boxShape->removeReference();
boxShape = HK_NULL;
//
// CREATE RAGDOLL CONSTRAINT
//
{
hkReal planeMin = HK_REAL_PI * -0.2f;
hkReal planeMax = HK_REAL_PI * 0.1f;
hkReal twistMin = HK_REAL_PI * -0.1f;
hkReal twistMax = HK_REAL_PI * 0.4f;
hkReal coneMin = HK_REAL_PI * 0.3f;
hkpRagdollConstraintData* rdc = new hkpRagdollConstraintData();
rdc->setConeAngularLimit(coneMin);
rdc->setPlaneMinAngularLimit(planeMin);
rdc->setPlaneMaxAngularLimit(planeMax);
rdc->setTwistMinAngularLimit(twistMin);
rdc->setTwistMaxAngularLimit(twistMax);
hkVector4 twistAxis(1.0f, 0.0f, 0.0f);
hkVector4 planeAxis(0.0f, 1.0f, 0.0f);
pivot.set(0.0f, 0.0f, 0.0f);
rdc->setInWorldSpace(moveableBody->getTransform(), fixedBody->getTransform(), pivot, twistAxis, planeAxis);
//
// Create and add the constraint
//
{
hkpConstraintInstance* constraint = new hkpConstraintInstance(moveableBody, fixedBody, rdc );
m_world->addConstraint(constraint);
constraint->removeReference();
}
rdc->removeReference();
}
m_world->unlock();
}
void PrevailingWindDemo::createPalmTree ( hkpWorld* world, const hkpWind* wind, const hkVector4& pos )
{
const hkReal trunkHeight = 4.0f;
const hkReal trunkBottomRadius = 0.5f;
const hkReal trunkTopRadius = 0.2f;
const hkReal trunkStiffness = 0.1f;
const hkReal segmentMass = 0.6f;
const int numberOfSegments = 4;
const hkReal segmentGap = 0.2f;
const int numberOfFronds = 6;
const hkReal frondWidth = 2.0f;
const hkReal frondLength = 3.0f;
const hkReal frondMass = 0.4f;
// The trunk
hkArray<hkpRigidBody*> trunk;
const hkReal segmentHeight = (trunkHeight - ((numberOfSegments - 1) * segmentGap)) / numberOfSegments;
const hkReal radiusIncrement = (trunkBottomRadius - trunkTopRadius) / numberOfSegments;
for ( int i = 0; i < numberOfSegments; i++ )
{
hkpShape* segmentShape;
hkpRigidBodyCinfo info;
{
hkVector4 bottom( 0.0f, (segmentHeight + segmentGap) * i, 0.0f );
hkVector4 top( 0.0f, (segmentHeight + segmentGap) * i + segmentHeight, 0.0f );
hkReal radius = trunkBottomRadius - (radiusIncrement * i);
segmentShape = new hkpCylinderShape( bottom, top, radius, 0.03f );
info.m_shape = segmentShape;
info.m_position = pos;
if (i == 0)
{
info.m_motionType = hkpMotion::MOTION_FIXED;
}
else
{
hkpMassProperties massProperties;
{
hkpInertiaTensorComputer::computeCylinderVolumeMassProperties( bottom, top, radius, segmentMass, massProperties );
}
info.m_motionType = hkpMotion::MOTION_DYNAMIC;
info.m_mass = massProperties.m_mass;
info.m_inertiaTensor = massProperties.m_inertiaTensor;
info.m_centerOfMass = massProperties.m_centerOfMass;
}
}
hkpRigidBody* segment = new hkpRigidBody( info );
segmentShape->removeReference();
trunk.pushBack( segment );
world->addEntity( segment );
segment->removeReference();
if (i > 0)
{
hkpWindAction* action = new hkpWindAction( segment, wind, 0.1f );
world->addAction(action);
action->removeReference();
}
}
for ( int i = 1; i < numberOfSegments; i++ )
{
// We model the connection between the segments with a ragdoll constraint.
hkpRagdollConstraintData* rdc;
{
hkReal planeMin = HK_REAL_PI * -0.025f;
hkReal planeMax = HK_REAL_PI * 0.025f;
hkReal twistMin = HK_REAL_PI * -0.025f;
hkReal twistMax = HK_REAL_PI * 0.025f;
hkReal coneMin = HK_REAL_PI * -0.05f;
hkReal coneMax = HK_REAL_PI * 0.05f;
rdc = new hkpRagdollConstraintData();
rdc->setPlaneMinAngularLimit( planeMin );
rdc->setPlaneMaxAngularLimit( planeMax );
rdc->setTwistMinAngularLimit( twistMin );
rdc->setTwistMaxAngularLimit( twistMax );
hkVector4 twistAxis( 0.0f, 1.0f, 0.0f );
hkVector4 planeAxis( 0.0f, 0.0f, 1.0f );
hkVector4 pivot( 0.0f, (segmentHeight + segmentGap) * i, 0.0f );
rdc->setInBodySpace( pivot, pivot, planeAxis, planeAxis, twistAxis, twistAxis );
rdc->setAsymmetricConeAngle( coneMin, coneMax );
//world->createAndAddConstraintInstance( trunk[i - 1], trunk[i], rdc )->removeReference();
hkpConstraintInstance* constraint = new hkpConstraintInstance( trunk[i - 1], trunk[i], rdc );
world->addConstraint(constraint);
hkpPositionConstraintMotor* motor = new hkpPositionConstraintMotor( 0 );
motor->m_tau = trunkStiffness;
motor->m_maxForce = 1000.0f;
motor->m_constantRecoveryVelocity = 0.1f;
rdc->setTwistMotor( motor );
rdc->setConeMotor( motor );
rdc->setPlaneMotor( motor );
rdc->setMotorsActive(constraint, true);
motor->removeReference();
constraint->removeReference();
rdc->removeReference();
}
}
// The angle that the leaves make with the ground in their half lifted position.
hkQuaternion tilt;
{
hkVector4 axis( 0.0f, 0.0f, 1.0f );
tilt.setAxisAngle( axis, HK_REAL_PI * 0.1f );
}
hkQuaternion tiltRot;
// The fronds
for ( int i = 0; i < numberOfFronds; i++ )
{
hkQuaternion rotation;
{
hkVector4 axis( 0.0f, 1.0f, 0.0f );
rotation.setAxisAngle( axis, HK_REAL_PI * 2.0f * ( i / (hkReal) numberOfFronds ) );
rotation.normalize();
}
hkpShape* frondShape;
hkpRigidBodyCinfo info;
{
hkVector4 vertexA( 0.0f, 0.0f, 0.0f );
hkVector4 vertexB( frondLength, 0.0f, frondWidth / 2.0f );
hkVector4 vertexC( frondLength, 0.0f, - frondWidth / 2.0f );
frondShape = new hkpTriangleShape( vertexA, vertexB, vertexC, 0.01f );
info.m_shape = frondShape;
hkVector4 relPos;
relPos.setRotatedDir( rotation, hkVector4( trunkTopRadius + 0.3f, trunkHeight, 0.0f ) );
info.m_position.setAdd4( pos, relPos );
hkpMassProperties massProperties;
{
hkReal mass = frondMass;
hkpInertiaTensorComputer::computeTriangleSurfaceMassProperties( vertexA, vertexB, vertexC, mass, 0.01f, massProperties );
}
info.m_motionType = hkpMotion::MOTION_DYNAMIC;
info.m_mass = massProperties.m_mass;
info.m_inertiaTensor = massProperties.m_inertiaTensor;
info.m_centerOfMass = massProperties.m_centerOfMass;
tiltRot.setMul( rotation, tilt );
info.m_rotation = tiltRot;
}
hkpRigidBody* frond = new hkpRigidBody( info );
frondShape->removeReference();
world->addEntity( frond );
hkpWindAction* action = new hkpWindAction( frond, wind, 0.1f );
world->addAction(action);
action->removeReference();
// We model the connection between the fronds and the trunk with a ragdoll constraint.
hkpRagdollConstraintData* rdc;
{
hkReal planeMin = HK_REAL_PI * -0.005f;
hkReal planeMax = HK_REAL_PI * 0.005f;
hkReal twistMin = HK_REAL_PI * -0.05f;
hkReal twistMax = HK_REAL_PI * 0.05f;
hkReal coneMin = HK_REAL_PI * -0.2f;
hkReal coneMax = HK_REAL_PI * 0.2f;
rdc = new hkpRagdollConstraintData();
rdc->setPlaneMinAngularLimit( planeMin );
rdc->setPlaneMaxAngularLimit( planeMax );
rdc->setTwistMinAngularLimit( twistMin );
rdc->setTwistMaxAngularLimit( twistMax );
hkVector4 twistAxisFrond( 1.0f, 0.0f, 0.0f );
hkVector4 twistAxisTrunk;
twistAxisTrunk.setRotatedDir( tiltRot, twistAxisFrond );
hkVector4 planeAxisFrond( 0.0f, 0.0f, 1.0f );
hkVector4 planeAxisTrunk;
planeAxisTrunk.setRotatedDir( tiltRot, planeAxisFrond );
hkVector4 pivotFrond( 0.0f, 0.0f, 0.0f );
hkVector4 pivotTrunk;
pivotTrunk.setRotatedDir( rotation, hkVector4( trunkTopRadius + 0.3f, trunkHeight, 0.0f ) );
rdc->setInBodySpace( pivotTrunk, pivotFrond, planeAxisTrunk, planeAxisFrond, twistAxisTrunk, twistAxisFrond );
rdc->setAsymmetricConeAngle( coneMin, coneMax );
world->createAndAddConstraintInstance( trunk[ numberOfSegments - 1 ], frond, rdc )->removeReference();
rdc->removeReference();
frond->removeReference();
}
}
}
PoweredRagdollDemo::PoweredRagdollDemo(hkDemoEnvironment* env)
: hkDefaultPhysicsDemo(env)
{
m_time = 0.0f;
//
// Setup the camera
//
{
hkVector4 from(5.0f, 2.0f, 5.0f);
hkVector4 to(0.0f, 0.0f, 0.0f);
hkVector4 up(0.0f, 1.0f, 0.0f);
setupDefaultCameras( env, from, to, up );
}
//
// Create the world
//
{
hkpWorldCinfo info;
info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_8ITERS_MEDIUM);
info.m_enableDeactivation = false;
m_world = new hkpWorld( info );
m_world->lock();
//
// Create constraint viewer
//
m_debugViewerNames.pushBack( hkpConstraintViewer::getName() );
hkpConstraintViewer::m_scale = 1.0f;
setupGraphics();
// Register the single agent required (a hkpBoxBoxAgent)
hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
}
//
// Create vectors to be used for setup
//
hkVector4 pivot(0.0f, 0.0f, 0.0f);
hkVector4 halfSize(1.0f, 0.25f, 0.5f);
//
// Create Box Shape
//
hkpBoxShape* boxShape;
{
boxShape = new hkpBoxShape( halfSize , 0 );
}
//
// Create fixed rigid body
//
hkpRigidBody* fixedBody;
{
hkpRigidBodyCinfo info;
info.m_position.set(-halfSize(0) - 1.0f, 0.0f, 0.0f);
info.m_shape = boxShape;
info.m_motionType = hkpMotion::MOTION_FIXED;
fixedBody = new hkpRigidBody(info);
m_world->addEntity(fixedBody);
fixedBody->removeReference();
}
//
// Create movable rigid body
//
hkpRigidBody* moveableBody;
{
hkpRigidBodyCinfo info;
info.m_position.set(halfSize(0) + 1.0f, 0.0f, 0.0f);
info.m_shape = boxShape;
// Compute the box inertia tensor
hkpMassProperties massProperties;
info.m_mass = 10.0f;
hkpInertiaTensorComputer::computeBoxVolumeMassProperties(halfSize, info.m_mass, massProperties);
info.m_inertiaTensor = massProperties.m_inertiaTensor;
info.m_centerOfMass = massProperties.m_centerOfMass;
info.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
moveableBody = new hkpRigidBody(info);
m_world->addEntity(moveableBody);
moveableBody->removeReference();
}
//
// Cleanup shape references
//
boxShape->removeReference();
boxShape = HK_NULL;
//
// CREATE POWERED RAGDOLL CONSTRAINT
//
{
hkReal planeMin = HK_REAL_PI * -0.2f;
hkReal planeMax = HK_REAL_PI * 0.1f;
hkReal twistMin = HK_REAL_PI * -0.1f;
hkReal twistMax = HK_REAL_PI * 0.4f;
hkReal coneMin = HK_REAL_PI * 0.3f;
hkpRagdollConstraintData* rdc = new hkpRagdollConstraintData();
rdc->setConeAngularLimit(coneMin);
rdc->setPlaneMinAngularLimit(planeMin);
rdc->setPlaneMaxAngularLimit(planeMax);
rdc->setTwistMinAngularLimit(twistMin);
rdc->setTwistMaxAngularLimit(twistMax);
hkVector4 twistAxis(1.0f, 0.0f, 0.0f);
hkVector4 planeAxis(0.0f, 1.0f, 0.0f);
pivot.set(0.0f, 0.0f, 0.0f);
rdc->setInWorldSpace(moveableBody->getTransform(), fixedBody->getTransform(), pivot, twistAxis, planeAxis);
//
// Create and add the constraint
//
hkpConstraintInstance* constraint = new hkpConstraintInstance(moveableBody, fixedBody, rdc );
m_world->addConstraint(constraint);
hkpPositionConstraintMotor* motor = new hkpPositionConstraintMotor( 0 );
motor->m_tau = 0.6f;
motor->m_maxForce = 1000.0f;
motor->m_constantRecoveryVelocity = 0.1f;
rdc->setTwistMotor( motor );
rdc->setConeMotor( motor );
rdc->setPlaneMotor( motor );
rdc->setMotorsActive(constraint, true);
motor->removeReference();
hkRotation bRa;
bRa.setTranspose( fixedBody->getTransform().getRotation() );
bRa.mul( moveableBody->getTransform().getRotation() );
rdc->setTargetRelativeOrientationOfBodies( bRa );
m_constraintData = rdc;
constraint->removeReference();
}
//
// So that we can actually see the motor in action, rotate the constrained body slightly.
// When simulation starts it will then by driven to the target position (frame).
//
{
hkQuaternion rot;
hkVector4 axis( 1.0f, 0.0f, 0.0f );
axis.normalize3();
rot.setAxisAngle( axis, 0.4f );
moveableBody->setRotation( rot );
}
m_world->unlock();
}
void MakeKukaRobot(DemoEntityManager* const scene, DemoEntity* const model)
{
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
NewtonBody* const baseFrameBody = CreateBodyPart(model, armRobotConfig[0]);
void* const baseFrameNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, baseFrameBody);
NewtonBodySetMassMatrix(baseFrameBody, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix boneAligmentMatrix(
dVector(0.0f, 1.0f, 0.0f, 0.0f),
dVector(0.0f, 0.0f, 1.0f, 0.0f),
dVector(1.0f, 0.0f, 0.0f, 0.0f),
dVector(0.0f, 0.0f, 0.0f, 1.0f));
int stackIndex = 0;
DemoEntity* childEntities[32];
void* parentBones[32];
for (DemoEntity* child = model->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = baseFrameNode;
childEntities[stackIndex] = child;
stackIndex++;
}
dKukaEffector* effector = NULL;
const int partCount = sizeof(armRobotConfig) / sizeof(armRobotConfig[0]);
while (stackIndex) {
stackIndex--;
DemoEntity* const entity = childEntities[stackIndex];
void* const parentJoint = parentBones[stackIndex];
const char* const name = entity->GetName().GetStr();
for (int i = 0; i < partCount; i++) {
if (!strcmp(armRobotConfig[i].m_partName, name)) {
if (strstr(name, "bone")) {
// add a bone and all it children
dMatrix matrix;
NewtonBody* const limbBody = CreateBodyPart(entity, armRobotConfig[i]);
NewtonBodyGetMatrix(limbBody, &matrix[0][0]);
NewtonBody* const parentBody = NewtonInverseDynamicsGetBody(m_kinematicSolver, parentJoint);
dCustomInverseDynamics* const rotatingColumnHinge = new dCustomInverseDynamics(boneAligmentMatrix * matrix, limbBody, parentBody);
rotatingColumnHinge->SetJointTorque(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f);
rotatingColumnHinge->SetTwistAngle(armRobotConfig[i].m_minLimit * dDegreeToRad, armRobotConfig[i].m_maxLimit * dDegreeToRad);
void* const limbJoint = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, parentJoint, rotatingColumnHinge->GetJoint());
for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = limbJoint;
childEntities[stackIndex] = child;
stackIndex++;
}
} else if (strstr(name, "effector")) {
// add effector
dMatrix gripperMatrix(entity->CalculateGlobalMatrix());
effector = new dKukaEffector(m_kinematicSolver, parentJoint, baseFrameBody, gripperMatrix);
effector->SetAsThreedof();
effector->SetLinearSpeed(2.0f);
effector->SetMaxLinearFriction(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f);
}
break;
}
}
}
// create the Animation tree for manipulation
DemoEntity* const effectoBone = model->Find("effector_arm");
dMatrix baseFrameMatrix(model->GetCurrentMatrix());
dMatrix effectorLocalMatrix(effectoBone->CalculateGlobalMatrix(model));
dVector upAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 1.0f, 0.0f, 1.0f)));
dVector planeAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 0.0f, 1.0f, 1.0f)));
dEffectorTreeFixPose* const fixPose = new dEffectorTreeFixPose(baseFrameBody);
m_inputModifier = new dEffectorTreeInputModifier(fixPose, upAxis, planeAxis);
m_animTreeNode = new dEffectorTreeRoot(baseFrameBody, m_inputModifier);
// set base pose
dEffectorTreeInterface::dEffectorTransform frame;
frame.m_effector = effector;
frame.m_posit = effectorLocalMatrix.m_posit;
frame.m_rotation = dQuaternion(effectorLocalMatrix);
fixPose->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
}