Checking constraint jacobian and normal directions.
Martin Felis
parent
3fac49ed9e
commit
5037eda185
|
|
@ -292,6 +292,7 @@ void CalcImpulseVariables(
|
|||
VectorNd* jac,
|
||||
double* G_MInv_GT) {
|
||||
if (body == nullptr || body->mIsStatic) {
|
||||
jac->setZero();
|
||||
*G_MInv_GT = 0.;
|
||||
return;
|
||||
}
|
||||
|
|
@ -326,7 +327,7 @@ void PrepareConstraintImpulse(
|
|||
body_a,
|
||||
cinfo.mBodyAIndex,
|
||||
cinfo.posA,
|
||||
cinfo.dir,
|
||||
-cinfo.dir,
|
||||
&cinfo.MInvA,
|
||||
&cinfo.jacA,
|
||||
&cinfo.GMInvGTA);
|
||||
|
|
@ -334,7 +335,7 @@ void PrepareConstraintImpulse(
|
|||
body_b,
|
||||
cinfo.mBodyBIndex,
|
||||
cinfo.posB,
|
||||
-cinfo.dir,
|
||||
cinfo.dir,
|
||||
&cinfo.MInvB,
|
||||
&cinfo.jacB,
|
||||
&cinfo.GMInvGTB);
|
||||
|
|
@ -355,14 +356,14 @@ void CalcConstraintImpulse(
|
|||
rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacA * body_a->qdot;
|
||||
}
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
|
||||
rhs -= (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
|
||||
}
|
||||
|
||||
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
|
||||
|
||||
if (body_a && !body_a->mIsStatic) {
|
||||
double old_impulse = cinfo.accumImpulseA;
|
||||
cinfo.deltaImpulseA = -rhs / denom;
|
||||
cinfo.deltaImpulseA = rhs / denom;
|
||||
cinfo.accumImpulseA += cinfo.deltaImpulseA;
|
||||
cinfo.accumImpulseA = std::max(0., cinfo.accumImpulseA);
|
||||
cinfo.deltaImpulseA = cinfo.accumImpulseA - old_impulse;
|
||||
|
|
@ -370,7 +371,7 @@ void CalcConstraintImpulse(
|
|||
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
double old_impulse = cinfo.accumImpulseB;
|
||||
cinfo.deltaImpulseB = -rhs / denom;
|
||||
cinfo.deltaImpulseB = rhs / denom;
|
||||
cinfo.accumImpulseB += cinfo.deltaImpulseB;
|
||||
cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB);
|
||||
cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse;
|
||||
|
|
@ -387,7 +388,7 @@ void ApplyConstraintImpulse(
|
|||
}
|
||||
|
||||
if (body_b && !body_b->mIsStatic) {
|
||||
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
|
||||
body_b->qdot -= cinfo.MInvB * cinfo.jacB.transpose()
|
||||
* (cinfo.deltaImpulseB);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -153,20 +153,29 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
ground_shape.mType = SimShape::Plane;
|
||||
ground_shape.pos = Vector3d::Zero();
|
||||
ground_shape.orientation = Quaternion(0., 0., 0., 1.);
|
||||
ground_body.mCollisionShapes.push_back(SimBody::BodyCollisionInfo(-1, ground_shape));
|
||||
ground_body.mCollisionShapes.push_back(
|
||||
SimBody::BodyCollisionInfo(-1, ground_shape));
|
||||
ground_body.mIsStatic = true;
|
||||
|
||||
double sphere_a_mass = 1.5;
|
||||
double sphere_b_mass = 1.5;
|
||||
|
||||
SimBody sphere_a_body = CreateSphereBody(
|
||||
sphere_a_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
|
||||
sphere_a_mass,
|
||||
1.0,
|
||||
0.,
|
||||
Vector3d(0., 0.5, 0.),
|
||||
Vector3d(0., -1., 0.));
|
||||
SimBody sphere_b_body = CreateSphereBody(
|
||||
sphere_b_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
|
||||
sphere_b_mass,
|
||||
1.0,
|
||||
0.,
|
||||
Vector3d(0., 0.5, 0.),
|
||||
Vector3d(0., -1., 0.));
|
||||
|
||||
CollisionInfo cinfo;
|
||||
|
||||
SECTION ("SphereOnGroundButColliding") {
|
||||
SECTION("SphereOnGroundButColliding") {
|
||||
sphere_a_body.q[1] = 0.5;
|
||||
sphere_a_body.qdot[1] = -1.23;
|
||||
|
||||
|
|
@ -176,24 +185,58 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
REQUIRE(collisions.size() == 1);
|
||||
cinfo = collisions[0];
|
||||
|
||||
bool cresult = CheckPenetration(ground_shape,
|
||||
sphere_a_body.mCollisionShapes[0].second, cinfo);
|
||||
bool cresult = CheckPenetration(
|
||||
ground_shape,
|
||||
sphere_a_body.mCollisionShapes[0].second,
|
||||
cinfo);
|
||||
REQUIRE(cresult == true);
|
||||
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
|
||||
|
||||
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
CalcConstraintImpulse(&ground_body,&sphere_a_body, cinfo, 0);
|
||||
double reference_impulseB = sphere_a_mass * sphere_a_body.qdot[1];
|
||||
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
|
||||
double reference_impulseB = -sphere_a_mass * sphere_a_body.qdot[1];
|
||||
REQUIRE(cinfo.accumImpulseA < 1.0e-12);
|
||||
REQUIRE(cinfo.accumImpulseB + reference_impulseB < 1.0e-12);
|
||||
REQUIRE(cinfo.accumImpulseB - reference_impulseB < 1.0e-12);
|
||||
|
||||
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
|
||||
REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
|
||||
}
|
||||
|
||||
SECTION ("SphereVsSphereCollision") {
|
||||
SECTION("SphereOnGroundButCollidingReverseBodyOrder") {
|
||||
sphere_a_body.q[1] = 0.5;
|
||||
sphere_a_body.qdot[1] = -1.23;
|
||||
|
||||
sphere_a_body.updateCollisionShapes();
|
||||
std::vector<CollisionInfo> collisions;
|
||||
CalcCollisions(sphere_a_body, ground_body, collisions);
|
||||
REQUIRE(collisions.size() == 1);
|
||||
cinfo = collisions[0];
|
||||
|
||||
bool cresult = CheckPenetration(
|
||||
sphere_a_body.mCollisionShapes[0].second,
|
||||
ground_shape,
|
||||
cinfo);
|
||||
REQUIRE(cresult == true);
|
||||
REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
|
||||
|
||||
PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
|
||||
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
|
||||
double reference_impulseA = -sphere_a_mass * sphere_a_body.qdot[1];
|
||||
REQUIRE(cinfo.accumImpulseA - reference_impulseA < 1.0e-12);
|
||||
REQUIRE(cinfo.accumImpulseB < 1.0e-12);
|
||||
|
||||
ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
|
||||
REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
|
||||
}
|
||||
|
||||
SECTION("SphereVsSphereCollision") {
|
||||
double sphere_b_mass = 1.5;
|
||||
SimBody sphere_b_body = CreateSphereBody(sphere_b_mass, 1.0, 0., Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
|
||||
SimBody sphere_b_body = CreateSphereBody(
|
||||
sphere_b_mass,
|
||||
1.0,
|
||||
0.,
|
||||
Vector3d(0., -0.5, 0.),
|
||||
Vector3d(0., 1., 0.));
|
||||
|
||||
sphere_a_body.q[1] = 0.5;
|
||||
sphere_a_body.qdot[1] = -1.23;
|
||||
|
|
@ -212,8 +255,8 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
|
|||
|
||||
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
|
||||
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
|
||||
// REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
|
||||
// REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
|
||||
// REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
|
||||
// REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
|
||||
|
||||
cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl;
|
||||
cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,157 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
# This file is part of SimpleMath, a lightweight C++ template library
|
||||
# for linear algebra.
|
||||
#
|
||||
# Copyright (C) 2009 Benjamin Schindler <bschindler@inf.ethz.ch>
|
||||
#
|
||||
# This Source Code Form is subject to the terms of the Mozilla Public
|
||||
# License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
# file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
# Pretty printers for SimpleMath::Matrix
|
||||
# This is still pretty basic as the python extension to gdb is still pretty basic.
|
||||
# It cannot handle complex simplemath types and it doesn't support many of the other simplemath types
|
||||
# This code supports fixed size as well as dynamic size matrices
|
||||
|
||||
# To use it:
|
||||
#
|
||||
# * Create a directory and put the file as well as an empty __init__.py in
|
||||
# that directory.
|
||||
# * Create a ~/.gdbinit file, that contains the following:
|
||||
# python
|
||||
# import sys
|
||||
# sys.path.insert(0, '/path/to/simplemath/printer/directory')
|
||||
# from printers import register_simplemath_printers
|
||||
# register_simplemath_printers (None)
|
||||
# end
|
||||
|
||||
import gdb
|
||||
import re
|
||||
import itertools
|
||||
from bisect import bisect_left
|
||||
|
||||
# Basic row/column iteration code for use with Sparse and Dense matrices
|
||||
class _MatrixEntryIterator(object):
|
||||
|
||||
def __init__ (self, rows, cols):
|
||||
self.rows = rows
|
||||
self.cols = cols
|
||||
self.currentRow = 0
|
||||
self.currentCol = 0
|
||||
|
||||
def __iter__ (self):
|
||||
return self
|
||||
|
||||
def next(self):
|
||||
return self.__next__() # Python 2.x compatibility
|
||||
|
||||
def __next__(self):
|
||||
row = self.currentRow
|
||||
col = self.currentCol
|
||||
if self.currentRow >= self.rows:
|
||||
raise StopIteration
|
||||
|
||||
self.currentCol = self.currentCol + 1
|
||||
if self.currentCol >= self.cols:
|
||||
self.currentCol = 0
|
||||
self.currentRow = self.currentRow + 1
|
||||
|
||||
return (row, col)
|
||||
|
||||
class SimpleMathMatrixPrinter:
|
||||
"Print SimpleMath Matrix or Array of some kind"
|
||||
|
||||
def __init__(self, variety, val):
|
||||
"Extract all the necessary information"
|
||||
|
||||
# Save the variety (presumably "Matrix" or "Array") for later usage
|
||||
self.variety = variety
|
||||
|
||||
# The gdb extension does not support value template arguments - need to extract them by hand
|
||||
type = val.type
|
||||
if type.code == gdb.TYPE_CODE_REF:
|
||||
type = type.target()
|
||||
self.type = type.unqualified().strip_typedefs()
|
||||
tag = self.type.tag
|
||||
regex = re.compile('\<.*\>')
|
||||
m = regex.findall(tag)[0][1:-1]
|
||||
template_params = m.split(',')
|
||||
template_params = [x.replace(" ", "") for x in template_params]
|
||||
|
||||
self.rows = 3
|
||||
self.cols = 1
|
||||
self.innerType = self.type.template_argument(0)
|
||||
print ("type: ", str(self.type))
|
||||
self.data = val['mStorage']['mData']
|
||||
# self.rows = val['mStorage']['mRows']
|
||||
# self.cols = val['mStorage']['mCols']
|
||||
#
|
||||
# self.innerType = self.type.template_argument(0)
|
||||
#
|
||||
# self.val = val
|
||||
#
|
||||
# # Fixed size matrices have a struct as their storage, so we need to walk through this
|
||||
# self.data = self.val['mStorage']['mData']
|
||||
# if self.data.type.code == gdb.TYPE_CODE_STRUCT:
|
||||
# self.data = self.data['array']
|
||||
# self.data = self.data.cast(self.innerType.pointer())
|
||||
|
||||
class _iterator(_MatrixEntryIterator):
|
||||
def __init__ (self, rows, cols, dataPtr):
|
||||
super(SimpleMathMatrixPrinter._iterator, self).__init__(rows, cols)
|
||||
|
||||
self.dataPtr = dataPtr
|
||||
|
||||
def __next__(self):
|
||||
|
||||
row, col = super(SimpleMathMatrixPrinter._iterator, self).__next__()
|
||||
|
||||
item = self.dataPtr.dereference()
|
||||
self.dataPtr = self.dataPtr + 1
|
||||
if (self.cols == 1): #if it's a column vector
|
||||
return ('[%d]' % (row,), item)
|
||||
elif (self.rows == 1): #if it's a row vector
|
||||
return ('[%d]' % (col,), item)
|
||||
return ('[%d,%d]' % (row, col), item)
|
||||
|
||||
def children(self):
|
||||
|
||||
return self._iterator(self.rows, self.cols, self.data)
|
||||
|
||||
def to_string(self):
|
||||
return "SimpleMath::%s<%s,%d,%d,%s> (data ptr: %s)" % (self.variety, self.innerType, self.rows, self.cols, self.data)
|
||||
|
||||
def build_simplemath_dictionary ():
|
||||
pretty_printers_dict[re.compile('^SimpleMath::MatrixBase<.*>$')] = lambda val: SimpleMathMatrixPrinter("Matrix", val)
|
||||
print(str(pretty_printers_dict))
|
||||
|
||||
def register_simplemath_printers(obj):
|
||||
"Register simplemath pretty-printers with objfile Obj"
|
||||
|
||||
if obj == None:
|
||||
obj = gdb
|
||||
obj.pretty_printers.append(lookup_function)
|
||||
|
||||
def lookup_function(val):
|
||||
"Look-up and return a pretty-printer that can print va."
|
||||
|
||||
type = val.type
|
||||
|
||||
if type.code == gdb.TYPE_CODE_REF:
|
||||
type = type.target()
|
||||
type = type.unqualified().strip_typedefs()
|
||||
|
||||
typename = type.tag
|
||||
if typename == None:
|
||||
return None
|
||||
|
||||
for function in pretty_printers_dict:
|
||||
if function.search(typename):
|
||||
return pretty_printers_dict[function](val)
|
||||
|
||||
return None
|
||||
|
||||
pretty_printers_dict = {}
|
||||
|
||||
build_simplemath_dictionary ()
|
||||
|
||||
Loading…
Reference in New Issue