@Override
public void eyePtChanged() {
Point3d eyePt = getViewPosInLocal(frontRoot);
// System.out.println("eye pt = " + eyePt)
if (eyePt != null) {
for (int i = 0; i < 6; i++) {
// if (perspectiveAttr.getValue() == true)
// {
// eyeVec.sub(eyePt, faceCenter[i]);
// } else
{
eyeVec.sub(eyePt, volCenter);
}
frontFaceBits.set(i);
backFaceBits.clear(i);
if (eyeVec.dot(faceNormal[i]) < 0) {
backFaceBits.set(i);
frontFaceBits.clear(i);
} else {
frontFaceBits.set(i);
backFaceBits.clear(i);
}
}
frontAnnotations.setChildMask(frontFaceBits);
backAnnotations.setChildMask(backFaceBits);
}
}
private void yawCamera(double angle) {
Vector3d backToZero;
Point3d center;
if (choosingPivotPoint && pivotPointAdjustor.getPivotPointBG() != null) {
center = pivotPointAdjustor.getPivotPointPos();
} else {
center = getObjCenter();
}
backToZero = new Vector3d(center);
backToZero.negate();
Transform3D back = new Transform3D();
back.set(backToZero);
Transform3D rotate = new Transform3D();
rotate.set(new AxisAngle4d(up, angle));
Transform3D toOri = new Transform3D();
toOri.set(new Vector3d(center));
// creating composite transform, reversing the order
Transform3D fullRotate = new Transform3D(toOri);
fullRotate.mul(rotate);
fullRotate.mul(back);
fullRotate.transform(cameraPos);
fullRotate.transform(cameraFocus);
}
public void lookAt(Vector3d target) {
Vector3d targetDirection = new Vector3d();
targetDirection.sub(target, getPosition());
targetDirection.normalize();
setPitchYawFromVector(targetDirection);
}
protected boolean recoverFromPenetration(CollisionWorld collisionWorld) {
boolean penetration = false;
collisionWorld
.getDispatcher()
.dispatchAllCollisionPairs(
ghostObject.getOverlappingPairCache(),
collisionWorld.getDispatchInfo(),
collisionWorld.getDispatcher());
currentPosition.set(ghostObject.getWorldTransform(new Transform()).origin);
double maxPen = 0.0f;
for (int i = 0; i < ghostObject.getOverlappingPairCache().getNumOverlappingPairs(); i++) {
manifoldArray.clear();
BroadphasePair collisionPair =
ghostObject.getOverlappingPairCache().getOverlappingPairArray().getQuick(i);
if (collisionPair.algorithm != null) {
collisionPair.algorithm.getAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.size(); j++) {
PersistentManifold manifold = manifoldArray.getQuick(j);
double directionSign = manifold.getBody0() == ghostObject ? -1.0f : 1.0f;
for (int p = 0; p < manifold.getNumContacts(); p++) {
ManifoldPoint pt = manifold.getContactPoint(p);
double dist = pt.getDistance();
if (dist < 0.0f) {
if (dist < maxPen) {
maxPen = dist;
touchingNormal.set(pt.normalWorldOnB); // ??
touchingNormal.scale(directionSign);
}
currentPosition.scaleAdd(
directionSign * dist * 0.2f, pt.normalWorldOnB, currentPosition);
penetration = true;
} else {
// printf("touching %f\n", dist);
}
}
// manifold->clearManifold();
}
}
Transform newTrans = ghostObject.getWorldTransform(new Transform());
newTrans.origin.set(currentPosition);
ghostObject.setWorldTransform(newTrans);
// printf("m_touchingNormal =
// %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
// System.out.println("recoverFromPenetration "+penetration+" "+touchingNormal);
return penetration;
}
private void dotest(final Record rec) {
PolyTree ptree1;
PolyTree ptree2;
ptree1 = (PolyTree) bodies.get(rec.poly1);
if (ptree1 == null) {
System.err.println("PolyTree " + rec.poly1 + " not found");
System.exit(1);
}
ptree2 = (PolyTree) bodies.get(rec.poly2);
if (ptree2 == null) {
System.err.println("PolyTree " + rec.poly2 + " not found");
System.exit(1);
}
double[] dlist;
final Vector3d offset = new Vector3d();
if (ptree1.numNodes() > 0 || ptree2.numNodes() > 0)
dlist = new double[] {-0.1, 0.1, rec.minDist};
else dlist = new double[] {-0.1, 0.1, 1.0, rec.minDist};
// dlist = new double[] { rec.minDist };
for (final double element : dlist) {
offset.scale(element - rec.minDist, rec.minNrml);
if (exhaustive) exhaustiveCheck(rec, offset, ptree1, ptree2);
else singleCheck(rec, offset, ptree1, ptree2, null, null);
}
}
/**
* setLinkStatus
*
* <p>Debugようにこのメソッドを叩くと現在のRobotの状態をIntegratorに通知する
*
* @param objectName
*/
private void _setLinkStatus(String objectName) {
if (robot_ == null) robot_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, objectName);
TransformGroup tg = robot_.getTransformGroupRoot();
Transform3D t3d = new Transform3D();
tg.getTransform(t3d);
Vector3d pos = new Vector3d();
Matrix3d mat = new Matrix3d();
t3d.get(mat, pos);
double[] value = new double[12];
pos.get(value);
value[3] = mat.m00;
value[4] = mat.m01;
value[5] = mat.m02;
value[6] = mat.m10;
value[7] = mat.m11;
value[8] = mat.m12;
value[9] = mat.m20;
value[10] = mat.m21;
value[11] = mat.m22;
integrator_.setCharacterLinkData(
objectName, robot_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, value);
integrator_.setCharacterAllLinkData(
objectName, LinkDataType.JOINT_VALUE, robot_.getJointValues());
integrator_.calcCharacterForwardKinematics(objectName);
}
/**
* Returns the reflection direction of a ray going 'direction' hitting a surface with normal
* 'normal'.
*
* <p>From: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
*/
protected Vector3d computeReflectionDirection(Vector3d direction, Vector3d normal, Vector3d out) {
// return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
out.set(normal);
out.scale(-2.0f * direction.dot(normal));
out.add(direction);
return out;
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 300000)
public void testTwoIntersectingBoxes() {
CombinedTerrainObject3D combinedTerrainObject =
new CombinedTerrainObject3D("Combined Terrain Object to Test");
Point3d pointToCheck = new Point3d();
Point3d expectedIntersection = new Point3d();
Vector3d expectedNormal = new Vector3d();
Point3d resultIntersection = new Point3d();
Vector3d resultNormal = new Vector3d();
setupTwoIntersectingBoxesMadeFromPolygons(combinedTerrainObject);
pointToCheck.set(0.4, 0.45, 0.25);
expectedIntersection.set(0.4, 0.5, 0.25);
expectedNormal.set(0.0, 1.0, 0.0);
combinedTerrainObject.checkIfInside(
pointToCheck.getX(),
pointToCheck.getY(),
pointToCheck.getZ(),
resultIntersection,
resultNormal);
JUnitTools.assertTuple3dEquals(expectedIntersection, resultIntersection, 1e-4);
JUnitTools.assertTuple3dEquals(expectedNormal, resultNormal, 1e-4);
}
// Right hand rule for camera:
// index finger: cameraPos to cameraFocus
// thumb: up
// middle finger:c cameraPos x up
private Vector3d getCameraDir() {
Vector3d camDir = new Vector3d();
camDir.x = cameraFocus.x - cameraPos.x;
camDir.y = cameraFocus.y - cameraPos.y;
camDir.z = cameraFocus.z - cameraPos.z;
return camDir;
}
public void setViewingDirection(double yaw, double pitch) {
_viewingDirection.set(
Math.sin(Math.toRadians(yaw)) * Math.cos(Math.toRadians(pitch)),
-Math.sin(Math.toRadians(pitch)),
-Math.cos(Math.toRadians(pitch)) * Math.cos(Math.toRadians(yaw)));
_viewingDirection.normalize(_viewingDirection);
}
/** Returns the portion of 'direction' that is parallel to 'normal' */
protected Vector3d parallelComponent(Vector3d direction, Vector3d normal, Vector3d out) {
// btScalar magnitude = direction.dot(normal);
// return normal * magnitude;
out.set(normal);
out.scale(direction.dot(normal));
return out;
}
/** Updates the status if the entity is currently swimming (in water). */
private void updateSwimStatus() {
ArrayList<BlockPosition> blockPositions = gatherAdjacentBlockPositions(getPosition());
boolean swimming = false, headUnderWater = false;
for (int i = 0; i < blockPositions.size(); i++) {
BlockPosition p = blockPositions.get(i);
byte blockType = _parent.getWorldProvider().getBlockAtPosition(new Vector3d(p.x, p.y, p.z));
AABB blockAABB = Block.AABBForBlockAt(p.x, p.y, p.z);
if (BlockManager.getInstance().getBlock(blockType).isLiquid()
&& getAABB().overlaps(blockAABB)) {
swimming = true;
}
Vector3d eyePos = calcEyePosition();
eyePos.y += 0.25;
if (BlockManager.getInstance().getBlock(blockType).isLiquid() && blockAABB.contains(eyePos)) {
headUnderWater = true;
}
}
_headUnderWater = headUnderWater;
_isSwimming = swimming;
}
/**
* Returns the normal of the plane closest to the given origin.
*
* @param pointOnAABB A point on the AABB
* @param origin The origin
* @param testX True if the x-axis should be tested
* @param testY True if the y-axis should be tested
* @param testZ True if the z-axis should be tested
* @return The normal
*/
public Vector3d normalForPlaneClosestToOrigin(
Vector3d pointOnAABB, Vector3d origin, boolean testX, boolean testY, boolean testZ) {
ArrayList<Vector3d> normals = new ArrayList<Vector3d>();
if (pointOnAABB.z == minZ() && testZ) normals.add(new Vector3d(0, 0, -1));
if (pointOnAABB.z == maxZ() && testZ) normals.add(new Vector3d(0, 0, 1));
if (pointOnAABB.x == minX() && testX) normals.add(new Vector3d(-1, 0, 0));
if (pointOnAABB.x == maxX() && testX) normals.add(new Vector3d(1, 0, 0));
if (pointOnAABB.y == minY() && testY) normals.add(new Vector3d(0, -1, 0));
if (pointOnAABB.y == maxY() && testY) normals.add(new Vector3d(0, 1, 0));
double minDistance = Double.MAX_VALUE;
Vector3d closestNormal = new Vector3d();
for (int i = 0; i < normals.size(); i++) {
Vector3d n = normals.get(i);
Vector3d diff = new Vector3d(centerPointForNormal(n));
diff.sub(origin);
double distance = diff.length();
if (distance < minDistance) {
minDistance = distance;
closestNormal = n;
}
}
return closestNormal;
}
/** Returns the portion of 'direction' that is perpindicular to 'normal' */
protected Vector3d perpindicularComponent(Vector3d direction, Vector3d normal, Vector3d out) {
// return direction - parallelComponent(direction, normal);
Vector3d perpendicular = parallelComponent(direction, normal, out);
perpendicular.scale(-1);
perpendicular.add(direction);
return perpendicular;
}
/**
* Return a midpoint of a helix, calculated from three positions of three adjacent subunit
* centers.
*
* @param p1 center of first subunit
* @param p2 center of second subunit
* @param p3 center of third subunit
* @return midpoint of helix
*/
private Point3d getMidPoint(Point3d p1, Point3d p2, Point3d p3) {
Vector3d v1 = new Vector3d();
v1.sub(p1, p2);
Vector3d v2 = new Vector3d();
v2.sub(p3, p2);
Vector3d v3 = new Vector3d();
v3.add(v1);
v3.add(v2);
v3.normalize();
// calculat the total distance between to subunits
double dTotal = v1.length();
// calculate the rise along the y-axis. The helix axis is aligned with y-axis,
// therfore, the rise between subunits is the y-distance
double rise = p2.y - p1.y;
// use phythagorean theoremm to calculate chord length between two subunit centers
double chord = Math.sqrt(dTotal * dTotal - rise * rise);
// System.out.println("Chord d: " + dTotal + " rise: " + rise + "chord: " + chord);
double angle = helixLayers.getByLargestContacts().getAxisAngle().angle;
// using the axis angle and the chord length, we can calculate the radius of the helix
// http://en.wikipedia.org/wiki/Chord_%28geometry%29
double radius = chord / Math.sin(angle / 2) / 2; // can this go to zero?
// System.out.println("Radius: " + radius);
// project the radius onto the vector that points toward the helix axis
v3.scale(radius);
v3.add(p2);
// System.out.println("Angle: " +
// Math.toDegrees(helixLayers.getByLowestAngle().getAxisAngle().angle));
Point3d cor = new Point3d(v3);
return cor;
}
public void propertyChange(PropertyChangeEvent pce) {
Object source = pce.getSource();
if (source == posSlider_x) {
double posX = ((Double) pce.getNewValue()).doubleValue();
posDisk.x = posX;
Disk.setNode3D(ShapeNodeDisk);
Disk.setPosition(posDisk);
PlaceBNVectors();
} else if (source == posSlider_y) {
double posY = ((Double) pce.getNewValue()).doubleValue();
posDisk.y = posY;
Disk.setNode3D(ShapeNodeDisk);
Disk.setPosition(posDisk);
PlaceBNVectors();
} else if (source == angDisk) {
angleDisk = ((Double) pce.getNewValue()).doubleValue();
double angDisk_rad = angleDisk * Math.PI / 180.;
double compx = Math.cos(angDisk_rad);
double compy = Math.sin(angDisk_rad);
Disk.setNode3D(ShapeNodeDisk);
Disk.setDirection(new Vector3d(compx, compy, 0.));
flux_plot.setNormalDisk(new Vector3d(compx, compy, 0.));
PlaceBNVectors();
} else if (source == radDisk) {
radiusDisk = ((Double) pce.getNewValue()).doubleValue();
ShapeNodeDisk.setGeometry(Cylinder.makeGeometry(32, radiusDisk, heightDisk));
Disk.setNode3D(ShapeNodeDisk);
arrowScale = arrowScaleInitial * radiusDisk / radiusDiskInitial;
flux_plot.setRadiusDisk(radiusDisk);
PlaceBNVectors();
} else {
super.propertyChange(pce);
}
}
// initializes alignment axis
void initAlignmentAxis(float x, float y, float z) {
this.axis.set(x, y, z);
double invMag;
invMag = 1.0 / Math.sqrt(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
nAxis.x = (double) axis.x * invMag;
nAxis.y = (double) axis.y * invMag;
nAxis.z = (double) axis.z * invMag;
}
/* (non-Javadoc)
* @see org.biojava.nbio.structure.quaternary.core.AxisAligner#getDimension()
*/
@Override
public Vector3d getDimension() {
run();
Vector3d dimension = new Vector3d();
dimension.sub(maxBoundary, minBoundary);
dimension.scale(0.5);
return dimension;
}
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(
int nthWall) {
double alpha = ((double) nthWall) / 7.0;
Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha);
startingLocation.add(new Vector3d(-1.0 - 0.1, 1.0 - 0.1, 0.0));
return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(
startingLocation, yaw - Math.PI / 2.0);
}
@Override
public GroundProfile3D getGroundProfile() {
Vector3d surfaceNormal = new Vector3d(0.03, 0.27, 1.0);
surfaceNormal.normalize();
Point3d intersectionPoint = new Point3d(0.22, 2.2, -0.4);
double maxXY = 10.0;
return new SlopedPlaneGroundProfile(surfaceNormal, intersectionPoint, maxXY);
}
void doTranslate(double deltaX, double deltaY) {
Vector3d v = new Vector3d(deltaX, deltaY, 0d);
vt.transform(v);
if (lockZ.isSelected()) {
v.z = 0d;
}
parent.getModel().translateObject(v.x, v.y, v.z);
}
public void perturb(Vector3d direction) {
Vector3d force = new Vector3d(direction);
if (direction.lengthSquared() > 0.0) {
force.normalize();
force.scale(disturbanceMagnitude.getDoubleValue());
forcePerturbable.setForcePerturbance(force, disturbanceDuration.getDoubleValue());
}
}
// static helper method
private static Vector3d getNormalizedVector(Vector3d v, Vector3d out) {
out.set(v);
out.normalize();
if (out.length() < BulletGlobals.SIMD_EPSILON) {
out.set(0, 0, 0);
}
return out;
}
boolean check(final ClosestPointPair cpair) {
final Vector3d del = new Vector3d();
del.sub(cpair.pnt2, cpair.pnt1);
if (Math.abs(del.length() - d) > TOL) return false;
del.normalize();
if (!vec.epsilonEquals(del, TOL)) return false;
return true;
}
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup
createInitialSetupAtNthPlatformToesTouching(int nthPlatform) {
double alpha = ((double) nthPlatform) / 7.0;
Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha);
startingLocation.add(new Vector3d(-0.11, -0.16, 0.0));
return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw);
}
// gets a point not on vector a...b; this can be used to define a plan or cross products
private static Vector3d getNonColinearVector(Vector3d ab) {
Vector3d cr = new Vector3d();
cr.cross(ab, XV);
if (cr.length() > 0.00001) {
return XV;
} else {
return YV;
}
}
/**
* @param datasets input 2D dataset
* @return one 2D dataset
*/
@Override
public List<Dataset> value(IDataset... datasets) {
if (datasets.length == 0) return null;
List<Dataset> result = new ArrayList<Dataset>();
for (IDataset ids : datasets) {
Dataset ds = DatasetUtils.convertToDataset(ids);
// check if input is 2D
int[] s = ds.getShape();
if (s.length != 2) return null;
// find extent of projected dataset
int[] pos = new int[2];
int[] start = pshape.clone();
int[] stop = new int[2];
qToPixel(qspace.qFromPixelPosition(0, 0), pos);
adjustBoundingBox(start, stop, pos);
qToPixel(qspace.qFromPixelPosition(s[0], 0), pos);
adjustBoundingBox(start, stop, pos);
qToPixel(qspace.qFromPixelPosition(0, s[1]), pos);
adjustBoundingBox(start, stop, pos);
qToPixel(qspace.qFromPixelPosition(s[0], s[1]), pos);
adjustBoundingBox(start, stop, pos);
stop[0]++;
stop[1]++;
// iterate over bounding box in project dataset
// find image point
// calculate interpolated value
// store running average
// (could have done this using a slice iterator)
Vector3d qy = new Vector3d();
Vector3d q = new Vector3d();
Vector3d t = new Vector3d();
double delta, value;
int i = 0, n;
for (int y = start[0]; y < stop[0]; y++) {
qy.scale((y + roff) * cdel, col);
for (int x = start[1]; x < stop[1]; x++) {
q.scaleAdd((x + coff) * rdel, row, qy);
qspace.pixelPosition(q, t);
n = (int) (count.getElementLongAbs(i) + 1);
value = image.getElementDoubleAbs(i);
delta = Maths.interpolate(ds, t.y, t.x) - value;
image.setObjectAbs(i, value + (delta / n));
count.setObjectAbs(i, n);
i++;
}
}
result.add(image);
result.add(count);
}
return result;
}
public static Vector3d morph(Vector3d point1, Vector3d point2, double alpha) {
Vector3d framePoint1 = new Vector3d(point1);
Vector3d framePoint2 = new Vector3d(point2);
framePoint1.scale(1.0 - alpha);
framePoint2.scale(alpha);
framePoint1.add(framePoint2);
return framePoint1;
}
/**
* Convert from q to a pixel coordinate (discretization)
*
* @param q
* @param pos (to be used for a dataset so is row-major: x->2, y->1)
*/
private void qToPixel(final Vector3d q, int[] pos) {
pos[0] = -1;
pos[1] = -1;
final int x = (int) Math.floor(q.dot(row) / rdel) - roff;
pos[1] = (x < 0 || x > pshape[1]) ? -1 : x;
final int y = (int) Math.floor(q.dot(col) / cdel) - coff;
pos[0] = (y < 0 || y > pshape[0]) ? -1 : y;
}
public void playerStep(CollisionWorld collisionWorld, double dt) {
// printf("playerStep(): ");
// printf(" dt = %f", dt);
// quick check...
if (!useWalkDirection && velocityTimeInterval <= 0.0f) {
// printf("\n");
return; // no motion
}
wasOnGround = onGround();
// Update fall velocity.
verticalVelocity -= gravity * dt;
if (verticalVelocity > 0.0 && verticalVelocity > jumpSpeed) {
verticalVelocity = jumpSpeed;
}
if (verticalVelocity < 0.0 && Math.abs(verticalVelocity) > Math.abs(fallSpeed)) {
verticalVelocity = -Math.abs(fallSpeed);
}
verticalOffset = verticalVelocity * dt;
Transform xform = ghostObject.getWorldTransform(new Transform());
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp(collisionWorld);
if (useWalkDirection) {
// System.out.println("playerStep 3");
stepForwardAndStrafe(collisionWorld, walkDirection);
} else {
System.out.println("playerStep 4");
// printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
double dtMoving = (dt < velocityTimeInterval) ? dt : velocityTimeInterval;
velocityTimeInterval -= dt;
// how far will we move while we are moving?
Vector3d move = new Vector3d();
move.scale(dtMoving, walkDirection);
// printf(" dtMoving: %f", dtMoving);
// okay, step
stepForwardAndStrafe(collisionWorld, move);
}
stepDown(collisionWorld, dt);
// printf("\n");
xform.origin.set(currentPosition);
ghostObject.setWorldTransform(xform);
}