MetricAnIso::MetricAnIso( Real8 a,const MetricAnIso ma,
Real8 b,const MetricAnIso mb)
{
MetricAnIso mab(a*ma.a11+b*mb.a11,a*ma.a21+b*mb.a21,a*ma.a22+b*mb.a22);
MatVVP2x2 vab(mab);
R2 v1(vab.v.x,vab.v.y);
R2 v2(-v1.y,v1.x);
Real8 h1 = a / ma(v1) + b / mb(v1);
Real8 h2 = a / ma(v2) + b / mb(v2);
vab.lambda1 = 1 / (h1*h1);
vab.lambda2 = 1 / (h2*h2);
*this = vab;
}
MetricAnIso::MetricAnIso(const Real8 a[3],const MetricAnIso m0,
const MetricAnIso m1,const MetricAnIso m2 )
{
MetricAnIso mab(a[0]*m0.a11 + a[1]*m1.a11 + a[2]*m2.a11,
a[0]*m0.a21 + a[1]*m1.a21 + a[2]*m2.a21,
a[0]*m0.a22 + a[1]*m1.a22 + a[2]*m2.a22);
MatVVP2x2 vab(mab);
R2 v1(vab.v.x,vab.v.y);
R2 v2(-v1.y,v1.x);
Real8 h1 = a[0] / m0(v1) + a[1] / m1(v1) + a[2] / m2(v1);
Real8 h2 = a[0] / m0(v2) + a[1] / m1(v2) + a[2] / m2(v2);
vab.lambda1 = 1 / (h1*h1);
vab.lambda2 = 1 / (h2*h2);
*this = vab;
}
// Doc in parent
void
SoVRMLIndexedFaceSet::GLRender(SoGLRenderAction * action)
{
if (this->coordIndex.getNum() < 3 || this->coord.getValue() == NULL) return;
SoState * state = action->getState();
state->push();
// update state with coordinates, normals and texture information
SoVRMLVertexShape::GLRender(action);
if (!this->shouldGLRender(action)) {
state->pop();
return;
}
this->setupShapeHints(state, this->ccw.getValue(), this->solid.getValue());
Binding mbind = this->findMaterialBinding(state);
Binding nbind = this->findNormalBinding(state);
const SoCoordinateElement * coords;
const SbVec3f * normals;
const int32_t * cindices;
int numindices;
const int32_t * nindices;
const int32_t * tindices;
const int32_t * mindices;
SbBool doTextures;
SbBool normalCacheUsed;
SoMaterialBundle mb(action);
SoTextureCoordinateBundle tb(action, TRUE, FALSE);
doTextures = tb.needCoordinates();
SbBool sendNormals = !mb.isColorOnly() || tb.isFunction();
this->getVertexData(state, coords, normals, cindices,
nindices, tindices, mindices, numindices,
sendNormals, normalCacheUsed);
if (!sendNormals) {
nbind = OVERALL;
normals = NULL;
nindices = NULL;
}
else if (nbind == OVERALL) {
if (normals) glNormal3fv(normals[0].getValue());
else glNormal3f(0.0f, 0.0f, 1.0f);
}
else if (normalCacheUsed && nbind == PER_VERTEX) {
nbind = PER_VERTEX_INDEXED;
}
else if (normalCacheUsed && nbind == PER_FACE_INDEXED) {
nbind = PER_FACE;
}
if (mbind == PER_VERTEX) {
mbind = PER_VERTEX_INDEXED;
mindices = cindices;
}
if (nbind == PER_VERTEX) {
nbind = PER_VERTEX_INDEXED;
nindices = cindices;
}
Binding tbind = NONE;
if (doTextures) {
if (tb.isFunction() && !tb.needIndices()) {
tbind = NONE;
tindices = NULL;
}
else {
tbind = PER_VERTEX_INDEXED;
if (tindices == NULL) tindices = cindices;
}
}
SbBool convexcacheused = FALSE;
if (this->useConvexCache(action, normals, nindices, normalCacheUsed)) {
cindices = PRIVATE(this)->convexCache->getCoordIndices();
numindices = PRIVATE(this)->convexCache->getNumCoordIndices();
mindices = PRIVATE(this)->convexCache->getMaterialIndices();
nindices = PRIVATE(this)->convexCache->getNormalIndices();
tindices = PRIVATE(this)->convexCache->getTexIndices();
if (mbind == PER_VERTEX) mbind = PER_VERTEX_INDEXED;
else if (mbind == PER_FACE) mbind = PER_FACE_INDEXED;
if (nbind == PER_VERTEX) nbind = PER_VERTEX_INDEXED;
else if (nbind == PER_FACE) nbind = PER_FACE_INDEXED;
if (tbind != NONE) tbind = PER_VERTEX_INDEXED;
convexcacheused = TRUE;
}
mb.sendFirst(); // make sure we have the correct material
SoGLLazyElement * lelem = NULL;
const uint32_t contextid = action->getCacheContext();
SbBool dova =
SoVBO::shouldRenderAsVertexArrays(state, contextid, numindices) &&
!convexcacheused && !normalCacheUsed &&
((nbind == OVERALL) || ((nbind == PER_VERTEX_INDEXED) && ((nindices == cindices) || (nindices == NULL)))) &&
((tbind == NONE && !tb.needCoordinates()) ||
((tbind == PER_VERTEX_INDEXED) && ((tindices == cindices) || (tindices == NULL)))) &&
((mbind == NONE) || ((mbind == PER_VERTEX_INDEXED) && ((mindices == cindices) || (mindices == NULL)))) &&
SoGLDriverDatabase::isSupported(sogl_glue_instance(state), SO_GL_VERTEX_ARRAY);
const SoGLVBOElement * vboelem = SoGLVBOElement::getInstance(state);
SoVBO * colorvbo = NULL;
if (dova && (mbind != OVERALL)) {
dova = FALSE;
if ((mbind == PER_VERTEX_INDEXED) && ((mindices == cindices) || (mindices == NULL))) {
lelem = (SoGLLazyElement*) SoLazyElement::getInstance(state);
colorvbo = vboelem->getColorVBO();
if (colorvbo) dova = TRUE;
else {
// we might be able to do VA-rendering, but need to check the
// diffuse color type first.
if (!lelem->isPacked() && lelem->getNumTransparencies() <= 1) {
dova = TRUE;
}
}
}
}
SbBool didrenderasvbo = FALSE;
if (dova) {
SbBool dovbo = this->startVertexArray(action,
coords,
(nbind != OVERALL) ? normals : NULL,
doTextures,
mbind != OVERALL);
didrenderasvbo = dovbo;
LOCK_VAINDEXER(this);
if (PRIVATE(this)->vaindexer == NULL) {
SoVertexArrayIndexer * indexer = new SoVertexArrayIndexer;
int i = 0;
while (i < numindices) {
int cnt = 0;
while (i + cnt < numindices && cindices[i+cnt] >= 0) cnt++;
switch (cnt) {
case 3:
indexer->addTriangle(cindices[i],cindices[i+1], cindices[i+2]);
break;
case 4:
indexer->addQuad(cindices[i],cindices[i+1],cindices[i+2],cindices[i+3]);
break;
default:
if (cnt > 4) {
indexer->beginTarget(GL_POLYGON);
for (int j = 0; j < cnt; j++) {
indexer->targetVertex(GL_POLYGON, cindices[i+j]);
}
indexer->endTarget(GL_POLYGON);
}
}
i += cnt + 1;
}
indexer->close();
if (indexer->getNumVertices()) {
PRIVATE(this)->vaindexer = indexer;
}
else {
delete indexer;
}
#if 0
fprintf(stderr,"XXX: create VRML VertexArrayIndexer: %d\n", indexer->getNumVertices());
#endif
}
if (PRIVATE(this)->vaindexer) {
PRIVATE(this)->vaindexer->render(sogl_glue_instance(state), dovbo, contextid);
}
UNLOCK_VAINDEXER(this);
this->finishVertexArray(action,
dovbo,
(nbind != OVERALL),
doTextures,
mbind != OVERALL);
}
else {
SoVertexAttributeBundle vab(action, TRUE);
SbBool doattribs = vab.doAttributes();
SoVertexAttributeBindingElement::Binding attribbind =
SoVertexAttributeBindingElement::get(state);
if (!doattribs) {
// for overall attribute binding we check for doattribs before
// sending anything in SoGL::FaceSet::GLRender
attribbind = SoVertexAttributeBindingElement::OVERALL;
}
sogl_render_faceset((SoGLCoordinateElement *)coords,
cindices,
numindices,
normals,
nindices,
&mb,
mindices,
&tb,
tindices,
&vab,
(int)nbind,
(int)mbind,
(int)attribbind,
doTextures ? 1 : 0,
doattribs ? 1 : 0);
}
if (normalCacheUsed) {
this->readUnlockNormalCache();
}
if (convexcacheused) {
PRIVATE(this)->readUnlockConvexCache();
}
// send approx number of triangles for autocache handling
sogl_autocache_update(state, this->coordIndex.getNum() / 4, didrenderasvbo);
state->pop();
}
void World::update(float timeStep)
{
for(BodyList::iterator it = m_bodies.begin(); it != m_bodies.end(); ++it){
if(!(*it)->getBodyDef().isStatic()){
(*it)->acceleration(m_gravitation*timeStep);
(*it)->update(timeStep);
}
}
for(BodyList::iterator it = m_bodies.begin(); it != m_bodies.end(); ++it){
BodyList::iterator ith = it;
ith++;
for(BodyList::iterator it2 = ith; it2 != m_bodies.end(); ++it2){
if(!((*it)->getBodyDef().isStatic()) or !((*it2)->getBodyDef().isStatic())){
if((*it)->getShape()->collide((*it2)->getShape().get())){
Polygon *p1 = static_cast<Polygon *>((*it)->getShape().get());
Polygon *p2 = static_cast<Polygon *>((*it2)->getShape().get());
bool b = true;
Line cl;
Vector2f point1 = p1->getTransformedPoint(p1->getNumberOfPoints()-1);
Vector2f point2 = p2->getTransformedPoint(p2->getNumberOfPoints()-1);
for(size_t i = 0; i < p1->getNumberOfPoints(); ++i){
Line line1(p1->getTransformedPoint(i), point1-p1->getTransformedPoint(i));
point1 = p1->getTransformedPoint(i);
for(size_t j = 0; j < p2->getNumberOfPoints(); ++j){
Line line2(p2->getTransformedPoint(j), point2-p2->getTransformedPoint(j));
point2 = p2->getTransformedPoint(j);
float t1 = line1.intersects(line2);
float t2 = line2.intersects(line1);
if(0.f < t1 and t1 < 1.f and 0.f < t2 and t2 < 1.f){
Vector2f vec = line1.getPoint() + t1 * line1.getDirectionVector();
if(b){
cl.setPoint(vec);
b = false;
} else {
cl.setDirectionVector(vec - cl.getPoint());
}
}
}
}
Vector2f n = cl.getDirectionVector().normal();
//n.normalize();
Vector2f p = cl.getPoint()+(cl.getDirectionVector()/2.f);
Body::Ptr b1 = (*it);
Body::Ptr b2 = (*it2);
mx::Vector2f r1(p - b1->getShape()->getPosition());
mx::Vector2f r2(p - b2->getShape()->getPosition());
mx::Vector2f rap = r1.normal();
mx::Vector2f rbp = r2.normal();
float rapn = dot(rap, n);
float rbpn = dot(rbp, n);
mx::Vector2f vp1(b1->getVelocity()+b1->getAngularVelocity()*rap);
mx::Vector2f vp2(b2->getVelocity()+b2->getAngularVelocity()*rbp);
mx::Vector2f vab(vp2 - vp1);
float M1Inv = 0;
float M2Inv = 0;
float I1Inv = 0;
float I2Inv = 0;
if(!b1->getBodyDef().isStatic()){
M1Inv = 1.f / b1->getBodyDef().getMass();
I1Inv = 1.f / b1->getBodyDef().getMomentOfInertia();
}
if(!b2->getBodyDef().isStatic()){
M2Inv = 1.f / b2->getBodyDef().getMass();
I2Inv = 1.f / b2->getBodyDef().getMomentOfInertia();
}
float j = -(1+(b1->getBodyDef().getElasticity()+b2->getBodyDef().getElasticity())/2.f) * dot(vab, n);
j /= dot(n, n)*(M1Inv+M2Inv) + rapn*rapn*I1Inv + rbpn*rbpn*I2Inv;
b1->acceleration(-(j*M1Inv) * n);
b2->acceleration( (j*M2Inv) * n);
b1->angularAcceleration(-j*I1Inv * rapn);
b2->angularAcceleration( j*I2Inv * rbpn);
Shape::Ptr s1 = b1->getShape();
Shape::Ptr s2 = b2->getShape();
mx::Vector2f MTD = s1->MTD(s2.get());
float MInv = M1Inv + M2Inv;
s1->setPosition(s1->getPosition()+MTD*(M1Inv/MInv));
s2->setPosition(s2->getPosition()-MTD*(M2Inv/MInv));
}
}
}
}
}
