Java TimeStep类代码示例

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
void solveRigid(final TimeStep step) {
  for (ParticleGroup group = m_groupList; group != null; group = group.getNext()) {
    if ((group.m_groupFlags & ParticleGroupType.b2_rigidParticleGroup) != 0) {
      group.updateStatistics();
      Vec2 temp = tempVec;
      Vec2 cross = tempVec2;
      Rot rotation = tempRot;
      rotation.set(step.dt * group.m_angularVelocity);
      Rot.mulToOutUnsafe(rotation, group.m_center, cross);
      temp.set(group.m_linearVelocity).mulLocal(step.dt).addLocal(group.m_center).subLocal(cross);
      tempXf.p.set(temp);
      tempXf.q.set(rotation);
      Transform.mulToOut(tempXf, group.m_transform, group.m_transform);
      final Transform velocityTransform = tempXf2;
      velocityTransform.p.x = step.inv_dt * tempXf.p.x;
      velocityTransform.p.y = step.inv_dt * tempXf.p.y;
      velocityTransform.q.s = step.inv_dt * tempXf.q.s;
      velocityTransform.q.c = step.inv_dt * (tempXf.q.c - 1);
      for (int i = group.m_firstIndex; i < group.m_lastIndex; i++) {
        Transform.mulToOutUnsafe(velocityTransform, m_positionBuffer.data[i],
            m_velocityBuffer.data[i]);
      }
    }
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
void solveSpring(final TimeStep step) {
  float springStrength = step.inv_dt * m_springStrength;
  for (int k = 0; k < m_pairCount; k++) {
    final Pair pair = m_pairBuffer[k];
    if ((pair.flags & ParticleType.b2_springParticle) != 0) {
      int a = pair.indexA;
      int b = pair.indexB;
      final Vec2 pa = m_positionBuffer.data[a];
      final Vec2 pb = m_positionBuffer.data[b];
      final float dx = pb.x - pa.x;
      final float dy = pb.y - pa.y;
      float r0 = pair.distance;
      float r1 = MathUtils.sqrt(dx * dx + dy * dy);
      if (r1 == 0) r1 = Float.MAX_VALUE;
      float strength = springStrength * pair.strength;
      final float fx = strength * (r0 - r1) / r1 * dx;
      final float fy = strength * (r0 - r1) / r1 * dy;
      final Vec2 va = m_velocityBuffer.data[a];
      final Vec2 vb = m_velocityBuffer.data[b];
      va.x -= fx;
      va.y -= fy;
      vb.x += fx;
      vb.y += fy;
    }
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
void solveSolid(final TimeStep step) {
  // applies extra repulsive force from solid particle groups
  m_depthBuffer = requestParticleBuffer(m_depthBuffer);
  float ejectionStrength = step.inv_dt * m_ejectionStrength;
  for (int k = 0; k < m_contactCount; k++) {
    final ParticleContact contact = m_contactBuffer[k];
    int a = contact.indexA;
    int b = contact.indexB;
    if (m_groupBuffer[a] != m_groupBuffer[b]) {
      float w = contact.weight;
      Vec2 n = contact.normal;
      float h = m_depthBuffer[a] + m_depthBuffer[b];
      final Vec2 va = m_velocityBuffer.data[a];
      final Vec2 vb = m_velocityBuffer.data[b];
      final float inter = ejectionStrength * h * w;
      final float fx = inter * n.x;
      final float fy = inter * n.y;
      va.x -= fx;
      va.y -= fy;
      vb.x += fx;
      vb.y += fy;
    }
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
void solveColorMixing(final TimeStep step) {
  // mixes color between contacting particles
  m_colorBuffer.data = requestParticleBuffer(ParticleColor.class, m_colorBuffer.data);
  int colorMixing256 = (int) (256 * m_colorMixingStrength);
  for (int k = 0; k < m_contactCount; k++) {
    final ParticleContact contact = m_contactBuffer[k];
    int a = contact.indexA;
    int b = contact.indexB;
    if ((m_flagsBuffer.data[a] & m_flagsBuffer.data[b] & ParticleType.b2_colorMixingParticle) != 0) {
      ParticleColor colorA = m_colorBuffer.data[a];
      ParticleColor colorB = m_colorBuffer.data[b];
      int dr = (colorMixing256 * (colorB.r - colorA.r)) >> 8;
      int dg = (colorMixing256 * (colorB.g - colorA.g)) >> 8;
      int db = (colorMixing256 * (colorB.b - colorA.b)) >> 8;
      int da = (colorMixing256 * (colorB.a - colorA.a)) >> 8;
      colorA.r += dr;
      colorA.g += dg;
      colorA.b += db;
      colorA.a += da;
      colorB.r -= dr;
      colorB.g -= dg;
      colorB.b -= db;
      colorB.a -= da;
    }
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
/**
 * @see org.jbox2d.dynamics.joints.Joint#solveVelocityConstraints(org.jbox2d.dynamics.TimeStep)
 */
@Override
public void solveVelocityConstraints(TimeStep step) {
	Body b1 = m_bodyA;
	Body b2 = m_bodyB;
	
	float Cdot = m_J.compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);
	
	float impulse = m_mass * (-Cdot);
	m_impulse += impulse;
	
	final Vec2 temp = pool.popVec2();
	temp.set(m_J.linearA).mulLocal(b1.m_invMass).mulLocal(impulse);
	b1.m_linearVelocity.addLocal(temp);
	b1.m_angularVelocity += b1.m_invI * impulse * m_J.angularA;
	
	temp.set(m_J.linearB).mulLocal(b2.m_invMass).mulLocal(impulse);
	b2.m_linearVelocity.addLocal(temp);
	b2.m_angularVelocity += b2.m_invI * impulse * m_J.angularB;
	
	pool.pushVec2(1);
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(final TimeStep step) {
	float crossMassSum = 0.0f;
	float dotMassSum = 0.0f;
	
	final Vec2 d[] = pool.getVec2Array(bodies.length);

	for (int i=0; i<bodies.length; ++i) {
		final int prev = (i==0)?bodies.length-1:i-1;
		final int next = (i==bodies.length-1)?0:i+1;
		d[i].set(bodies[next].getWorldCenter());
		d[i].subLocal(bodies[prev].getWorldCenter());
		dotMassSum += (d[i].lengthSquared())/bodies[i].getMass();
		crossMassSum += Vec2.cross(bodies[i].getLinearVelocity(),d[i]);
	}
	float lambda = -2.0f * crossMassSum / dotMassSum;
	//System.out.println(crossMassSum + " " +dotMassSum);
	//lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
	m_impulse += lambda;
	//System.out.println(m_impulse);
	for (int i=0; i<bodies.length; ++i) {
		bodies[i].m_linearVelocity.x += bodies[i].m_invMass * d[i].y * .5f * lambda;
		bodies[i].m_linearVelocity.y += bodies[i].m_invMass * -d[i].x * .5f * lambda;
	}
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(final TimeStep step) {
  float crossMassSum = 0.0f;
  float dotMassSum = 0.0f;

  final Vec2 d[] = pool.getVec2Array(bodies.length);

  for (int i = 0; i < bodies.length; ++i) {
    final int prev = (i == 0) ? bodies.length - 1 : i - 1;
    final int next = (i == bodies.length - 1) ? 0 : i + 1;
    d[i].set(bodies[next].getWorldCenter());
    d[i].subLocal(bodies[prev].getWorldCenter());
    dotMassSum += (d[i].lengthSquared()) / bodies[i].getMass();
    crossMassSum += Vec2.cross(bodies[i].getLinearVelocity(), d[i]);
  }
  float lambda = -2.0f * crossMassSum / dotMassSum;
  // System.out.println(crossMassSum + " " +dotMassSum);
  // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection,
  // Settings.maxLinearCorrection);
  m_impulse += lambda;
  // System.out.println(m_impulse);
  for (int i = 0; i < bodies.length; ++i) {
    bodies[i].m_linearVelocity.x += bodies[i].m_invMass * d[i].y * .5f * lambda;
    bodies[i].m_linearVelocity.y += bodies[i].m_invMass * -d[i].x * .5f * lambda;
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
public void solveCollision(TimeStep step) {
  final AABB aabb = temp;
  final Vec2 lowerBound = aabb.lowerBound;
  final Vec2 upperBound = aabb.upperBound;
  lowerBound.x = Float.MAX_VALUE;
  lowerBound.y = Float.MAX_VALUE;
  upperBound.x = -Float.MAX_VALUE;
  upperBound.y = -Float.MAX_VALUE;
  for (int i = 0; i < m_count; i++) {
    final Vec2 v = m_velocityBuffer.data[i];
    final Vec2 p1 = m_positionBuffer.data[i];
    final float p1x = p1.x;
    final float p1y = p1.y;
    final float p2x = p1x + step.dt * v.x;
    final float p2y = p1y + step.dt * v.y;
    final float bx = p1x < p2x ? p1x : p2x;
    final float by = p1y < p2y ? p1y : p2y;
    lowerBound.x = lowerBound.x < bx ? lowerBound.x : bx;
    lowerBound.y = lowerBound.y < by ? lowerBound.y : by;
    final float b1x = p1x > p2x ? p1x : p2x;
    final float b1y = p1y > p2y ? p1y : p2y;
    upperBound.x = upperBound.x > b1x ? upperBound.x : b1x;
    upperBound.y = upperBound.y > b1y ? upperBound.y : b1y;
  }
  sccallback.step = step;
  sccallback.system = this;
  m_world.queryAABB(sccallback, aabb);
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
public void solveWall(TimeStep step) {
  for (int i = 0; i < m_count; i++) {
    if ((m_flagsBuffer.data[i] & ParticleType.b2_wallParticle) != 0) {
      final Vec2 r = m_velocityBuffer.data[i];
      r.x = 0.0f;
      r.y = 0.0f;
    }
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(final TimeStep step) {
	final Body b1 = m_bodyA;
	final Body b2 = m_bodyB;
	
	final Vec2 r1 = pool.popVec2();
	final Vec2 r2 = pool.popVec2();
	
	r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
	r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
	Mat22.mulToOut(b1.getTransform().R, r1, r1);
	Mat22.mulToOut(b2.getTransform().R, r2, r2);
	
	final Vec2 v1 = pool.popVec2();
	final Vec2 v2 = pool.popVec2();
	
	// Cdot = dot(u, v + cross(w, r))
	Vec2.crossToOut(b1.m_angularVelocity, r1, v1);
	Vec2.crossToOut(b2.m_angularVelocity, r2, v2);
	v1.set(b1.m_linearVelocity).addLocal(b1.m_linearVelocity);
	v2.set(b2.m_linearVelocity).addLocal(b2.m_linearVelocity);
	
	float Cdot = Vec2.dot(m_u, v2.subLocal(v1));
	
	float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
	m_impulse += impulse;
	
	float Px = impulse * m_u.x;
	float Py = impulse * m_u.y;
	b1.m_linearVelocity.x -= b1.m_invMass * Px;
	b1.m_linearVelocity.y -= b1.m_invMass * Py;
	b1.m_angularVelocity -= b1.m_invI * (r1.x * Py - r1.y * Px);// b2Cross(r1, P);
	b2.m_linearVelocity.x += b2.m_invMass * Px;
	b2.m_linearVelocity.y += b2.m_invMass * Py;
	b2.m_angularVelocity += b2.m_invI * (r2.x * Py - r2.y * Px);// b2Cross(r2, P);
	
	pool.pushVec2(4);
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(TimeStep step) {
	Body b = m_bodyB;

	Vec2 r = pool.popVec2();

	r.set(m_localAnchor).subLocal(b.getLocalCenter());
	Mat22.mulToOut(b.getTransform().R, r, r);
	
	// Cdot = v + cross(w, r)
	Vec2 Cdot = pool.popVec2();
	Vec2.crossToOut(b.m_angularVelocity, r, Cdot);
	Cdot.addLocal(b.m_linearVelocity);
	
	Vec2 impulse = pool.popVec2();
	Vec2 temp = pool.popVec2();
	
	//Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
	impulse.set(m_C).mulLocal(m_beta);
	temp.set(m_impulse).mulLocal(m_gamma);
	temp.addLocal(impulse).addLocal(Cdot).mulLocal(-1);
	Mat22.mulToOut(m_mass, temp, impulse);

	Vec2 oldImpulse = temp;
	oldImpulse.set(m_impulse);
	m_impulse.addLocal(impulse);
	float maxImpulse = step.dt * m_maxForce;
	if (m_impulse.lengthSquared() > maxImpulse * maxImpulse){
		m_impulse.mulLocal(maxImpulse / m_impulse.length());
	}
	impulse.set(m_impulse).subLocal(oldImpulse);

	// pooling
	oldImpulse.set(impulse).mulLocal(b.m_invMass);
	b.m_linearVelocity.addLocal(oldImpulse);
	b.m_angularVelocity += b.m_invI * Vec2.cross(r, impulse);
	
	pool.pushVec2(4);
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void initVelocityConstraints(final TimeStep step) {
	m_step = step;
	
	final Vec2[] d = pool.getVec2Array(bodies.length);
	
	for (int i=0; i<bodies.length; ++i) {
		final int prev = (i==0)?bodies.length-1:i-1;
		final int next = (i==bodies.length-1)?0:i+1;
		d[i].set(bodies[next].getWorldCenter());
		d[i].subLocal(bodies[prev].getWorldCenter());
	}

	if (step.warmStarting) {
		m_impulse *= step.dtRatio;
		//float lambda = -2.0f * crossMassSum / dotMassSum;
		//System.out.println(crossMassSum + " " +dotMassSum);
		//lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
		//m_impulse = lambda;
		for (int i=0; i<bodies.length; ++i) {
			bodies[i].m_linearVelocity.x += bodies[i].m_invMass * d[i].y * .5f * m_impulse;
			bodies[i].m_linearVelocity.y += bodies[i].m_invMass * -d[i].x * .5f * m_impulse;
		}
	} else {
		m_impulse = 0.0f;
	}
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void solveVelocityConstraints(final TimeStep step) {
	final Body b1 = m_bodyA;
	final Body b2 = m_bodyB;
	
	final Vec2 r1 = pool.popVec2();
	final Vec2 r2 = pool.popVec2();
	
	r1.set(m_localAnchor1).subLocal(b1.getLocalCenter());
	r2.set(m_localAnchor2).subLocal(b2.getLocalCenter());
	Mat22.mulToOut(b1.getTransform().R, r1, r1);
	Mat22.mulToOut(b2.getTransform().R, r2, r2);
	
	final Vec2 v1 = pool.popVec2();
	final Vec2 v2 = pool.popVec2();
	
	// Cdot = dot(u, v + cross(w, r))
	Vec2.crossToOut(b1.m_angularVelocity, r1, v1);
	Vec2.crossToOut(b2.m_angularVelocity, r2, v2);
	v1.addLocal(b1.m_linearVelocity);
	v2.addLocal(b2.m_linearVelocity);
	
	float Cdot = Vec2.dot(m_u, v2.subLocal(v1));
	
	float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
	m_impulse += impulse;
	
	float Px = impulse * m_u.x;
	float Py = impulse * m_u.y;
	b1.m_linearVelocity.x -= b1.m_invMass * Px;
	b1.m_linearVelocity.y -= b1.m_invMass * Py;
	b1.m_angularVelocity -= b1.m_invI * (r1.x * Py - r1.y * Px);// b2Cross(r1, P);
	b2.m_linearVelocity.x += b2.m_invMass * Px;
	b2.m_linearVelocity.y += b2.m_invMass * Py;
	b2.m_angularVelocity += b2.m_invI * (r2.x * Py - r2.y * Px);// b2Cross(r2, P);
	
	pool.pushVec2(4);
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
@Override
public void initVelocityConstraints(final TimeStep step) {
  m_step = step;

  final Vec2[] d = pool.getVec2Array(bodies.length);

  for (int i = 0; i < bodies.length; ++i) {
    final int prev = (i == 0) ? bodies.length - 1 : i - 1;
    final int next = (i == bodies.length - 1) ? 0 : i + 1;
    d[i].set(bodies[next].getWorldCenter());
    d[i].subLocal(bodies[prev].getWorldCenter());
  }

  if (step.warmStarting) {
    m_impulse *= step.dtRatio;
    // float lambda = -2.0f * crossMassSum / dotMassSum;
    // System.out.println(crossMassSum + " " +dotMassSum);
    // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection,
    // Settings.maxLinearCorrection);
    // m_impulse = lambda;
    for (int i = 0; i < bodies.length; ++i) {
      bodies[i].m_linearVelocity.x += bodies[i].m_invMass * d[i].y * .5f * m_impulse;
      bodies[i].m_linearVelocity.y += bodies[i].m_invMass * -d[i].x * .5f * m_impulse;
    }
  } else {
    m_impulse = 0.0f;
  }
}
 

import org.jbox2d.dynamics.TimeStep; //导入依赖的package包/类
public void solve(TimeStep step) {
  ++m_timestamp;
  if (m_count == 0) {
    return;
  }
  m_allParticleFlags = 0;
  for (int i = 0; i < m_count; i++) {
    m_allParticleFlags |= m_flagsBuffer.data[i];
  }
  if ((m_allParticleFlags & ParticleType.b2_zombieParticle) != 0) {
    solveZombie();
  }
  if (m_count == 0) {
    return;
  }
  m_allGroupFlags = 0;
  for (ParticleGroup group = m_groupList; group != null; group = group.getNext()) {
    m_allGroupFlags |= group.m_groupFlags;
  }
  final float gravityx = step.dt * m_gravityScale * m_world.getGravity().x;
  final float gravityy = step.dt * m_gravityScale * m_world.getGravity().y;
  float criticalVelocytySquared = getCriticalVelocitySquared(step);
  for (int i = 0; i < m_count; i++) {
    Vec2 v = m_velocityBuffer.data[i];
    v.x += gravityx;
    v.y += gravityy;
    float v2 = v.x * v.x + v.y * v.y;
    if (v2 > criticalVelocytySquared) {
      float a = v2 == 0 ? Float.MAX_VALUE : MathUtils.sqrt(criticalVelocytySquared / v2);
      v.x *= a;
      v.y *= a;
    }
  }
  solveCollision(step);
  if ((m_allGroupFlags & ParticleGroupType.b2_rigidParticleGroup) != 0) {
    solveRigid(step);
  }
  if ((m_allParticleFlags & ParticleType.b2_wallParticle) != 0) {
    solveWall(step);
  }
  for (int i = 0; i < m_count; i++) {
    Vec2 pos = m_positionBuffer.data[i];
    Vec2 vel = m_velocityBuffer.data[i];
    pos.x += step.dt * vel.x;
    pos.y += step.dt * vel.y;
  }
  updateBodyContacts();
  updateContacts(false);
  if ((m_allParticleFlags & ParticleType.b2_viscousParticle) != 0) {
    solveViscous(step);
  }
  if ((m_allParticleFlags & ParticleType.b2_powderParticle) != 0) {
    solvePowder(step);
  }
  if ((m_allParticleFlags & ParticleType.b2_tensileParticle) != 0) {
    solveTensile(step);
  }
  if ((m_allParticleFlags & ParticleType.b2_elasticParticle) != 0) {
    solveElastic(step);
  }
  if ((m_allParticleFlags & ParticleType.b2_springParticle) != 0) {
    solveSpring(step);
  }
  if ((m_allGroupFlags & ParticleGroupType.b2_solidParticleGroup) != 0) {
    solveSolid(step);
  }
  if ((m_allParticleFlags & ParticleType.b2_colorMixingParticle) != 0) {
    solveColorMixing(step);
  }
  solvePressure(step);
  solveDamping(step);
}