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

Merge pull request #1899 from saran-t:saran-t-patch-5

Browse Source 
PiperOrigin-RevId: 662454010
Change-Id: Ie3d6c59dd787aefa3c95869a91451387783b6057
This commit is contained in:
Copybara-Service
2024-08-13 03:51:18 -07:00
parent 1132632627 acab66d799
commit 4ebf381b4d
1 changed files with 13 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+13 -31
View File
@@ -2077,39 +2077,33 @@ double mjCGeom::GetVolume() const {
switch (typeinertia) {
case mjINERTIA_SHELL:
return 4 * mjPI * radius * radius;
break;
case mjINERTIA_VOLUME:
return 4 * mjPI * radius * radius * radius / 3;
break;
}
break;
}
break;
case mjGEOM_CAPSULE: {
double height = 2 * size[1];
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_SHELL:
return 4 * mjPI * radius * radius + 2 * mjPI * radius * height;
break;
case mjINERTIA_VOLUME:
return mjPI * (radius * radius * height + 4 * radius * radius * radius / 3);
break;
}
break;
}
break;
case mjGEOM_CYLINDER: {
double height = 2 * size[1];
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_SHELL:
return 2 * mjPI * radius * radius + 2 * mjPI * radius * height;
break;
case mjINERTIA_VOLUME:
return mjPI * radius * radius * height;
break;
}
break;
}
break;
case mjGEOM_ELLIPSOID: {
switch (typeinertia) {
case mjINERTIA_SHELL: {
@@ -2120,28 +2114,24 @@ double mjCGeom::GetVolume() const {
std::pow(size[1] * size[2], p) +
std::pow(size[2] * size[0], p);
return 4 * mjPI * std::pow(tmp / 3, 1 / p);
break;
}
case mjINERTIA_VOLUME:
return 4 * mjPI * size[0] * size[1] * size[2] / 3;
break;
}
break;
}
break;
case mjGEOM_HFIELD:
case mjGEOM_BOX: {
switch (typeinertia) {
case mjINERTIA_SHELL:
return 8 * (size[0] * size[1] + size[1] * size[2] + size[2] * size[0]);
break;
case mjINERTIA_VOLUME:
return size[0] * size[1] * size[2] * 8;
break;
}
break;
}
break;
default:
return 0;
break;
}
return 0;
}
@@ -2182,14 +2172,12 @@ void mjCGeom::SetInertia(void) {
case mjINERTIA_SHELL:
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3;
return;
break;
case mjINERTIA_VOLUME:
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 5;
return;
break;
}
break;
}
break;
case mjGEOM_CAPSULE: {
double halfheight = size[1];
double height = 2 * size[1];
@@ -2217,7 +2205,6 @@ void mjCGeom::SetInertia(void) {
inertia[1] += sphere_inertia + sphere_mass * (hs_pos * hs_pos - hs_com * hs_com);
inertia[2] += sphere_inertia;
return;
break;
}
case mjINERTIA_VOLUME: {
double sphere_mass =
@@ -2234,11 +2221,11 @@ void mjCGeom::SetInertia(void) {
inertia[1] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
inertia[2] += sphere_inertia;
return;
break;
}
break;
}
break;
}
break;
case mjGEOM_CYLINDER: {
double halfheight = size[1];
double height = 2 * halfheight;
@@ -2268,16 +2255,14 @@ void mjCGeom::SetInertia(void) {
inertia[1] += 2 * inertia_disk_x;
inertia[2] += 2 * inertia_disk_z;
return;
break;
}
case mjINERTIA_VOLUME:
inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12;
inertia[2] = mass_ * radius * radius / 2;
return;
break;
}
break;
}
break;
case mjGEOM_ELLIPSOID: {
double s00 = size[0] * size[0];
double s11 = size[1] * size[1];
@@ -2317,18 +2302,16 @@ void mjCGeom::SetInertia(void) {
inertia[1] = inertia_b[1] - inertia_a[1];
inertia[2] = inertia_b[2] - inertia_a[2];
return;
break;
}
case mjINERTIA_VOLUME: {
inertia[0] = mass_ * (s11 + s22) / 5;
inertia[1] = mass_ * (s00 + s22) / 5;
inertia[2] = mass_ * (s00 + s11) / 5;
return;
break;
}
}
break;
}
break;
case mjGEOM_HFIELD:
case mjGEOM_BOX: {
double s00 = size[0] * size[0];
@@ -2370,18 +2353,17 @@ void mjCGeom::SetInertia(void) {
inertia[1] = 2 * (mass0 * s22 + mass1 * s00 + Iy0 + Iy1 + Iy2);
inertia[2] = 2 * (mass1 * s00 + mass2 * s11 + Iz0 + Iz1 + Iz2);
return;
break;
}
case mjINERTIA_VOLUME: {
inertia[0] = mass_ * (s11 + s22) / 3;
inertia[1] = mass_ * (s00 + s22) / 3;
inertia[2] = mass_ * (s00 + s11) / 3;
return;
break;
}
break;
}
break;
}
break;
default:
inertia[0] = inertia[1] = inertia[2] = 0;
return;