chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add mjmBodyPair to C API.

Browse Source 
PiperOrigin-RevId: 606997065
Change-Id: Iefc2dff6ae942ce93ca83ec5f5fe9d7ae8bb6a40
This commit is contained in:
Alessio Quaglino
2024-02-14 08:39:01 -08:00
committed by Copybara-Service
parent c9e7586499
commit 80a986c0f5
7 changed files with 95 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+34 -9
View File
@@ -3585,34 +3585,59 @@ mjCBodyPair::mjCBodyPair(mjCModel* _model) {
model = _model;
// set defaults
bodyname1.clear();
bodyname2.clear();
spec_bodyname1_.clear();
spec_bodyname2_.clear();
// clear internal variables
body1 = body2 = signature = -1;
PointToLocal();
CopyFromSpec();
}
void mjCBodyPair::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.bodyname1 = (mjString)&spec_bodyname1_;
spec.bodyname2 = (mjString)&spec_bodyname2_;
spec.info = (mjString)&info;
}
void mjCBodyPair::CopyFromSpec() {
*static_cast<mjmExclude*>(this) = spec;
bodyname1_ = spec_bodyname1_;
bodyname2_ = spec_bodyname2_;
bodyname1 = (mjString)&bodyname1_;
bodyname2 = (mjString)&bodyname2_;
}
// compiler
void mjCBodyPair::Compile(void) {
CopyFromSpec();
// find body 1
mjCBody* pb1 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname1);
mjCBody* pb1 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname1_);
if (!pb1) {
throw mjCError(this, "body '%s' not found in bodypair %d", bodyname1.c_str(), id);
throw mjCError(this, "body '%s' not found in bodypair %d", bodyname1_.c_str(), id);
}
// find body 2
mjCBody* pb2 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname2);
mjCBody* pb2 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname2_);
if (!pb2) {
throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2.c_str(), id);
throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2_.c_str(), id);
}
// swap if body1 > body2
if (pb1->id > pb2->id) {
string nametmp = bodyname1;
bodyname1 = bodyname2;
bodyname2 = nametmp;
string nametmp = bodyname1_;
bodyname1_ = bodyname2_;
bodyname2_ = nametmp;
mjCBody* bodytmp = pb1;
pb1 = pb2;