Using manifold points instead of posA, posB for adding constraints.
Martin Felis
parent
4b19fd9c0b
commit
cb387beb33
|
|
@ -38,7 +38,7 @@ void SimBody::calcNextPositions(
|
|||
Vector3d omega(in_qdot.block(joint.q_index, 0, 3, 1));
|
||||
Quaternion qd = q0.omegaToQDot(omega);
|
||||
Quaternion q1 = (q0 + qd * dt).normalize();
|
||||
assert (!isnan(q1.squaredNorm()));
|
||||
assert(!isnan(q1.squaredNorm()));
|
||||
mModel.SetQuaternion(i, q1, io_q);
|
||||
}
|
||||
}
|
||||
|
|
@ -81,7 +81,7 @@ void SimShapeSupport(
|
|||
len = ccdVec3Len2(&dir);
|
||||
if (len - CCD_EPS > CCD_ZERO) {
|
||||
ccdVec3Copy(v, &dir);
|
||||
ccdVec3Scale(v, shape->scale[0] / CCD_SQRT(len));
|
||||
ccdVec3Scale(v, shape->scale[0] * CCD_REAL(0.5) / CCD_SQRT(len));
|
||||
} else {
|
||||
ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO);
|
||||
}
|
||||
|
|
@ -95,7 +95,7 @@ void SimShapeSupport(
|
|||
ccdVec3Add(v, &pos);
|
||||
}
|
||||
|
||||
static void sSwapCollisionInfoShapeOrder(CollisionInfo &cinfo) {
|
||||
static void sSwapCollisionInfoShapeOrder(CollisionInfo& cinfo) {
|
||||
cinfo.dir *= -1.;
|
||||
Vector3d temp_pos = cinfo.posA;
|
||||
cinfo.posA = cinfo.posB;
|
||||
|
|
@ -186,11 +186,6 @@ bool CheckPenetrationSphereVsPlane(
|
|||
assert(shape_a.mType == SimShape::Sphere);
|
||||
assert(shape_b.mType == SimShape::Plane);
|
||||
|
||||
// For now only support aligned spheres
|
||||
assert(
|
||||
(shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
|
||||
< cCollisionEps);
|
||||
|
||||
Vector3d plane_normal =
|
||||
shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
|
||||
Vector3d plane_point = shape_b.pos;
|
||||
|
|
@ -203,6 +198,8 @@ bool CheckPenetrationSphereVsPlane(
|
|||
cinfo.dir = -plane_normal;
|
||||
cinfo.depth = sphere_center_height;
|
||||
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] =
|
||||
sphere_point_to_plane - sphere_center_height * plane_normal;
|
||||
cinfo.posA = sphere_point_to_plane;
|
||||
cinfo.posB = sphere_point_to_plane - sphere_center_height * plane_normal;
|
||||
|
||||
|
|
@ -249,17 +246,23 @@ bool CheckPenetrationBoxVsPlane(
|
|||
bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
|
||||
|
||||
if (isnan(cinfo.posA.squaredNorm())) {
|
||||
gLog ("NaN error!");
|
||||
gLog("NaN error!");
|
||||
}
|
||||
|
||||
assert (!isnan(cinfo.dir.squaredNorm()));
|
||||
assert (!isnan(cinfo.posA.squaredNorm()));
|
||||
assert (!isnan(cinfo.posB.squaredNorm()));
|
||||
assert(!isnan(cinfo.dir.squaredNorm()));
|
||||
assert(!isnan(cinfo.posA.squaredNorm()));
|
||||
assert(!isnan(cinfo.posB.squaredNorm()));
|
||||
|
||||
cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
|
||||
cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
|
||||
cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
|
||||
|
||||
// also transform all contact manifold points
|
||||
for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
|
||||
cinfo.mManifoldPoints[i] =
|
||||
shape_a_rot.transpose() * cinfo.mManifoldPoints[i] + shape_a.pos;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -322,7 +325,8 @@ bool CheckPenetrationAABBVsPlane(
|
|||
for (int i = 0; i < 8; i++) {
|
||||
distances[i] = (vertices[i] - plane_pos).dot(plane_normal);
|
||||
if (distances[i] >= 0. && distances[i] < cCollisionEps) {
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = vertices[i];
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints] = vertices[i];
|
||||
cinfo.mNumManifoldPoints++;
|
||||
}
|
||||
max_depth = distances[i] < max_depth ? distances[i] : max_depth;
|
||||
}
|
||||
|
|
@ -358,9 +362,26 @@ bool CheckPenetrationAABBVsPlane(
|
|||
assert(s >= 0);
|
||||
assert(s <= 1.);
|
||||
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
|
||||
s = s < cCollisionEps ? 0. : s;
|
||||
s = s > 1.0 - cCollisionEps ? 1.0 : s;
|
||||
|
||||
Vector3d vc = v0 + s * (v1 - v0);
|
||||
bool found_duplicate_point = false;
|
||||
for (int j = 0; j < cinfo.mNumManifoldPoints; j++) {
|
||||
if ((cinfo.mManifoldPoints[j] - vc).squaredNorm() < cCollisionEps) {
|
||||
gLog("Removing duplicate point");
|
||||
found_duplicate_point = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found_duplicate_point) {
|
||||
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] =
|
||||
v0 + s * (v1 - v0);
|
||||
}
|
||||
|
||||
if (cinfo.mNumManifoldPoints > 4) {
|
||||
gLog ("Have %d manifold points?!", cinfo.mNumManifoldPoints);
|
||||
gLog("Have %d manifold points?!", cinfo.mNumManifoldPoints);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -487,7 +508,7 @@ void CalcImpulseVariables(
|
|||
const VectorNd& q = body->q;
|
||||
const VectorNd& qdot = body->qdot;
|
||||
|
||||
assert (!isnan(q.squaredNorm()));
|
||||
assert(!isnan(q.squaredNorm()));
|
||||
|
||||
// Calculate local coordinates of the contact point
|
||||
UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
|
||||
|
|
@ -504,7 +525,7 @@ void CalcImpulseVariables(
|
|||
(*jac) = dir.transpose() * G_constr;
|
||||
|
||||
*G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
|
||||
assert (!isnan(*G_MInv_GT));
|
||||
assert(!isnan(*G_MInv_GT));
|
||||
|
||||
*bias_vel = (*jac) * qdot * restitution;
|
||||
}
|
||||
|
|
@ -564,7 +585,7 @@ void CalcConstraintImpulse(
|
|||
ref = ref_a + ref_b;
|
||||
|
||||
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
|
||||
assert (denom > cCollisionEps);
|
||||
assert(denom > cCollisionEps);
|
||||
|
||||
double old_impulse = cinfo.accumImpulse;
|
||||
// TODO: is this really needed here??
|
||||
|
|
@ -580,18 +601,18 @@ void ApplyConstraintImpulse(
|
|||
if (body_a && !body_a->mIsStatic) {
|
||||
body_a->qdot +=
|
||||
cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
|
||||
assert (!isnan(body_a->qdot.squaredNorm()));
|
||||
assert(!isnan(body_a->qdot.squaredNorm()));
|
||||
}
|
||||
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
|
||||
assert (!isnan(body_b->qdot.squaredNorm()));
|
||||
assert(!isnan(body_b->qdot.squaredNorm()));
|
||||
}
|
||||
}
|
||||
|
||||
void World::calcUnconstrainedVelUpdate(double dt) {
|
||||
for (SimBody& body : mBodies) {
|
||||
assert (!isnan(body.q.squaredNorm()));
|
||||
assert(!isnan(body.q.squaredNorm()));
|
||||
|
||||
ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
|
||||
|
||||
|
|
@ -633,15 +654,22 @@ void World::detectCollisions() {
|
|||
|
||||
if (has_penetration) {
|
||||
if (isnan(cinfo.posA.squaredNorm())) {
|
||||
gLog ("NaN error!");
|
||||
gLog("NaN error!");
|
||||
}
|
||||
|
||||
cinfo.mBodyA = nullptr;
|
||||
cinfo.mBodyAIndex = -1;
|
||||
cinfo.mBodyB = &ref_body;
|
||||
cinfo.mBodyBIndex = body_col_info.first;
|
||||
assert (!isnan(cinfo.posA.squaredNorm()));
|
||||
assert (!isnan(cinfo.posB.squaredNorm()));
|
||||
mContactPoints.push_back(cinfo);
|
||||
assert(!isnan(cinfo.posA.squaredNorm()));
|
||||
assert(!isnan(cinfo.posB.squaredNorm()));
|
||||
|
||||
for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
|
||||
CollisionInfo cpinfo(cinfo);
|
||||
cinfo.posA = cinfo.mManifoldPoints[i];
|
||||
cinfo.posB = cinfo.mManifoldPoints[i];
|
||||
mContactPoints.push_back(cinfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,23 +43,29 @@ void simulator_init() {
|
|||
|
||||
sWorld.mStaticShapes.push_back(sGroundShape);
|
||||
|
||||
double restitution = 0.01;
|
||||
double restitution = 0.3;
|
||||
|
||||
int num_bodies = 5;
|
||||
for (int i = 0; i < num_bodies; i++) {
|
||||
SimBody body = CreateBoxBody(
|
||||
1.,
|
||||
Vector3d(2., 1., 1.),
|
||||
restitution,
|
||||
Vector3d::Random() * 5.,
|
||||
Vector3d::Zero());
|
||||
SimBody body;
|
||||
|
||||
// SimBody body = CreateSphereBody(
|
||||
// 1.,
|
||||
// 1.,
|
||||
// restitution,
|
||||
// Vector3d::Random() * 5.,
|
||||
// Vector3d::Zero());
|
||||
bool create_sphere = false;
|
||||
|
||||
if (!create_sphere) {
|
||||
body = CreateBoxBody(
|
||||
1.,
|
||||
Vector3d(2., 1., 1.),
|
||||
restitution,
|
||||
Vector3d::Random() * 5.,
|
||||
Vector3d::Zero());
|
||||
} else {
|
||||
body = CreateSphereBody(
|
||||
1.,
|
||||
1.,
|
||||
restitution,
|
||||
Vector3d::Random() * 5.,
|
||||
Vector3d::Zero());
|
||||
}
|
||||
|
||||
sWorld.mBodies.push_back(body);
|
||||
}
|
||||
|
|
@ -232,14 +238,14 @@ void simulator_draw(srcmdbuf* cmdbuf) {
|
|||
gLog("Error: cannot render shape of type %d", cinfo.second.mType);
|
||||
}
|
||||
|
||||
simd4x4f trans = simd4x4f_create (
|
||||
simd4x4f trans = simd4x4f_create(
|
||||
// clang-format off
|
||||
simd4f_create(1.f, 0.f, 0.f, 0.f),
|
||||
simd4f_create(0.f, 1.f, 0.f, 0.f),
|
||||
simd4f_create(0.f, 0.f, 1.f, 0.f),
|
||||
simd4f_create(cinfo.second.pos[0], cinfo.second.pos[1], cinfo.second.pos[2], 1.f)
|
||||
// clang-format on
|
||||
);
|
||||
);
|
||||
|
||||
simd4x4f scale = simd4x4f_create(
|
||||
// clang-format off
|
||||
|
|
|
|||
Loading…
Reference in New Issue