Java Vector3f.scale方法代码示例

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
/**
 *
 * @param pl line point
 * @param u line vector
 * @param n plane normal
 * @param p point from plane
 * @return intersection of line and plane, if it doesn't exist returns null
 */
public static Vector3f findLinePlaneIntersection(Vector3f pl, Vector3f u, Vector3f n, Vector3f p) {
    Vector3f w = new Vector3f(pl);
    w.sub(p);

    float D = n.dot(u);
    float N = -n.dot(w);

    if (Math.abs(D) == 0) {   // segment is parallel to plane
        if (N == 0) // segment lies in plane
        {
            return pl;
        } else {
            return null;                    // no intersection
        }
    }
    // they are not parallel
    // compute intersect param
    float sI = N / D;

    Vector3f intersection = new Vector3f(u);
    intersection.scale(sI);
    intersection.add(pl);

    return intersection;
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
/**
 * 
 * @param vector  a vector, same as for getNormalDirected
 * @return   return theta 
 */
public float getThetaDirected(Vector3f vector) {
  //fills in .w;
  float theta = getTheta();
  Vector3f v = getNormal();
  if (vector.x * q1 + vector.y * q2 + vector.z * q3 < 0) {
    v.scale(-1);
    theta = -theta;
  }
  return theta;
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
/**
 *
 * @param p0 segment point
 * @param p1 segment point
 * @param n plane normal
 * @param p point from plane
 * @return intersection of segment and plane, if it doesn't exist returns
 * null
 */
public static Vector3f findSegmentPlaneIntersection(Vector3f p0, Vector3f p1, Vector3f n, Vector3f p) {
    Vector3f u = new Vector3f(p1);
    u.sub(p0);
    Vector3f w = new Vector3f(p0);
    w.sub(p);

    float D = n.dot(u);
    float N = -n.dot(w);

    if (Math.abs(D) == 0) {   // segment is parallel to plane
        if (N == 0) // segment lies in plane
        {
            return p0;
        } else {
            return null;                    // no intersection
        }
    }
    // they are not parallel
    // compute intersect param
    float sI = N / D;
    if (sI < 0 || sI > 1) {
        // if (t < 0 || t> 1) {
        return null;                        // no intersection
    }
    Vector3f intersection = new Vector3f(u);
    intersection.scale(sI);
    intersection.add(p0);

    return intersection;
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
private static void scaleDrawing(DrawMesh mesh, float newScale) {
  /*
   * allows for Draw to scale object
   * have to watch out for double-listed vertices
   * 
   */
  if (newScale == 0 || mesh.vertexCount == 0 || mesh.scale == newScale)
    return;
  float f = newScale / mesh.scale;
  mesh.scale = newScale;
  if (mesh.haveXyPoints || mesh.drawType == JmolConstants.DRAW_ARC || mesh.drawType == JmolConstants.DRAW_CIRCLE || mesh.drawType == JmolConstants.DRAW_CIRCULARPLANE)
    return; // done in renderer
  Vector3f diff = new Vector3f();
  int iptlast = -1;
  int ipt = 0;
  for (int i = mesh.polygonCount; --i >= 0;) {
    Point3f center = (mesh.isVector ? mesh.vertices[0] 
        : mesh.ptCenters == null ? mesh.ptCenter
        : mesh.ptCenters[i]);
    if (center == null)
      return;
    if (mesh.polygonIndexes[i] == null)
      continue;
    iptlast = -1;
    for (int iV = mesh.polygonIndexes[i].length; --iV >= 0;) {
      ipt = mesh.polygonIndexes[i][iV];
      if (ipt == iptlast)
        continue;
      iptlast = ipt;
      diff.sub(mesh.vertices[ipt], center);
      diff.scale(f);
      diff.add(center);
      mesh.vertices[ipt].set(diff);
    }
  }
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
private void setDepth(Vector3f depth, Point3f c, Point3f a, Point3f b, float ratio) {
  T1.sub(a, c);
  T1.scale(ratio);
  T2.sub(a, b);
  depth.cross(T1, T2);
  depth.scale(T1.length() / depth.length());
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
@Override
public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0, Vector3f out) {
	int i;
	Vector3f supVec = out;
	supVec.set(0f, 0f, 0f);

	float maxDot = -1e30f;

	Vector3f vec = Stack.alloc(vec0);
	float lenSqr = vec.lengthSquared();
	if (lenSqr < 0.0001f) {
		vec.set(1f, 0f, 0f);
	}
	else {
		float rlen = 1f / (float) Math.sqrt(lenSqr);
		vec.scale(rlen);
	}

	Vector3f vtx = Stack.alloc(Vector3f.class);
	float newDot;

	for (i = 0; i < getNumVertices(); i++) {
		getVertex(i, vtx);
		newDot = vec.dot(vtx);
		if (newDot > maxDot) {
			maxDot = newDot;
			supVec = vtx;
		}
	}

	return out;
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
protected int _calc_splitting_axis(BvhDataArray primitive_boxes, int startIndex, int endIndex) {
	Vector3f means = Stack.alloc(Vector3f.class);
	means.set(0f, 0f, 0f);
	Vector3f variance = Stack.alloc(Vector3f.class);
	variance.set(0f, 0f, 0f);

	int numIndices = endIndex - startIndex;

	Vector3f center = Stack.alloc(Vector3f.class);
	Vector3f diff2 = Stack.alloc(Vector3f.class);

	Vector3f tmp1 = Stack.alloc(Vector3f.class);
	Vector3f tmp2 = Stack.alloc(Vector3f.class);

	for (int i=startIndex; i<endIndex; i++) {
		primitive_boxes.getBoundMax(i, tmp1);
		primitive_boxes.getBoundMin(i, tmp2);
		center.add(tmp1, tmp2);
		center.scale(0.5f);
		means.add(center);
	}
	means.scale(1f / (float)numIndices);

	for (int i=startIndex; i<endIndex; i++) {
		primitive_boxes.getBoundMax(i, tmp1);
		primitive_boxes.getBoundMin(i, tmp2);
		center.add(tmp1, tmp2);
		center.scale(0.5f);
		diff2.sub(center, means);
		VectorUtil.mul(diff2, diff2, diff2);
		variance.add(diff2);
	}
	variance.scale(1f / (float)(numIndices - 1));

	return VectorUtil.maxAxis(variance);
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
@Override
public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
	Vector3f tmp = Stack.alloc(Vector3f.class);

	Vector3f localHalfExtents = Stack.alloc(Vector3f.class);
	localHalfExtents.sub(localAabbMax, localAabbMin);
	localHalfExtents.scale(0.5f);

	Vector3f localCenter = Stack.alloc(Vector3f.class);
	localCenter.add(localAabbMax, localAabbMin);
	localCenter.scale(0.5f);

	Matrix3f abs_b = Stack.alloc(trans.basis);
	MatrixUtil.absolute(abs_b);

	Vector3f center = Stack.alloc(localCenter);
	trans.transform(center);

	Vector3f extent = Stack.alloc(Vector3f.class);
	abs_b.getRow(0, tmp);
	extent.x = tmp.dot(localHalfExtents);
	abs_b.getRow(1, tmp);
	extent.y = tmp.dot(localHalfExtents);
	abs_b.getRow(2, tmp);
	extent.z = tmp.dot(localHalfExtents);

	Vector3f margin = Stack.alloc(Vector3f.class);
	margin.set(getMargin(), getMargin(), getMargin());
	extent.add(margin);

	aabbMin.sub(center, extent);
	aabbMax.add(center, extent);
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
@StaticAlloc
public void applyImpulse(Vector3f impulse, Vector3f rel_pos) {
	if (inverseMass != 0f) {
		applyCentralImpulse(impulse);
		if (angularFactor != 0f) {
			Vector3f tmp = Stack.alloc(Vector3f.class);
			tmp.cross(rel_pos, impulse);
			tmp.scale(angularFactor);
			applyTorqueImpulse(tmp);
		}
	}
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
public void assignAtom(int atomIndex, String type, boolean autoBond) {

    if (type == null)
      type = "C";

    // not as simple as just defining an atom.
    // if we click on an H, and C is being defined,
    // this sprouts an sp3-carbon at that position.

    Atom atom = atoms[atomIndex];
    BitSet bs = new BitSet();
    boolean wasH = (atom.getElementNumber() == 1);
    int atomicNumber = Elements.elementNumberFromSymbol(type, true);

    // 1) change the element type or charge

    boolean isDelete = false;
    if (atomicNumber > 0) {
      setElement(atom, atomicNumber);
      viewer.setShapeSize(JmolConstants.SHAPE_BALLS, 
          viewer.getDefaultRadiusData(), BitSetUtil.setBit(atomIndex));
      setAtomName(atomIndex, type + atom.getAtomNumber());
      if (!models[atom.modelIndex].isModelKit)
        taint(atomIndex, TAINT_ATOMNAME);
    } else if (type.equals("Pl")) {
      atom.setFormalCharge(atom.getFormalCharge() + 1);
    } else if (type.equals("Mi")) {
      atom.setFormalCharge(atom.getFormalCharge() - 1);
    } else if (type.equals("X")) {
      isDelete = true;
    } else if (!type.equals(".")) {
      return; // uninterpretable
    }

    // 2) delete noncovalent bonds and attached hydrogens for that atom.

    removeUnnecessaryBonds(atom, isDelete);

    // 3) adjust distance from previous atom.

    float dx = 0;
    if (atom.getCovalentBondCount() == 1)
      if (wasH) {
        dx = 1.50f;
      } else if (!wasH && atomicNumber == 1) {
        dx = 1.0f;
      }
    if (dx != 0) {
      Vector3f v = new Vector3f(atom);
      v.sub(atoms[atom.getBondedAtomIndex(0)]);
      float d = v.length();
      v.normalize();
      v.scale(dx - d);
      setAtomCoordRelative(atomIndex, v.x, v.y, v.z);
    }

    BitSet bsA = BitSetUtil.setBit(atomIndex);

    if (atomicNumber != 1 && autoBond) {

      // 4) clear out all atoms within 1.0 angstrom
      bspf = null;
      bs = getAtomsWithin(1.0f, bsA, false);
      bs.andNot(bsA);
      if (bs.nextSetBit(0) >= 0)
        viewer.deleteAtoms(bs, false);

      // 5) attach nearby non-hydrogen atoms (rings)

      bs = viewer.getModelUndeletedAtomsBitSet(atom.modelIndex);
      bs.andNot(getAtomBits(Token.hydrogen, null));
      makeConnections(0.1f, 1.8f, 1, JmolConstants.CONNECT_CREATE_ONLY, bsA,
          bs, null, false, 0);

      // 6) add hydrogen atoms

    }
    viewer.addHydrogens(bsA, false, true);
  }
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
@StaticAlloc
public float solveLinearAxis(float timeStep, float jacDiagABInv, RigidBody body1, Vector3f pointInA, RigidBody body2, Vector3f pointInB, int limit_index, Vector3f axis_normal_on_a, Vector3f anchorPos) {
	Vector3f tmp = Stack.alloc(Vector3f.class);
	Vector3f tmpVec = Stack.alloc(Vector3f.class);
	
	// find relative velocity
	Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
	//rel_pos1.sub(pointInA, body1.getCenterOfMassPosition(tmpVec));
	rel_pos1.sub(anchorPos, body1.getCenterOfMassPosition(tmpVec));

	Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
	//rel_pos2.sub(pointInB, body2.getCenterOfMassPosition(tmpVec));
	rel_pos2.sub(anchorPos, body2.getCenterOfMassPosition(tmpVec));

	Vector3f vel1 = body1.getVelocityInLocalPoint(rel_pos1, Stack.alloc(Vector3f.class));
	Vector3f vel2 = body2.getVelocityInLocalPoint(rel_pos2, Stack.alloc(Vector3f.class));
	Vector3f vel = Stack.alloc(Vector3f.class);
	vel.sub(vel1, vel2);

	float rel_vel = axis_normal_on_a.dot(vel);

	// apply displacement correction

	// positional error (zeroth order error)
	tmp.sub(pointInA, pointInB);
	float depth = -(tmp).dot(axis_normal_on_a);
	float lo = -1e30f;
	float hi = 1e30f;

	float minLimit = VectorUtil.getCoord(lowerLimit, limit_index);
	float maxLimit = VectorUtil.getCoord(upperLimit, limit_index);

	// handle the limits
	if (minLimit < maxLimit) {
		{
			if (depth > maxLimit) {
				depth -= maxLimit;
				lo = 0f;

			}
			else {
				if (depth < minLimit) {
					depth -= minLimit;
					hi = 0f;
				}
				else {
					return 0.0f;
				}
			}
		}
	}

	float normalImpulse = limitSoftness * (restitution * depth / timeStep - damping * rel_vel) * jacDiagABInv;

	float oldNormalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index);
	float sum = oldNormalImpulse + normalImpulse;
	VectorUtil.setCoord(accumulatedImpulse, limit_index, sum > hi ? 0f : sum < lo ? 0f : sum);
	normalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index) - oldNormalImpulse;

	Vector3f impulse_vector = Stack.alloc(Vector3f.class);
	impulse_vector.scale(normalImpulse, axis_normal_on_a);
	body1.applyImpulse(impulse_vector, rel_pos1);

	tmp.negate(impulse_vector);
	body2.applyImpulse(tmp, rel_pos2);
	return normalImpulse;
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
public void calculateTemporalAabb(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Vector3f temporalAabbMin, Vector3f temporalAabbMax) {
	//start with static aabb
	getAabb(curTrans, temporalAabbMin, temporalAabbMax);

	float temporalAabbMaxx = temporalAabbMax.x;
	float temporalAabbMaxy = temporalAabbMax.y;
	float temporalAabbMaxz = temporalAabbMax.z;
	float temporalAabbMinx = temporalAabbMin.x;
	float temporalAabbMiny = temporalAabbMin.y;
	float temporalAabbMinz = temporalAabbMin.z;

	// add linear motion
	Vector3f linMotion = Stack.alloc(linvel);
	linMotion.scale(timeStep);

	//todo: simd would have a vector max/min operation, instead of per-element access
	if (linMotion.x > 0f) {
		temporalAabbMaxx += linMotion.x;
	}
	else {
		temporalAabbMinx += linMotion.x;
	}
	if (linMotion.y > 0f) {
		temporalAabbMaxy += linMotion.y;
	}
	else {
		temporalAabbMiny += linMotion.y;
	}
	if (linMotion.z > 0f) {
		temporalAabbMaxz += linMotion.z;
	}
	else {
		temporalAabbMinz += linMotion.z;
	}

	//add conservative angular motion
	float angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
	Vector3f angularMotion3d = Stack.alloc(Vector3f.class);
	angularMotion3d.set(angularMotion, angularMotion, angularMotion);
	temporalAabbMin.set(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
	temporalAabbMax.set(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);

	temporalAabbMin.sub(angularMotion3d);
	temporalAabbMax.add(angularMotion3d);
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
public Vertex getMiddle() {
    Vector3f mid = new Vector3f(u.position);
    mid.add(v.position);
    mid.scale(0.5f);
    return new Vertex(mid);
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
public HingeConstraint(RigidBody rbA, Vector3f pivotInA, Vector3f axisInA) {
	super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA);
	angularOnly = false;
	enableAngularMotor = false;

	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	Vector3f rbAxisA1 = Stack.alloc(Vector3f.class);
	Transform centerOfMassA = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class));
	centerOfMassA.basis.getColumn(0, rbAxisA1);

	float projection = rbAxisA1.dot(axisInA);
	if (projection > BulletGlobals.FLT_EPSILON) {
		rbAxisA1.scale(projection);
		rbAxisA1.sub(axisInA);
	}
	else {
		centerOfMassA.basis.getColumn(1, rbAxisA1);
	}

	Vector3f rbAxisA2 = Stack.alloc(Vector3f.class);
	rbAxisA2.cross(axisInA, rbAxisA1);

	rbAFrame.origin.set(pivotInA);
	rbAFrame.basis.setRow(0, rbAxisA1.x, rbAxisA2.x, axisInA.x);
	rbAFrame.basis.setRow(1, rbAxisA1.y, rbAxisA2.y, axisInA.y);
	rbAFrame.basis.setRow(2, rbAxisA1.z, rbAxisA2.z, axisInA.z);

	Vector3f axisInB = Stack.alloc(Vector3f.class);
	axisInB.negate(axisInA);
	centerOfMassA.basis.transform(axisInB);

	Quat4f rotationArc = QuaternionUtil.shortestArcQuat(axisInA, axisInB, Stack.alloc(Quat4f.class));
	Vector3f rbAxisB1 = QuaternionUtil.quatRotate(rotationArc, rbAxisA1, Stack.alloc(Vector3f.class));
	Vector3f rbAxisB2 = Stack.alloc(Vector3f.class);
	rbAxisB2.cross(axisInB, rbAxisB1);

	rbBFrame.origin.set(pivotInA);
	centerOfMassA.transform(rbBFrame.origin);
	rbBFrame.basis.setRow(0, rbAxisB1.x, rbAxisB2.x, axisInB.x);
	rbBFrame.basis.setRow(1, rbAxisB1.y, rbAxisB2.y, axisInB.y);
	rbBFrame.basis.setRow(2, rbAxisB1.z, rbAxisB2.z, axisInB.z);

	// start with free
	lowerLimit = 1e30f;
	upperLimit = -1e30f;
	biasFactor = 0.3f;
	relaxationFactor = 1.0f;
	limitSoftness = 0.9f;
	solveLimit = false;
}
 

import javax.vecmath.Vector3f; //导入方法依赖的package包/类
/**
 * velocity + friction
 * response  between two dynamic objects with friction
 */
private float resolveSingleCollisionCombinedCacheFriendly(
		SolverBody body1,
		SolverBody body2,
		SolverConstraint contactConstraint,
		ContactSolverInfo solverInfo) {

	float normalImpulse;

	{
		//  Optimized version of projected relative velocity, use precomputed cross products with normal
		//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
		//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
		//	btVector3 vel = vel1 - vel2;
		//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);

		float rel_vel;
		float vel1Dotn = contactConstraint.contactNormal.dot(body1.linearVelocity) + contactConstraint.relpos1CrossNormal.dot(body1.angularVelocity);
		float vel2Dotn = contactConstraint.contactNormal.dot(body2.linearVelocity) + contactConstraint.relpos2CrossNormal.dot(body2.angularVelocity);

		rel_vel = vel1Dotn - vel2Dotn;

		float positionalError = 0.f;
		if (!solverInfo.splitImpulse || (contactConstraint.penetration > solverInfo.splitImpulsePenetrationThreshold)) {
			positionalError = -contactConstraint.penetration * solverInfo.erp / solverInfo.timeStep;
		}

		float velocityError = contactConstraint.restitution - rel_vel;// * damping;

		float penetrationImpulse = positionalError * contactConstraint.jacDiagABInv;
		float velocityImpulse = velocityError * contactConstraint.jacDiagABInv;
		normalImpulse = penetrationImpulse + velocityImpulse;


		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
		float oldNormalImpulse = contactConstraint.appliedImpulse;
		float sum = oldNormalImpulse + normalImpulse;
		contactConstraint.appliedImpulse = 0f > sum ? 0f : sum;

		normalImpulse = contactConstraint.appliedImpulse - oldNormalImpulse;

		Vector3f tmp = Stack.alloc(Vector3f.class);

		tmp.scale(body1.invMass, contactConstraint.contactNormal);
		body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, normalImpulse);

		tmp.scale(body2.invMass, contactConstraint.contactNormal);
		body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -normalImpulse);
	}

	return normalImpulse;
}