Package org.jbox2d.dynamics.contacts

Source Code of org.jbox2d.dynamics.contacts.TOISolver

/*******************************************************************************
* Copyright (c) 2011, Daniel Murphy
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*     * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*     * Neither the name of the <organization> nor the
*       names of its contributors may be used to endorse or promote products
*       derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL DANIEL MURPHY BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
/**
* Created at 2:25:52 PM Jul 6, 2010
*/
package org.jbox2d.dynamics.contacts;

import org.jbox2d.collision.Manifold;
import org.jbox2d.collision.ManifoldPoint;
import org.jbox2d.collision.shapes.Shape;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.Fixture;

// updated to rev 100
// pooling: local, non-thread
/**
* This is a pure position solver for a single movable body in contact with
* multiple non-moving bodies.
* @author daniel
*/
public class TOISolver {
  // TODO djm: find out the best number to start with
  private TOIConstraint[] m_constraints = new TOIConstraint[4];
  private int m_count;
  private Body m_toiBody;
 
  public TOISolver(){
    m_count = 0;
    m_toiBody = null;
    for(int i=0; i<m_constraints.length; i++){
      m_constraints[i] = new TOIConstraint();
    }
  }
 
  public void clear(){
    // does nothing
  }
 
  public void initialize(Contact[] contacts, int count, Body toiBody){
    //clear();
   
    m_count = count;
    m_toiBody = toiBody;
   
    // TODO djm: can I pool this? for now just having an expandable array
    if(m_count >= m_constraints.length){
      TOIConstraint[] old = m_constraints;
      m_constraints = new TOIConstraint[old.length*2];
      System.arraycopy(old, 0, m_constraints, 0, old.length);
      for(int i=old.length; i<m_constraints.length; i++){
        m_constraints[i] = new TOIConstraint();
      }
    }
//    m_constraints = new TOIConstraint[count];

    for(int i=0; i<m_count; i++){
      Contact contact = contacts[i];
     
      Fixture fixtureA = contact.getFixtureA();
      Fixture fixtureB = contact.getFixtureB();
      Shape shapeA = fixtureA.getShape();
      Shape shapeB = fixtureB.getShape();
      float radiusA = shapeA.m_radius;
      float radiusB = shapeB.m_radius;
      Body bodyA = fixtureA.getBody();
      Body bodyB = fixtureB.getBody();
      Manifold manifold = contact.getManifold();
     
      assert(manifold.pointCount > 0);
     
//      m_constraints[i] = new TOIConstraint();
      TOIConstraint constraint = m_constraints[i];
      constraint.bodyA = bodyA;
      constraint.bodyB = bodyB;
      constraint.localNormal.set(manifold.localNormal);
      constraint.localPoint.set(manifold.localPoint);
      constraint.type = manifold.type;
      constraint.pointCount = manifold.pointCount;
      constraint.radius = radiusA + radiusB;

      for (int j = 0; j < constraint.pointCount; ++j){
        ManifoldPoint cp = manifold.points[j];
        constraint.localPoints[j] = cp.localPoint;
      }
    }
  }
 
