bool CDynamics3DEntity::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_check_only) {
/* Move the body to the new position */
dBodySetPosition(m_tBody, c_position.GetX(), c_position.GetY(), c_position.GetZ());
/* Rotate the body to the new orientation */
dQuaternion tQuat = { c_orientation.GetW(),
c_orientation.GetX(),
c_orientation.GetY(),
c_orientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
/* Check for collisions */
bool bCollisions = m_cEngine.IsEntityColliding(m_tEntitySpace);
if(bCollisions || b_check_only) {
/*
* Undo the changes if there is a collision or
* if the move was just a check
*/
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ());
const CQuaternion& cOrientation = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat2 = { cOrientation.GetW(),
cOrientation.GetX(),
cOrientation.GetY(),
cOrientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat2);
return !bCollisions;
}
else {
/* Set the new position and orientation */
GetEmbodiedEntity().SetPosition(c_position);
GetEmbodiedEntity().SetOrientation(c_orientation);
return true;
}
}
void CPointMass3DSpiriModel::UpdateCameraAnchor(SAnchor& s_anchor) {
s_anchor.Orientation = GetEmbodiedEntity().GetOriginAnchor().Orientation;
s_anchor.Orientation *= s_anchor.OffsetOrientation;
s_anchor.Position = s_anchor.OffsetPosition;
s_anchor.Position.Rotate(s_anchor.Orientation);
s_anchor.Position += GetEmbodiedEntity().GetOriginAnchor().Position;
}
void CKinematics2DEntity::Reset() {
/* Reset body position */
m_cPosition = GetEmbodiedEntity().GetInitPosition().ProjectOntoXY();
/* Reset body orientation */
CRadians cXAngle, cYAngle;
GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(m_cOrientation, cYAngle, cXAngle);
}
void CKinematics2DEntity::UpdateEntityStatus() {
/* Update entity position */
m_cEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_cPosition);
GetEmbodiedEntity().SetPosition(m_cSpacePosition);
/* Update entity orientation */
m_cEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_cOrientation);
GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
}
void CDynamics2DBoxEntity::UpdateEntityStatus() {
if(m_ptBody != NULL) {
m_cEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_ptBody);
GetEmbodiedEntity().SetPosition(m_cSpacePosition);
m_cEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_ptBody);
GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
}
/* Update components */
m_cBoxEntity.UpdateComponents();
}
void CPhysXMultiBodyObjectModel::Reset() {
/* Move the bodies to the initial position and orientation */
MoveTo(GetEmbodiedEntity().GetOriginAnchor().Position,
GetEmbodiedEntity().GetOriginAnchor().Orientation);
/* Clear all forces */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
m_vecBodies[i].Body.clearForce();
m_vecBodies[i].Body.clearTorque();
}
/* Update ARGoS entity status */
UpdateEntityStatus();
}
void CDynamics2DSingleBodyObjectModel::SetBody(cpBody* pt_body,
Real f_height) {
/* Set the body and its data field for ray queries */
m_ptBody = pt_body;
m_ptBody->data = this;
/* Register the origin anchor update method */
RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
&CDynamics2DSingleBodyObjectModel::UpdateOriginAnchor);
/* Calculate the bounding box */
GetBoundingBox().MinCorner.SetZ(GetEmbodiedEntity().GetOriginAnchor().Position.GetZ());
GetBoundingBox().MaxCorner.SetZ(GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + f_height);
CalculateBoundingBox();
}
void CDynamics2DMultiBodyObjectModel::Reset() {
/* Reset body position */
MoveTo(GetEmbodiedEntity().GetOriginAnchor().Position,
GetEmbodiedEntity().GetOriginAnchor().Orientation);
/* For each body: */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
/* Zero the speeds */
m_vecBodies[i].Body->v = cpvzero;
m_vecBodies[i].Body->w = 0.0f;
/* Zero forces and torques */
cpBodyResetForces(m_vecBodies[i].Body);
}
}
void CDynamics3DEntity::UpdateEntityStatus() {
/* Update entity position and orientation */
const dReal* ptPosition = dBodyGetPosition(m_tBody);
GetEmbodiedEntity().SetPosition(
CVector3(ptPosition[0],
ptPosition[1],
ptPosition[2]));
const dReal* ptOrientation = dBodyGetQuaternion(m_tBody);
GetEmbodiedEntity().SetOrientation(
CQuaternion(ptOrientation[0],
ptOrientation[1],
ptOrientation[2],
ptOrientation[3]));
}
void CDynamics2DCylinderEntity::Reset() {
if(m_ptBody != NULL) {
/* Reset body position */
const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
/* Reset body orientation */
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Zero speed and applied forces */
m_ptBody->v = cpvzero;
m_ptBody->w = 0.0f;
cpBodyResetForces(m_ptBody);
}
}
void CKinematics2DEntity::UpdateFromEntityStatus() {
if( m_bEnabled ) {
m_cPreviousPosition = m_cPosition;
m_cPreviousOrientation = m_cOrientation;
GetEmbodiedEntity().ClearCollisionDetected();
}
}
CKinematics2DEntity::CKinematics2DEntity(CKinematics2DEngine& c_engine, CEmbodiedEntity& c_entity) :
CPhysicsEngineEntity(c_entity),
m_cEngine(c_engine),
m_cPosition(),
m_cOrientation(),
m_cSpacePosition(),
m_cSpaceOrientation(),
m_bEnabled(false),
m_tCollisionType(KINEMATICS2D_COLLISION_NONE) {
/* Set the initial body position */
m_cPosition = GetEmbodiedEntity().GetPosition().ProjectOntoXY();
/* set the initial body orientation */
CRadians cXAngle, cYAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(m_cOrientation, cYAngle, cXAngle);
}
void CDynamics2DSingleBodyObjectModel::Reset() {
/* Nothing to do for a static body */
if(cpBodyIsStatic(m_ptBody)) return;
/* Reset body position */
const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
/* Reset body orientation */
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Zero speed and applied forces */
m_ptBody->v = cpvzero;
m_ptBody->w = 0.0f;
cpBodyResetForces(m_ptBody);
/* Update bounding box */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
CalculateBoundingBox();
}
CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
CBoxEntity& c_box) :
CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
m_cBoxEntity(c_box),
m_sGeomData(GEOM_NORMAL) {
/* Check whether the box is movable or not */
if(c_box.GetEmbodiedEntity().IsMovable()) {
/* Movable box */
/* Set the body to its initial position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Create the geometry and the mass */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Set Geom gripping properties. */
m_sGeomData.Type = GEOM_GRIPPABLE;
dGeomSetData(m_tGeom, &m_sGeomData);
/* Create its mass */
dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Associate the body to the geom */
dGeomSetBody(m_tGeom, m_tBody);
/* Set the parent body total mass */
dBodySetMass(m_tBody, &m_tMass);
}
else {
/* Unmovable box, get rid of the body and add only the geometry */
dBodyDestroy(m_tBody);
/* Create the geometry */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
dGeomSetData(m_tGeom, &m_sGeomData);
/* Set the geom to its position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dGeomSetQuaternion(m_tGeom, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Associate the geom to null body (this makes it static) */
dGeomSetBody(m_tGeom, 0);
}
}
void CCylinderEntity::UpdateComponents() {
/* Set LED position and update led equipped entity */
CVector3 cLEDPosition;
for(UInt32 i = 0; i < m_pcLEDEquippedEntity->GetAllLeds().size(); ++i) {
cLEDPosition = m_vecBaseLEDPositions[i];
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
cLEDPosition += GetEmbodiedEntity().GetPosition();
m_pcLEDEquippedEntity->SetLedPosition(i, cLEDPosition);
}
m_pcEmbodiedEntity->UpdateBoundingBox();
}
bool CDynamics2DCylinderEntity::CheckIntersectionWithRay(Real& f_t_on_ray,
const CRay& c_ray) const {
cpSegmentQueryInfo tInfo;
if(cpShapeSegmentQuery(m_ptShape,
cpv(c_ray.GetStart().GetX(), c_ray.GetStart().GetY()),
cpv(c_ray.GetEnd().GetX() , c_ray.GetEnd().GetY() ),
&tInfo)) {
CVector3 cIntersectionPoint;
c_ray.GetPoint(cIntersectionPoint, tInfo.t);
if((cIntersectionPoint.GetZ() >= GetEmbodiedEntity().GetPosition().GetZ() - m_fHalfHeight) &&
(cIntersectionPoint.GetZ() <= GetEmbodiedEntity().GetPosition().GetZ() + m_fHalfHeight) ) {
f_t_on_ray = tInfo.t;
return true;
}
else {
return false;
}
}
else {
return false;
}
}
CPointMass3DSpiriModel::CPointMass3DSpiriModel(CPointMass3DEngine& c_engine,
CSpiriEntity& c_spiri) :
CPointMass3DQuadRotorModel(c_engine,
c_spiri.GetEmbodiedEntity(),
c_spiri.GetQuadRotorEntity(),
BODY_HEIGHT,
BODY_RADIUS,
BODY_MASS,
BODY_INERTIA,
POS_K_P,
POS_K_D,
YAW_K_P,
YAW_K_D,
VEL_K_P,
VEL_K_D,
ROT_K_P,
ROT_K_D) {
/* Register anchor update methods */
RegisterAnchorMethod(GetEmbodiedEntity().GetAnchor("rab"),
&CPointMass3DSpiriModel::UpdateRABAnchor);
RegisterAnchorMethod(GetEmbodiedEntity().GetAnchor("camera"),
&CPointMass3DSpiriModel::UpdateCameraAnchor);
}
void CDynamics3DEntity::Reset() {
/* Clear force and torque on the body */
dBodySetForce(m_tBody, 0.0f, 0.0f, 0.0f);
dBodySetTorque(m_tBody, 0.0f, 0.0f, 0.0f);
/* Clear speeds */
dBodySetLinearVel(m_tBody, 0.0f, 0.0f, 0.0f);
dBodySetAngularVel(m_tBody, 0.0f, 0.0f, 0.0f);
/* Reset position */
const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
dBodySetPosition(m_tBody,
cPosition.GetX(),
cPosition.GetY(),
cPosition.GetZ());
/* Reset orientation */
const CQuaternion& cQuaternion = GetEmbodiedEntity().GetInitOrientation();
dQuaternion tQuat = {
cQuaternion.GetW(),
cQuaternion.GetX(),
cQuaternion.GetY(),
cQuaternion.GetZ()
};
dBodySetQuaternion(m_tBody, tQuat);
}
void CEPuckEntity::SetLedPosition() {
/* Set LED positions */
const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
CVector3 cLEDPosition;
CRadians cLEDAngle;
SET_RING_LED_POSITION(0);
SET_RING_LED_POSITION(1);
SET_RING_LED_POSITION(2);
SET_RING_LED_POSITION(3);
SET_RING_LED_POSITION(4);
SET_RING_LED_POSITION(5);
SET_RING_LED_POSITION(6);
SET_RING_LED_POSITION(7);
}
CDynamics2DEPuckModel::CDynamics2DEPuckModel(CDynamics2DEngine& c_engine,
CEPuckEntity& c_entity) :
CDynamics2DSingleBodyObjectModel(c_engine, c_entity),
m_cEPuckEntity(c_entity),
m_cWheeledEntity(m_cEPuckEntity.GetWheeledEntity()),
m_cDiffSteering(c_engine,
EPUCK_MAX_FORCE,
EPUCK_MAX_TORQUE,
EPUCK_INTERWHEEL_DISTANCE),
m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
/* Create the body with initial position and orientation */
cpBody* ptBody =
cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
cpBodyNew(EPUCK_MASS,
cpMomentForCircle(EPUCK_MASS,
0.0f,
EPUCK_RADIUS + EPUCK_RADIUS,
cpvzero)));
const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(ptBody, cZAngle.GetValue());
/* Create the body shape */
cpShape* ptShape =
cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
cpCircleShapeNew(ptBody,
EPUCK_RADIUS,
cpvzero));
ptShape->e = 0.0; // No elasticity
ptShape->u = 0.7; // Lots of friction
/* Constrain the actual base body to follow the diff steering control */
m_cDiffSteering.AttachTo(ptBody);
/* Set the body so that the default methods work as expected */
SetBody(ptBody, EPUCK_HEIGHT);
}
void CPhysicsModel::UpdateEntityStatus() {
CalculateAnchors();
CalculateBoundingBox();
/*
* Update entity components
*/
/* Get a reference to the root entity */
/* NOTE: here the cast is static because we know that an embodied entity MUST have a parent
* which, by definition, is a composable entity */
CComposableEntity& cRoot = static_cast<CComposableEntity&>(m_cEmbodiedEntity.GetRootEntity());
/* Update its components */
cRoot.UpdateComponents();
/*
* Check whether a transfer is necessary
*/
if(!m_cEngine.IsPointContained(GetEmbodiedEntity().GetOriginAnchor().Position))
m_cEngine.ScheduleEntityForTransfer(m_cEmbodiedEntity);
}
/**
* Calculate the AABB in the global coordinate frame
*/
void CBulletSphereModel::CalculateBoundingBox()
{
GetBoundingBox().MinCorner.Set(
GetEmbodiedEntity().GetOriginAnchor().Position.GetX() - (entity->GetRadius()),
GetEmbodiedEntity().GetOriginAnchor().Position.GetY() - (entity->GetRadius()),
GetEmbodiedEntity().GetOriginAnchor().Position.GetZ()
);
GetBoundingBox().MaxCorner.Set(
GetEmbodiedEntity().GetOriginAnchor().Position.GetX() + (entity->GetRadius()),
GetEmbodiedEntity().GetOriginAnchor().Position.GetY() + (entity->GetRadius()),
GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + entity->GetRadius()
);
}
void CFootBotEntity::SetLedPosition() {
/* Set LED positions */
const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
CVector3 cLEDPosition;
CRadians cLEDAnglePhase = FOOTBOT_HALF_LED_ANGLE_SLICE + m_cTurretRotation;
CRadians cLEDAngle;
SET_RING_LED_POSITION(0);
SET_RING_LED_POSITION(1);
SET_RING_LED_POSITION(2);
SET_RING_LED_POSITION(3);
SET_RING_LED_POSITION(4);
SET_RING_LED_POSITION(5);
SET_RING_LED_POSITION(6);
SET_RING_LED_POSITION(7);
SET_RING_LED_POSITION(8);
SET_RING_LED_POSITION(9);
SET_RING_LED_POSITION(10);
SET_RING_LED_POSITION(11);
/* Set beacon position */
cLEDPosition.Set(0.0f, 0.0f, FOOTBOT_BEACON_ELEVATION);
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
cLEDPosition += cEntityPosition;
m_pcLEDEquippedEntity->SetLedPosition(12, cLEDPosition);
}
CDynamics2DCylinderEntity::CDynamics2DCylinderEntity(CDynamics2DEngine& c_engine,
CCylinderEntity& c_entity) :
CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
m_cCylinderEntity(c_entity),
m_fMass(c_entity.GetMass()),
m_ptShape(NULL),
m_ptBody(NULL) {
/* Get the radius of the entity */
Real fRadius = c_entity.GetRadius();
m_fHalfHeight = c_entity.GetHeight() * 0.5f;
/* Create a circle object in the physics space */
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
if(c_entity.IsMovable()) {
/* The cylinder is movable */
/* Create the body */
m_ptBody = cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
cpBodyNew(m_fMass,
cpMomentForCircle(m_fMass,
0,
fRadius + fRadius,
cpvzero)));
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create the geometry */
m_ptShape = cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
cpCircleShapeNew(m_ptBody, fRadius, cpvzero));
/* This object is grippable */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Lots surface contact friction to help pushing */
m_ptShape->u = 0.7;
/* Friction with ground */
m_ptLinearFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpPivotJointNew2(m_cEngine.GetGroundBody(),
m_ptBody,
cpvzero,
cpvzero));
m_ptLinearFriction->biasCoef = 0.0f; // disable joint correction
m_ptLinearFriction->maxForce = 1.0f; // emulate linear friction
m_ptAngularFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpGearJointNew(m_cEngine.GetGroundBody(),
m_ptBody,
0.0f,
1.0f));
m_ptAngularFriction->biasCoef = 0.0f; // disable joint correction
m_ptAngularFriction->maxForce = 5.0f; // emulate angular friction
}
else {
/* The cylinder is not movable */
/* Create the geometry */
m_ptShape = cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
cpCircleShapeNew(m_cEngine.GetGroundBody(),
fRadius,
cpv(cPosition.GetX(), cPosition.GetY())));
/* This object is normal */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Little contact friction to help sliding away */
m_ptShape->u = 0.1;
}
}
CDynamics2DBoxEntity::CDynamics2DBoxEntity(CDynamics2DEngine& c_engine,
CBoxEntity& c_entity) :
CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
m_cBoxEntity(c_entity),
m_fMass(c_entity.GetMass()),
m_ptShape(NULL),
m_ptBody(NULL) {
/* Get the size of the entity */
CVector3 cHalfSize = c_entity.GetSize() * 0.5f;
m_fHalfHeight = cHalfSize.GetZ();
/* Create a polygonal object in the physics space */
/* Start defining the vertices
NOTE: points must be defined in a clockwise winding
*/
cpVect tVertices[] = {
cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
cpv(-cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), -cHalfSize.GetY())
};
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
if(c_entity.GetEmbodiedEntity().IsMovable()) {
/* The box is movable */
/* Create the body */
m_ptBody =
cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
cpBodyNew(m_fMass,
cpMomentForPoly(m_fMass,
4,
tVertices,
cpvzero)));
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create the geometry */
m_ptShape =
cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_ptBody,
4,
tVertices,
cpvzero));
/* This object is grippable */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Lots contact friction to help pushing */
m_ptShape->u = 0.7;
/* Friction with ground */
m_ptLinearFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpPivotJointNew2(m_cEngine.GetGroundBody(),
m_ptBody,
cpvzero,
cpvzero));
m_ptLinearFriction->maxBias = 0.0f; // disable joint correction
m_ptLinearFriction->maxForce = 1.49f; // emulate linear friction (this is just slightly smaller than FOOTBOT_MAX_FORCE)
m_ptAngularFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpGearJointNew(m_cEngine.GetGroundBody(),
m_ptBody,
0.0f,
1.0f));
m_ptAngularFriction->maxBias = 0.0f; // disable joint correction
m_ptAngularFriction->maxForce = 1.49f; // emulate angular friction (this is just slightly smaller than FOOTBOT_MAX_TORQUE)
}
else {
/* The box is not movable */
/* Manually rotate the vertices */
cpVect tRot = cpvforangle(cZAngle.GetValue());
tVertices[0] = cpvrotate(tVertices[0], tRot);
tVertices[1] = cpvrotate(tVertices[1], tRot);
tVertices[2] = cpvrotate(tVertices[2], tRot);
tVertices[3] = cpvrotate(tVertices[3], tRot);
/* Create the geometry */
m_ptShape =
cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_cEngine.GetGroundBody(),
4,
tVertices,
cpv(cPosition.GetX(), cPosition.GetY())));
/* This object is normal */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Little contact friction to help sliding away */
m_ptShape->u = 0.1;
}
}