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

Flip order of cases in user_objects.cc (readability)

Browse Source 
PiperOrigin-RevId: 663040865
Change-Id: I27d06a6a769d5bfc41189d270021b4fc1d4b5932
This commit is contained in:
Yuval Tassa
2024-08-14 13:48:20 -07:00
committed by Copybara-Service
parent 4c64e6f036
commit d932faa1d4
1 changed files with 47 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+47 -47
View File
@@ -2128,15 +2128,15 @@ double mjCGeom::GetVolume() const {
return mesh->GetVolumeRef(typeinertia);
}
// compute from geom shape (type) and inertia (typeinertia)
// compute from geom shape (type) and inertia type (typeinertia)
switch (type) {
case mjGEOM_SPHERE: {
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_SHELL:
return 4 * mjPI * radius * radius;
case mjINERTIA_VOLUME:
return 4 * mjPI * radius * radius * radius / 3;
case mjINERTIA_SHELL:
return 4 * mjPI * radius * radius;
}
break;
}
@@ -2144,10 +2144,10 @@ double mjCGeom::GetVolume() const {
double height = 2 * size[1];
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_SHELL:
return 4 * mjPI * radius * radius + 2 * mjPI * radius * height;
case mjINERTIA_VOLUME:
return mjPI * (radius * radius * height + 4 * radius * radius * radius / 3);
case mjINERTIA_SHELL:
return 4 * mjPI * radius * radius + 2 * mjPI * radius * height;
}
break;
}
@@ -2155,15 +2155,17 @@ double mjCGeom::GetVolume() const {
double height = 2 * size[1];
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_SHELL:
return 2 * mjPI * radius * radius + 2 * mjPI * radius * height;
case mjINERTIA_VOLUME:
return mjPI * radius * radius * height;
case mjINERTIA_SHELL:
return 2 * mjPI * radius * radius + 2 * mjPI * radius * height;
}
break;
}
case mjGEOM_ELLIPSOID: {
switch (typeinertia) {
case mjINERTIA_VOLUME:
return 4 * mjPI * size[0] * size[1] * size[2] / 3;
case mjINERTIA_SHELL: {
// Thomsen approximation
// https://www.numericana.com/answer/ellipsoid.htm#thomsen
@@ -2173,18 +2175,16 @@ double mjCGeom::GetVolume() const {
std::pow(size[2] * size[0], p);
return 4 * mjPI * std::pow(tmp / 3, 1 / p);
}
case mjINERTIA_VOLUME:
return 4 * mjPI * size[0] * size[1] * size[2] / 3;
}
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]);
case mjINERTIA_VOLUME:
return size[0] * size[1] * size[2] * 8;
case mjINERTIA_SHELL:
return 8 * (size[0] * size[1] + size[1] * size[2] + size[2] * size[0]);
}
break;
}
@@ -2223,16 +2223,16 @@ void mjCGeom::SetInertia(void) {
return;
}
// compute from geom shape (type) and inertia (typeinertia)
// compute from geom shape (type) and inertia type (typeinertia)
switch (type) {
case mjGEOM_SPHERE: {
switch (typeinertia) {
case mjINERTIA_SHELL:
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3;
return;
case mjINERTIA_VOLUME:
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 5;
return;
case mjINERTIA_SHELL:
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3;
return;
}
break;
}
@@ -2241,6 +2241,22 @@ void mjCGeom::SetInertia(void) {
double height = 2 * size[1];
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_VOLUME: {
double sphere_mass =
mass_ * 4 * radius / (4 * radius + 3 * height); // mass*(sphere_vol/total_vol)
double cylinder_mass = mass_ - sphere_mass;
// cylinder part
inertia[0] = inertia[1] = cylinder_mass * (3 * radius * radius + height * height) / 12;
inertia[2] = cylinder_mass * radius * radius / 2;
// add two hemispheres, displace along third axis
double sphere_inertia = 2 * sphere_mass * radius * radius / 5;
inertia[0] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
inertia[1] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
inertia[2] += sphere_inertia;
return;
}
case mjINERTIA_SHELL: {
// surface area
double Asphere = 4 * mjPI * radius * radius;
@@ -2264,22 +2280,6 @@ void mjCGeom::SetInertia(void) {
inertia[2] += sphere_inertia;
return;
}
case mjINERTIA_VOLUME: {
double sphere_mass =
mass_ * 4 * radius / (4 * radius + 3 * height); // mass*(sphere_vol/total_vol)
double cylinder_mass = mass_ - sphere_mass;
// cylinder part
inertia[0] = inertia[1] = cylinder_mass * (3 * radius * radius + height * height) / 12;
inertia[2] = cylinder_mass * radius * radius / 2;
// add two hemispheres, displace along third axis
double sphere_inertia = 2 * sphere_mass * radius * radius / 5;
inertia[0] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
inertia[1] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
inertia[2] += sphere_inertia;
return;
}
break;
}
break;
@@ -2289,6 +2289,10 @@ void mjCGeom::SetInertia(void) {
double height = 2 * halfheight;
double radius = size[0];
switch (typeinertia) {
case mjINERTIA_VOLUME:
inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12;
inertia[2] = mass_ * radius * radius / 2;
return;
case mjINERTIA_SHELL: {
// surface area
double Adisk = mjPI * radius * radius;
@@ -2314,10 +2318,6 @@ void mjCGeom::SetInertia(void) {
inertia[2] += 2 * inertia_disk_z;
return;
}
case mjINERTIA_VOLUME:
inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12;
inertia[2] = mass_ * radius * radius / 2;
return;
}
break;
}
@@ -2326,6 +2326,12 @@ void mjCGeom::SetInertia(void) {
double s11 = size[1] * size[1];
double s22 = size[2] * size[2];
switch (typeinertia) {
case mjINERTIA_VOLUME: {
inertia[0] = mass_ * (s11 + s22) / 5;
inertia[1] = mass_ * (s00 + s22) / 5;
inertia[2] = mass_ * (s00 + s11) / 5;
return;
}
case mjINERTIA_SHELL: {
// approximate shell inertia by subtracting ellipsoid from expanded ellipsoid
double eps = 1e-6;
@@ -2361,12 +2367,6 @@ void mjCGeom::SetInertia(void) {
inertia[2] = inertia_b[2] - inertia_a[2];
return;
}
case mjINERTIA_VOLUME: {
inertia[0] = mass_ * (s11 + s22) / 5;
inertia[1] = mass_ * (s00 + s22) / 5;
inertia[2] = mass_ * (s00 + s11) / 5;
return;
}
}
break;
}
@@ -2376,6 +2376,12 @@ void mjCGeom::SetInertia(void) {
double s11 = size[1] * size[1];
double s22 = size[2] * size[2];
switch (typeinertia) {
case mjINERTIA_VOLUME: {
inertia[0] = mass_ * (s11 + s22) / 3;
inertia[1] = mass_ * (s00 + s22) / 3;
inertia[2] = mass_ * (s00 + s11) / 3;
return;
}
case mjINERTIA_SHELL: {
// length
double lx = 2 * size[0]; // side 0
@@ -2412,12 +2418,6 @@ void mjCGeom::SetInertia(void) {
inertia[2] = 2 * (mass1 * s00 + mass2 * s11 + Iz0 + Iz1 + Iz2);
return;
}
case mjINERTIA_VOLUME: {
inertia[0] = mass_ * (s11 + s22) / 3;
inertia[1] = mass_ * (s00 + s22) / 3;
inertia[2] = mass_ * (s00 + s11) / 3;
return;
}
break;
}
break;