00001
00007 #include "dynamics2d_gripping.h"
00008 #include "dynamics2d_engine.h"
00009
00010 #include <argos3/core/simulator/entity/embodied_entity.h>
00011
00012 #include <algorithm>
00013
00014 namespace argos {
00015
00016
00017
00018
00019 CDynamics2DGripper::CDynamics2DGripper(CDynamics2DEngine& c_engine,
00020 CGripperEquippedEntity& c_gripper_entity,
00021 cpShape* pt_gripper_shape) :
00022 m_cEngine(c_engine),
00023 m_cGripperEntity(c_gripper_entity),
00024 m_ptGripperShape(pt_gripper_shape),
00025 m_pcGrippee(NULL),
00026 m_tConstraint(NULL) {
00027 m_ptGripperShape->sensor = 1;
00028 m_ptGripperShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPER;
00029 m_ptGripperShape->data = this;
00030 m_tAnchor = cpvzero;
00031 }
00032
00033
00034
00035
00036 CDynamics2DGripper::~CDynamics2DGripper() {
00037 Release();
00038 }
00039
00040
00041
00042
00043 void CDynamics2DGripper::CalculateAnchor(cpArbiter* pt_arb) {
00044
00045
00046 m_tAnchor = cpvzero;
00047 for(SInt32 i = 0; i < cpArbiterGetCount(pt_arb); ++i) {
00048 m_tAnchor = cpvadd(m_tAnchor, cpArbiterGetPoint(pt_arb, i));
00049 }
00050 m_tAnchor = cpvmult(m_tAnchor, 1.0f / cpArbiterGetCount(pt_arb));
00051 }
00052
00053
00054
00055
00056 void CDynamics2DGripper::Grip(CDynamics2DGrippable* pc_grippee) {
00057 m_tConstraint =
00058 cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
00059 cpPivotJointNew(
00060 m_ptGripperShape->body,
00061 pc_grippee->GetShape()->body,
00062 m_tAnchor));
00063 m_tConstraint->maxBias = 0.95f;
00064 m_tConstraint->maxForce = 10000.0f;
00065 m_cGripperEntity.SetGrippedEntity(pc_grippee->GetEmbodiedEntity());
00066 m_pcGrippee = pc_grippee;
00067 m_pcGrippee->Attach(*this);
00068 }
00069
00070
00071
00072
00073 void CDynamics2DGripper::Release() {
00074 if(IsGripping()) {
00075 cpSpaceRemoveConstraint(m_cEngine.GetPhysicsSpace(), m_tConstraint);
00076 cpConstraintFree(m_tConstraint);
00077 m_tConstraint = NULL;
00078 m_cGripperEntity.ClearGrippedEntity();
00079 m_pcGrippee->Remove(*this);
00080 m_pcGrippee = NULL;
00081 }
00082 }
00083
00084
00085
00086
00087 CDynamics2DGrippable::CDynamics2DGrippable(CEmbodiedEntity& c_entity,
00088 cpShape* pt_shape) :
00089 m_cEmbodiedEntity(c_entity),
00090 m_ptShape(pt_shape) {
00091 m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
00092 m_ptShape->data = this;
00093 }
00094
00095
00096
00097
00098 CDynamics2DGrippable::~CDynamics2DGrippable() {
00099 ReleaseAll();
00100 }
00101
00102
00103
00104
00105 void CDynamics2DGrippable::Attach(CDynamics2DGripper& c_gripper) {
00106 m_listGrippers.push_back(&c_gripper);
00107 }
00108
00109
00110
00111
00112 void CDynamics2DGrippable::Remove(CDynamics2DGripper& c_gripper) {
00113 CDynamics2DGripper::TList::iterator it =
00114 std::find(m_listGrippers.begin(), m_listGrippers.end(), &c_gripper);
00115 if(it != m_listGrippers.end()) {
00116 m_listGrippers.erase(it);
00117 }
00118 }
00119
00120
00121
00122
00123 void CDynamics2DGrippable::Release(CDynamics2DGripper& c_gripper) {
00124 CDynamics2DGripper::TList::iterator it =
00125 std::find(m_listGrippers.begin(), m_listGrippers.end(), &c_gripper);
00126 if(it != m_listGrippers.end()) {
00127 (*it)->Release();
00128 }
00129 }
00130
00131
00132
00133
00134 void CDynamics2DGrippable::ReleaseAll() {
00135 while(!m_listGrippers.empty()) {
00136 m_listGrippers.back()->Release();
00137 }
00138 }
00139
00140
00141
00142
00143 int BeginCollisionBetweenGripperAndGrippable(cpArbiter* pt_arb,
00144 cpSpace* pt_space,
00145 void* p_data) {
00146
00147 CP_ARBITER_GET_SHAPES(pt_arb, ptGripperShape, ptGrippableShape);
00148
00149 CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*>(ptGripperShape->data);
00150
00151 CDynamics2DGrippable* pcGrippable = reinterpret_cast<CDynamics2DGrippable*>(ptGrippableShape->data);
00152
00153 return (&(pcGripper->GetGripperEntity().GetParent()) != &(pcGrippable->GetEmbodiedEntity().GetParent()));
00154 }
00155
00156
00157
00158
00159 int ManageCollisionBetweenGripperAndGrippable(cpArbiter* pt_arb,
00160 cpSpace* pt_space,
00161 void* p_data) {
00162
00163 CP_ARBITER_GET_SHAPES(pt_arb, ptGripperShape, ptGrippableShape);
00164
00165 CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*>(ptGripperShape->data);
00166
00167
00168
00169
00170
00171
00172
00173 if(pcGripper->IsGripping() && !pcGripper->IsLocked()) {
00174
00175 cpSpaceAddPostStepCallback(
00176 pt_space,
00177 RemoveConstraintBetweenGripperAndGrippable,
00178 pcGripper,
00179 ptGrippableShape->data);
00180 return false;
00181 }
00182 if(!pcGripper->IsGripping() && pcGripper->IsLocked()) {
00183 pcGripper->CalculateAnchor(pt_arb);
00184
00185 cpSpaceAddPostStepCallback(
00186 pt_space,
00187 AddConstraintBetweenGripperAndGrippable,
00188 pcGripper,
00189 reinterpret_cast<CDynamics2DGrippable*>(ptGrippableShape->data));
00190 return false;
00191 }
00192
00193 return false;
00194 }
00195
00196
00197
00198
00199 void AddConstraintBetweenGripperAndGrippable(cpSpace* pt_space,
00200 void* p_obj,
00201 void* p_data) {
00202
00203 CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*> (p_obj);
00204 CDynamics2DGrippable* pcGrippable = reinterpret_cast<CDynamics2DGrippable*>(p_data);
00205
00206 pcGripper->Grip(pcGrippable);
00207 }
00208
00209
00210
00211
00212 void RemoveConstraintBetweenGripperAndGrippable(cpSpace* pt_space,
00213 void* p_obj,
00214 void* p_data) {
00215
00216 CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*> (p_obj);
00217
00218 pcGripper->Release();
00219 }
00220
00221
00222
00223
00224 }