/**
* 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;
}
/** 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;
}
/**
* 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;
}
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);
}
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);
}
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;
}
/** Method to place the magnetic field vectors and normals on the disk. */
public void PlaceBNVectors() {
// first place the vectors on the top of the cylinder
Vector3d normalTop = null;
Vector3d centerTop = new Vector3d(0, 0, 0);
double compx = Math.cos(angleDisk * Math.PI / 180.);
double compy = Math.sin(angleDisk * Math.PI / 180.);
normalTop = new Vector3d(compx, compy, 0.);
normalTop.scale(heightDisk / 2.);
centerTop.add(normalTop);
centerTop.add(posDisk);
for (int j = 0; j < numRadDisk; j++) {
double rad = (j + 1) * radiusDisk / (numRadDisk + 1);
for (int i = 0; i < numAziTopDisk; i++) {
double aziangle = i * 2. * Math.PI / (numAziTopDisk * 1.);
Vector3d azipos = new Vector3d(0., Math.cos(aziangle), Math.sin(aziangle));
Vector3d aziposTrans = new Vector3d(0, 0, 0);
Vector3d azidirTrans = new Vector3d(1, 0, 0);
Vector3d azidir = new Vector3d(1., 0., 0.);
azipos.scale(rad);
aziposTrans.x = azipos.x * compx - azipos.y * compy;
aziposTrans.y = azipos.x * compy + azipos.y * compx;
aziposTrans.z = azipos.z;
azidirTrans.x = azidir.x * compx - azidir.y * compy;
azidirTrans.y = azidir.x * compy + azidir.y * compx;
azidirTrans.z = azidir.z;
aziposTrans.add(centerTop);
theFieldDiskTop[i][j].setPosition(aziposTrans);
theFieldDiskTop[i][j].setScale(arrowScale);
theNormalDiskTop[i][j].setPosition(aziposTrans);
theNormalDiskTop[i][j].setValue(azidirTrans);
theNormalDiskTop[i][j].setDrawn(true);
theNormalDiskTop[i][j].setScale(arrowScale);
// here we make the field vector tip be at the location of the arrow if the arrow points
// inward at the local normal
if (theEngine != null) theEngine.requestSpatial();
}
}
}
protected void updateClosedJoint() {
super.updateClosedJoint();
integralForce.scale(
integralStiffness.getDoubleValue(), yoConnectionPositionIntegratedError.getFrameTuple());
connectionPointA.getForce(tempForce);
tempForce.add(integralForce.getVector());
connectionPointA.setForce(tempForce);
tempForce.scale(-1.0);
connectionPointB.setForce(tempForce);
}
/**
* Calculate new point(s) X in a B-A-C system. It forms form a B-A(-C)-X system.
*
* <p>(2) 2 ligands(B, C) of refAtom A (i) 1 points required; vector in ABC plane bisecting AB,
* AC. If ABC is linear, no points (ii) 2 points: 2 points X1, X2, X1-A-X2 = angle about 2i vector
*
* @param aPoint to which substituents are added
* @param bPoint first ligand of A
* @param cPoint second ligand of A
* @param nwanted number of points to calculate (1-2)
* @param length A-X length
* @param angle B-A-X angle
* @return Point3d[] nwanted points (or zero if failed)
*/
public static Point3d[] calculate3DCoordinates2(
Point3d aPoint, Point3d bPoint, Point3d cPoint, int nwanted, double length, double angle) {
Point3d newPoints[] = new Point3d[0];
double ang2 = angle / 2.0;
Vector3d ba = new Vector3d(aPoint);
ba.sub(bPoint);
Vector3d ca = new Vector3d(aPoint);
ca.sub(cPoint);
Vector3d baxca = new Vector3d();
baxca.cross(ba, ca);
if (baxca.length() < 0.00000001) {; // linear
} else if (nwanted == 1) {
newPoints = new Point3d[1];
Vector3d ax = new Vector3d(ba);
ax.add(ca);
ax.normalize();
ax.scale(length);
newPoints[0] = new Point3d(aPoint);
newPoints[0].add(ax);
} else if (nwanted == 2) {
newPoints = new Point3d[2];
Vector3d ax = new Vector3d(ba);
ax.add(ca);
ax.normalize();
baxca.normalize();
baxca.scale(Math.sin(ang2) * length);
ax.scale(Math.cos(ang2) * length);
newPoints[0] = new Point3d(aPoint);
newPoints[0].add(ax);
newPoints[0].add(baxca);
newPoints[1] = new Point3d(aPoint);
newPoints[1].add(ax);
newPoints[1].sub(baxca);
}
return newPoints;
}
@SuppressWarnings("all")
protected void updateTargetPositionBasedOnCollision(
Vector3d hitNormal, double tangentMag, double normalMag) {
Vector3d movementDirection = new Vector3d();
movementDirection.sub(targetPosition, currentPosition);
double movementLength = movementDirection.length();
if (movementLength > BulletGlobals.SIMD_EPSILON) {
movementDirection.normalize();
Vector3d reflectDir =
computeReflectionDirection(movementDirection, hitNormal, new Vector3d());
reflectDir.normalize();
Vector3d parallelDir = parallelComponent(reflectDir, hitNormal, new Vector3d());
Vector3d perpindicularDir = perpindicularComponent(reflectDir, hitNormal, new Vector3d());
targetPosition.set(currentPosition);
if (false) // tangentMag != 0.0)
{
Vector3d parComponent = new Vector3d();
parComponent.scale(tangentMag * movementLength, parallelDir);
// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
targetPosition.add(parComponent);
}
if (normalMag != 0.0f) {
Vector3d perpComponent = new Vector3d();
perpComponent.scale(normalMag * movementLength, perpindicularDir);
// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
targetPosition.add(perpComponent);
}
} else {
// printf("movementLength don't normalize a zero vector\n");
}
}
void translate(final int xNew, final int yNew) {
if (content == null || content.isLocked()) return;
final int dx = xNew - xLast;
final int dy = yNew - yLast;
translateTG.getTransform(translateOld);
v1.scale(dx, translationPerDx);
v2.scale(-dy, translationPerDy);
translation.add(v1, v2);
translateNew.set(translation);
translateNew.mul(translateOld);
translateTG.setTransform(translateNew);
transformChanged(BehaviorCallback.TRANSLATE, translateNew);
xLast = xNew;
yLast = yNew;
}
public Vector3d getReaction(Vector3d position, Vector3d velocity, Vector3d action, double mass) {
Vector3d relativePosition = new Vector3d(position);
relativePosition.sub(point);
double x = relativePosition.length();
Vector3d reaction = new Vector3d(relativePosition);
if (x > Teal.DoubleZero) {
reaction.scale(-k1);
}
double v = velocity.length();
Vector3d damping = new Vector3d(velocity);
if (v > Teal.DoubleZero) {
// damping.scale(1./v);
damping.scale(-k2 * (1.0 + 0.0 / v) / (1. + (x / p) * (x / p)));
reaction.add(damping);
}
lastReaction.set(reaction);
return reaction;
}
/**
* Calculate new point X in a B-A(-D)-C system. It forms a B-A(-D)(-C)-X system.
*
* <p>(3) 3 ligands(B, C, D) of refAtom A (i) 1 points required; if A, B, C, D coplanar, no
* points. else vector is resultant of BA, CA, DA
*
* @param aPoint to which substituents are added
* @param bPoint first ligand of A
* @param cPoint second ligand of A
* @param dPoint third ligand of A
* @param length A-X length
* @return Point3d nwanted points (or null if failed (coplanar))
*/
public static Point3d calculate3DCoordinates3(
Point3d aPoint, Point3d bPoint, Point3d cPoint, Point3d dPoint, double length) {
Vector3d v1 = new Vector3d(aPoint);
v1.sub(bPoint);
Vector3d v2 = new Vector3d(aPoint);
v2.sub(cPoint);
Vector3d v3 = new Vector3d(aPoint);
v3.sub(dPoint);
Vector3d v = new Vector3d(bPoint);
v.add(cPoint);
v.add(dPoint);
if (v.length() < 0.00001) {
return null;
}
v.normalize();
v.scale(length);
Point3d point = new Point3d(aPoint);
point.add(v);
return point;
}
/**
* Shades a point with the same algorithm used by the {@link <a
* href="http://surf.sourceforge.net">surf raytracer</a>}.
*
* @param hitPoint Intersection point.
* @param v View vector (from intersection point to eye).
* @param n Surface normal.
* @param material Surface material.
* @return
*/
protected Color3f shadeWithMaterial(
Point3d hitPoint,
Vector3d v,
Vector3d n,
Color3f ambientColor,
LightProducts[] lightProducts) {
Vector3d l = new Vector3d();
Vector3d h = new Vector3d();
Color3f color = new Color3f(ambientColor);
for (int i = 0; i < dcsd.lightSources.length; i++) {
LightSource lightSource = dcsd.lightSources[i];
l.sub(lightSource.getPosition(), hitPoint);
l.normalize();
float lambertTerm = (float) n.dot(l);
if (lambertTerm > 0.0f) {
// compute diffuse color component
color.scaleAdd(lambertTerm, lightProducts[i].getDiffuseProduct(), color);
// compute specular color component
h.add(l, v);
h.normalize();
color.scaleAdd(
(float)
Math.pow(Math.max(0.0f, n.dot(h)), lightProducts[i].getMaterial().getShininess()),
lightProducts[i].getSpecularProduct(),
color);
}
}
color.clampMax(1.0f);
return color;
}
/** performs the gl camera transformations for the right eye */
public void translateRightEye(GL gl, GLU glu, GLUT glut) {
// calculate the right vector
Vector3d right = new Vector3d();
Vector3d temp1 = (Vector3d) cameraTarget.clone();
temp1.scale(-1);
Vector3d temp2 = (Vector3d) cameraPosition.clone();
temp2.add(temp1);
right.cross(cameraUp, temp2);
right.normalize();
right.scale(eyespread / 2.);
glu.gluLookAt(
right.x + cameraPosition.x,
right.y + cameraPosition.y,
right.z + cameraPosition.z,
right.x + cameraTarget.x,
right.y + cameraTarget.y,
right.z + cameraTarget.z,
cameraUp.x,
cameraUp.y,
cameraUp.z);
}
public void addChargesToList2(int number) {
for (int k = 0; k < number; k++) {
Vector3d position = new Vector3d();
int count = 0;
do {
count++;
double x = (2. * Math.random() - 1.) * (plate_length / 2. - pc_radius);
double y = (2. * Math.random() - 1.) * (plate_height / 2. - pc_radius);
double z = (2. * Math.random() - 1.) * (plate_width / 2. - pc_radius);
position.set(x, y, z);
position.add(plate2_position);
} while (!isValidList2Position(position));
double charge = 0.;
if (!pcList2.isEmpty()) charge = ((PointCharge) pcList2.get(0)).getCharge();
else charge = -pc_charge;
PointCharge pc = new PointCharge();
pc.setPosition(position);
pc.setCharge(charge);
pc.setRadius(pc_radius);
pc.setMass(1.0);
pc.setID("pcList2_charge" + pcList2.size());
pc.setPickable(false);
pc.setColliding(true);
pc.setGeneratingP(false);
SphereCollisionController scc = new SphereCollisionController(pc);
scc.setRadius(pc_radius);
scc.setTolerance(0.5);
scc.setMode(SphereCollisionController.WALL_SPHERE);
pc.setCollisionController(scc);
addElement(pc);
pcList2.add(pc);
}
}
@Override
public void convertToAbsolutePosition(Vector3d point) {
M.transform(point);
point.add(origin);
}
protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
// printf("m_normalizedDirection=%f,%f,%f\n",
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
Transform start = new Transform();
Transform end = new Transform();
targetPosition.add(currentPosition, walkMove);
start.setIdentity();
end.setIdentity();
double fraction = 1.0f;
Vector3d distance2Vec = new Vector3d();
distance2Vec.sub(currentPosition, targetPosition);
double distance2 = distance2Vec.lengthSquared();
// printf("distance2=%f\n",distance2);
/*if (touchingContact) {
if (normalizedDirection.dot(touchingNormal) > 0.0f) {
updateTargetPositionBasedOnCollision(touchingNormal);
}
}*/
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0) {
start.origin.set(currentPosition);
end.origin.set(targetPosition);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(
ghostObject, upAxisDirection[upAxis], -1.0f);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
double margin = convexShape.getMargin();
convexShape.setMargin(margin + addedMargin);
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape,
start,
end,
callback,
collisionWorld.getDispatchInfo().allowedCcdPenetration);
} else {
collisionWorld.convexSweepTest(convexShape, start, end, callback);
}
convexShape.setMargin(margin);
fraction -= callback.closestHitFraction;
if (callback.hasHit()) {
// we moved only a fraction
Vector3d hitDistanceVec = new Vector3d();
hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
// double hitDistance = hitDistanceVec.length();
// if the distance is farther than the collision margin, move
// if (hitDistance > addedMargin) {
// //printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
// currentPosition.interpolate(currentPosition, targetPosition,
// callback.closestHitFraction);
// }
updateTargetPositionBasedOnCollision(callback.hitNormalWorld);
Vector3d currentDir = new Vector3d();
currentDir.sub(targetPosition, currentPosition);
distance2 = currentDir.lengthSquared();
if (distance2 > BulletGlobals.SIMD_EPSILON) {
currentDir.normalize();
// see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
// oscilations in sloping corners."
if (currentDir.dot(normalizedDirection) <= 0.0f) {
break;
}
} else {
// printf("currentDir: don't normalize a zero vector\n");
break;
}
} else {
// we moved whole way
currentPosition.set(targetPosition);
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}
public Vector3d calcEyePosition() {
Vector3d eyePosition = new Vector3d(getPosition());
eyePosition.add(calcEyeOffset());
return eyePosition;
}