  // djm pooling
  private final TOISolverManifold psm = new TOISolverManifold();
  private final Vec2 rA = new Vec2();
  private final Vec2 rB = new Vec2();
  private final Vec2 P = new Vec2();
  private final Vec2 temp = new Vec2();
  /**
   * Perform one solver iteration. Returns true if converged.
   * @param baumgarte
   * @return
   */
  public boolean solve(float baumgarte){
    // Push out the toi body to provide clearance for further simulation.
   
    float minSeparation = 0f;
   
    for (int i = 0; i < m_count; ++i){
      TOIConstraint c = m_constraints[i];
      Body bodyA = c.bodyA;
      Body bodyB = c.bodyB;

      float massA = bodyA.m_mass;
      float massB = bodyB.m_mass;

      // Only the TOI body should move.
      if (bodyA == m_toiBody){
        massB = 0.0f;
      }
      else{
        massA = 0.0f;
      }

      float invMassA = massA * bodyA.m_invMass;
      float invIA = massA * bodyA.m_invI;
      float invMassB = massB * bodyB.m_invMass;
      float invIB = massB * bodyB.m_invI;

      // Solve normal constraints
      for (int j = 0; j < c.pointCount; ++j){
        psm.initialize(c, j);
        Vec2 normal = psm.normal;

        Vec2 point = psm.point;
        float separation = psm.separation;

        rA.set(point).subLocal(bodyA.m_sweep.c);
        rB.set(point).subLocal(bodyB.m_sweep.c);

        // Track max constraint error.
        minSeparation = MathUtils.min(minSeparation, separation);

        // Prevent large corrections and allow slop.
        float C = MathUtils.clamp(baumgarte * (separation + Settings.linearSlop), -Settings.maxLinearCorrection, 0.0f);

        // Compute the effective mass.
        float rnA = Vec2.cross(rA, normal);
        float rnB = Vec2.cross(rB, normal);
        float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

        // Compute normal impulse
        float impulse = K > 0.0f ? - C / K : 0.0f;

        P.set(normal).mulLocal(impulse);

        temp.set(P).mulLocal(invMassA);
        bodyA.m_sweep.c.subLocal(temp);
        bodyA.m_sweep.a -= invIA * Vec2.cross(rA, P);
        bodyA.synchronizeTransform();

        temp.set(P).mulLocal(invMassB);
        bodyB.m_sweep.c.addLocal(temp);
        bodyB.m_sweep.a += invIB * Vec2.cross(rB, P);
        bodyB.synchronizeTransform();
      }
    }

    // We can't expect minSpeparation >= -_linearSlop because we don't
    // push the separation above -_linearSlop.
    return minSeparation >= -1.5f * Settings.linearSlop;
  }
}

class TOISolverManifold{
  public final Vec2 normal = new Vec2();
  public final Vec2 point = new Vec2();
  public float separation;
 
  // djm pooling
  private final Vec2 pointA = new Vec2();
  private final Vec2 pointB = new Vec2();
  private final Vec2 temp = new Vec2();
  private final Vec2 planePoint = new Vec2();
  private final Vec2 clipPoint = new Vec2();
 
  public void initialize(TOIConstraint cc, int index){
    assert(cc.pointCount > 0);

    switch (cc.type){
      case CIRCLES : {
        cc.bodyA.getWorldPointToOut(cc.localPoint, pointA);
        cc.bodyB.getWorldPointToOut(cc.localPoints[0], pointB);
        if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
          normal.set(pointB).subLocal(pointA);
          normal.normalize();
        }
        else {
          normal.set(1.0f, 0.0f);
        }
       
        point.set(pointA).addLocal(pointB).mulLocal(.5f);
        temp.set(pointB).subLocal(pointA);
        separation = Vec2.dot(temp, normal) - cc.radius;
        break;
      }
      case FACE_A : {
        cc.bodyA.getWorldVectorToOut(cc.localNormal, normal);
        cc.bodyA.getWorldPointToOut(cc.localPoint, planePoint);
       
        cc.bodyB.getWorldPointToOut(cc.localPoints[index], clipPoint);
        temp.set(clipPoint).subLocal(planePoint);
        separation = Vec2.dot(temp, normal) - cc.radius;
        point.set(clipPoint);
        break;
      }
     
      case FACE_B : {
        cc.bodyB.getWorldVectorToOut(cc.localNormal, normal);
        cc.bodyB.getWorldPointToOut(cc.localPoint, planePoint);
       
        cc.bodyA.getWorldPointToOut(cc.localPoints[index], clipPoint);
        temp.set(clipPoint).subLocal(planePoint);
        separation = Vec2.dot(temp, normal) - cc.radius;
        point.set(clipPoint);
       
        // Ensure normal points from A to B
        normal.negateLocal();
      }
        break;
    }
  }
}
TOP

Related Classes of org.jbox2d.dynamics.contacts.TOISolver

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.