public void fillEllipsoid(Point3f center, Point3f[] points, int x, int y, int z, int diameter, Matrix3f mToEllipsoidal, double[] coef, Matrix4f mDeriv, int selectedOctant, Point3i[] octantPoints) { switch (diameter) { case 1: plotPixelClipped(argbCurrent, x, y, z); return; case 0: return; } if (diameter <= (antialiasThisFrame ? Sphere3D.maxSphereDiameter2 : Sphere3D.maxSphereDiameter)) sphere3d.render(shadesCurrent, !addAllPixels, diameter, x, y, z, mToEllipsoidal, coef, mDeriv, selectedOctant, octantPoints); }
public void setNotionalUnitCell(float[] info, Matrix3f matUnitCellOrientation, Point3f unitCellOffset) { notionalUnitCell = new float[info.length]; this.unitCellOffset = unitCellOffset; for (int i = 0; i < info.length; i++) notionalUnitCell[i] = info[i]; haveUnitCell = true; setAtomSetAuxiliaryInfo("notionalUnitcell", notionalUnitCell); setGlobalBoolean(GLOBAL_UNITCELLS); getSymmetry().setUnitCell(notionalUnitCell); // we need to set the auxiliary info as well, because // ModelLoader creates a new symmetry object. if (unitCellOffset != null){ symmetry.setUnitCellOffset(unitCellOffset); setAtomSetAuxiliaryInfo("unitCellOffset", unitCellOffset); } if (matUnitCellOrientation != null) { symmetry.setUnitCellOrientation(matUnitCellOrientation); setAtomSetAuxiliaryInfo("matUnitCellOrientation", matUnitCellOrientation); } }
public void setSymmetryInfo(int modelIndex, Map<String, Object> modelAuxiliaryInfo) { symmetryInfo = new SymmetryInfo(); float[] notionalUnitcell = symmetryInfo.setSymmetryInfo(modelIndex, modelAuxiliaryInfo); if (notionalUnitcell == null) return; setUnitCell(notionalUnitcell); modelAuxiliaryInfo.put("infoUnitCell", getUnitCellAsArray()); setUnitCellOffset((Point3f) modelAuxiliaryInfo.get("unitCellOffset")); if (modelAuxiliaryInfo.containsKey("jmolData")) setUnitCellAllFractionalRelative(true); Matrix3f matUnitCellOrientation = (Matrix3f) modelAuxiliaryInfo.get("matUnitCellOrientation"); if (matUnitCellOrientation != null) setUnitCellOrientation(matUnitCellOrientation); if (Logger.debugging) Logger .debug("symmetryInfos[" + modelIndex + "]:\n" + unitCell.dumpInfo(true)); }
public static Object unescapeMatrix(String strMatrix) { if (strMatrix == null || strMatrix.length() == 0) return strMatrix; String str = strMatrix.replace('\n', ' ').trim(); if (str.lastIndexOf("[[") != 0 || str.indexOf("]]") != str.length() - 2) return strMatrix; float[] points = new float[16]; str = str.substring(2, str.length() - 2).replace('[',' ').replace(']',' ').replace(',',' '); int[] next = new int[1]; int nPoints = 0; for (; nPoints < 16; nPoints++) { points[nPoints] = Parser.parseFloat(str, next); if (Float.isNaN(points[nPoints])) { break; } } if (!Float.isNaN(Parser.parseFloat(str, next))) return strMatrix; // overflow if (nPoints == 9) return new Matrix3f(points); if (nPoints == 16) return new Matrix4f(points); return strMatrix; }
public static Matrix3f setEllipsoidMatrix(Vector3f[] unitAxes, float[] lengths, Vector3f vTemp, Matrix3f mat) { /* * Create a matrix that transforms cartesian coordinates * into ellipsoidal coordinates, where in that system we * are drawing a sphere. * */ for (int i = 0; i < 3; i++) { vTemp.set(unitAxes[i]); vTemp.scale(lengths[i]); mat.setColumn(i, vTemp); } mat.invert(mat); return mat; }
protected void setSphereMatrix(Point3f center, float rx, float ry, float rz, AxisAngle4f a, Matrix4f sphereMatrix) { if (a != null) { Matrix3f mq = new Matrix3f(); Matrix3f m = new Matrix3f(); m.m00 = rx; m.m11 = ry; m.m22 = rz; mq.set(a); mq.mul(m); sphereMatrix.set(mq); } else { sphereMatrix.setIdentity(); sphereMatrix.m00 = rx; sphereMatrix.m11 = ry; sphereMatrix.m22 = rz; } sphereMatrix.m03 = center.x; sphereMatrix.m13 = center.y; sphereMatrix.m23 = center.z; sphereMatrix.m33 = 1; }
public void init(/*StackAlloc psa,*/ Matrix3f wrot0, Vector3f pos0, ConvexShape shape0, Matrix3f wrot1, Vector3f pos1, ConvexShape shape1, float pmargin) { pushStack(); wrotations[0].set(wrot0); positions[0].set(pos0); shapes[0] = shape0; wrotations[1].set(wrot1); positions[1].set(pos1); shapes[1] = shape1; //sa =psa; //sablock =sa->beginBlock(); margin = pmargin; failed = false; }
/** * Sets rotation matrix from euler angles. The euler angles are applied in ZYX * order. This means a vector is first rotated about X then Y and then Z axis. */ public static void setEulerZYX(Matrix3f mat, float eulerX, float eulerY, float eulerZ) { float ci = (float) Math.cos(eulerX); float cj = (float) Math.cos(eulerY); float ch = (float) Math.cos(eulerZ); float si = (float) Math.sin(eulerX); float sj = (float) Math.sin(eulerY); float sh = (float) Math.sin(eulerZ); float cc = ci * ch; float cs = ci * sh; float sc = si * ch; float ss = si * sh; mat.setRow(0, cj * ch, sj * sc - cs, sj * cc + ss); mat.setRow(1, cj * sh, sj * ss + cc, sj * cs - sc); mat.setRow(2, -sj, cj * si, cj * ci); }
public static void setRotation(Matrix3f dest, Quat4f q) { float d = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; assert (d != 0f); float s = 2f / d; float xs = q.x * s, ys = q.y * s, zs = q.z * s; float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs; float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs; float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs; dest.m00 = 1f - (yy + zz); dest.m01 = xy - wz; dest.m02 = xz + wy; dest.m10 = xy + wz; dest.m11 = 1f - (xx + zz); dest.m12 = yz - wx; dest.m20 = xz - wy; dest.m21 = yz + wx; dest.m22 = 1f - (xx + yy); }
public static void transformAabb(Vector3f halfExtents, float margin, Transform t, Vector3f aabbMinOut, Vector3f aabbMaxOut) { Vector3f halfExtentsWithMargin = Stack.alloc(Vector3f.class); halfExtentsWithMargin.x = halfExtents.x + margin; halfExtentsWithMargin.y = halfExtents.y + margin; halfExtentsWithMargin.z = halfExtents.z + margin; Matrix3f abs_b = Stack.alloc(t.basis); MatrixUtil.absolute(abs_b); Vector3f tmp = Stack.alloc(Vector3f.class); Vector3f center = Stack.alloc(t.origin); Vector3f extent = Stack.alloc(Vector3f.class); abs_b.getRow(0, tmp); extent.x = tmp.dot(halfExtentsWithMargin); abs_b.getRow(1, tmp); extent.y = tmp.dot(halfExtentsWithMargin); abs_b.getRow(2, tmp); extent.z = tmp.dot(halfExtentsWithMargin); aabbMinOut.sub(center, extent); aabbMaxOut.add(center, extent); }
/** * Constraint between two different rigidbodies. */ public void init(Matrix3f world2A, Matrix3f world2B, Vector3f rel_pos1, Vector3f rel_pos2, Vector3f jointAxis, Vector3f inertiaInvA, float massInvA, Vector3f inertiaInvB, float massInvB) { linearJointAxis.set(jointAxis); aJ.cross(rel_pos1, linearJointAxis); world2A.transform(aJ); bJ.set(linearJointAxis); bJ.negate(); bJ.cross(rel_pos2, bJ); world2B.transform(bJ); VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ); Adiag = massInvA + m_0MinvJt.dot(aJ) + massInvB + m_1MinvJt.dot(bJ); assert (Adiag > 0f); }
/** * Angular constraint between two different rigidbodies. */ public void init(Vector3f jointAxis, Matrix3f world2A, Matrix3f world2B, Vector3f inertiaInvA, Vector3f inertiaInvB) { linearJointAxis.set(0f, 0f, 0f); aJ.set(jointAxis); world2A.transform(aJ); bJ.set(jointAxis); bJ.negate(); world2B.transform(bJ); VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ); Adiag = m_0MinvJt.dot(aJ) + m_1MinvJt.dot(bJ); assert (Adiag > 0f); }
/** * Constraint on one rigidbody. */ public void init( Matrix3f world2A, Vector3f rel_pos1, Vector3f rel_pos2, Vector3f jointAxis, Vector3f inertiaInvA, float massInvA) { linearJointAxis.set(jointAxis); aJ.cross(rel_pos1, jointAxis); world2A.transform(aJ); bJ.set(jointAxis); bJ.negate(); bJ.cross(rel_pos2, bJ); world2A.transform(bJ); VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); m_1MinvJt.set(0f, 0f, 0f); Adiag = massInvA + m_0MinvJt.dot(aJ); assert (Adiag > 0f); }
protected void buildLinearJacobian(/*JacobianEntry jacLinear*/int jacLinear_index, Vector3f normalWorld, Vector3f pivotAInW, Vector3f pivotBInW) { Matrix3f mat1 = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis; mat1.transpose(); Matrix3f mat2 = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis; mat2.transpose(); Vector3f tmpVec = Stack.alloc(Vector3f.class); Vector3f tmp1 = Stack.alloc(Vector3f.class); tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec)); Vector3f tmp2 = Stack.alloc(Vector3f.class); tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec)); jacLinear[jacLinear_index].init( mat1, mat2, tmp1, tmp2, normalWorld, rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)), rbB.getInvMass()); }
/** * Returns <code>true</code> if the rotation matrix matches only rotations of * a multiple of 90� degrees around x, y or z axis. */ private boolean isOrthogonalRotation(Transform3D transformation) { Matrix3f matrix = new Matrix3f(); transformation.get(matrix); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { // Return false if the matrix contains a value different from 0 1 or -1 if (Math.abs(matrix.getElement(i, j)) > 1E-6 && Math.abs(matrix.getElement(i, j) - 1) > 1E-6 && Math.abs(matrix.getElement(i, j) + 1) > 1E-6) { return false; } } } return true; }
@Override public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { Vector3f halfExtents = getHalfExtentsWithoutMargin(Stack.alloc(Vector3f.class)); Matrix3f abs_b = Stack.alloc(t.basis); MatrixUtil.absolute(abs_b); Vector3f tmp = Stack.alloc(Vector3f.class); Vector3f center = Stack.alloc(t.origin); Vector3f extent = Stack.alloc(Vector3f.class); abs_b.getRow(0, tmp); extent.x = tmp.dot(halfExtents); abs_b.getRow(1, tmp); extent.y = tmp.dot(halfExtents); abs_b.getRow(2, tmp); extent.z = tmp.dot(halfExtents); Vector3f margin = Stack.alloc(Vector3f.class); margin.set(getMargin(), getMargin(), getMargin()); extent.add(margin); aabbMin.sub(center, extent); aabbMax.add(center, extent); }
private void updateConeTransforms(ProGAL.geom3d.volumes.Cone c){ Transform3D trans = new Transform3D(); Vector v1 = new Vector(0,-1,0); Vector v2 = c.getAxis();//c.p1.vectorTo(c.p2); if(v2.length()>0.000001 && v1.angle(v2)>0.00001) { //Matrix m = Matrix.createRotationMatrix(v1.angle(v2), v1.cross(v2).normIn()); //trans.set(m.getCoordArray()); Vector v = v1.cross(v2).normalizeThis(); Matrix m4 = Matrix.createRotationMatrix(v1.angle(v2), v); // trans.set(to4x4CoordArray(m4)); trans.setRotation(new Matrix3f(to3x3CoordArray(m4))); } trans.setScale(new javax.vecmath.Vector3d(c.getBaseRadius(), c.getAxisLength(), c.getBaseRadius())); trans.setTranslation(toJ3DVec(c.getCenter().toVector())); ((TransformGroup)shapeTransforms.get(c).getChild(0)).setTransform(trans); }
/** get the rotation out of Jmol * * @return the jmol rotation matrix */ public Matrix getJmolRotation(){ Matrix jmolRotation = Matrix.identity(3, 3); //structurePanel.executeCmd("show orientation;"); JmolViewer jmol = getViewer(); Object obj = jmol.getProperty(null,"transformInfo",""); // System.out.println(obj); if ( obj instanceof Matrix3f ) { Matrix3f max = (Matrix3f) obj; jmolRotation = new Matrix(3,3); for (int x=0; x<3;x++) { for (int y=0 ; y<3;y++){ float val = max.getElement(x,y); // System.out.println("x " + x + " y " + y + " " + val); jmolRotation.set(x,y,val); } } } return jmolRotation; }
public static void matrixToFloatBuffer(Matrix3f m, FloatBuffer fb) { Matrix3f tempMatrix = new Matrix3f(); tempMatrix.transpose(m); fb.put(tempMatrix.m00); fb.put(tempMatrix.m01); fb.put(tempMatrix.m02); fb.put(tempMatrix.m10); fb.put(tempMatrix.m11); fb.put(tempMatrix.m12); fb.put(tempMatrix.m20); fb.put(tempMatrix.m21); fb.put(tempMatrix.m22); fb.flip(); }
public BulletPhysics(WorldProvider world) { collisionGroupManager = CoreRegistry.get(CollisionGroupManager.class); _broadphase = new DbvtBroadphase(); _broadphase.getOverlappingPairCache().setInternalGhostPairCallback(new GhostPairCallback()); _defaultCollisionConfiguration = new DefaultCollisionConfiguration(); _dispatcher = new CollisionDispatcher(_defaultCollisionConfiguration); _sequentialImpulseConstraintSolver = new SequentialImpulseConstraintSolver(); _discreteDynamicsWorld = new DiscreteDynamicsWorld(_dispatcher, _broadphase, _sequentialImpulseConstraintSolver, _defaultCollisionConfiguration); _discreteDynamicsWorld.setGravity(new Vector3f(0f, -15f, 0f)); blockEntityRegistry = CoreRegistry.get(BlockEntityRegistry.class); CoreRegistry.get(EventSystem.class).registerEventReceiver(this, BlockChangedEvent.class, BlockComponent.class); PhysicsWorldWrapper wrapper = new PhysicsWorldWrapper(world); VoxelWorldShape worldShape = new VoxelWorldShape(wrapper); Matrix3f rot = new Matrix3f(); rot.setIdentity(); DefaultMotionState blockMotionState = new DefaultMotionState(new Transform(new Matrix4f(rot, new Vector3f(0, 0, 0), 1.0f))); RigidBodyConstructionInfo blockConsInf = new RigidBodyConstructionInfo(0, blockMotionState, worldShape, new Vector3f()); RigidBody rigidBody = new RigidBody(blockConsInf); rigidBody.setCollisionFlags(CollisionFlags.STATIC_OBJECT | rigidBody.getCollisionFlags()); _discreteDynamicsWorld.addRigidBody(rigidBody, combineGroups(StandardCollisionGroup.WORLD), (short)(CollisionFilterGroups.ALL_FILTER ^ CollisionFilterGroups.STATIC_FILTER)); }
private static Matrix3f roundAndReduceMatrixElements(Matrix4f m) { Preconditions.checkArgument(m.m30 == 0); Preconditions.checkArgument(m.m31 == 0); Preconditions.checkArgument(m.m32 == 0); Preconditions.checkArgument(m.m03 == 0); Preconditions.checkArgument(m.m13 == 0); Preconditions.checkArgument(m.m23 == 0); Preconditions.checkArgument(m.m33 == 1); final Matrix3f result = new Matrix3f(); result.m00 = Math.round(m.m00); result.m01 = Math.round(m.m01); result.m02 = Math.round(m.m02); result.m10 = Math.round(m.m10); result.m11 = Math.round(m.m11); result.m12 = Math.round(m.m12); result.m20 = Math.round(m.m20); result.m21 = Math.round(m.m21); result.m22 = Math.round(m.m22); return result; }
/** * BodyのWorld座標系での回転行列(pitch, rollのみ)を返します. * * @param context * @param mat */ private void calculateBodyRotation(SomaticContext context, Matrix3f mat) { // FIXME 未テスト Matrix3f rot; if (context.isLeftOnGround()) rot = context.get(Frames.LSole).getBodyRotation(); else if (context.isRightOnGround()) rot = context.get(Frames.RSole).getBodyRotation(); else { mat.setIdentity(); return; } Vector3f tmp = new Vector3f(); MatrixUtils.rot2pyr(rot, tmp); tmp.y = 0; MatrixUtils.pyr2rot(tmp, mat); }
public SomaticContext(SomaticContext state) { frames = new EnumMap<Frames, FrameState>(Frames.class); for (Frames frame : state.frames.keySet()) frames.put(frame, state.frames.get(frame).clone()); robot = state.robot; com = new Vector3f(state.com); bodyPosture = new Matrix3f(state.bodyPosture); bodyHeight = state.bodyHeight; confidence = state.confidence; time = state.time; leftOnGround = state.leftOnGround; rightOnGround = state.rightOnGround; leftPressure = state.leftPressure; rightPressure = state.rightPressure; leftCOP = new Point2f(state.leftCOP); rightCOP = new Point2f(state.rightCOP); cop = new Point2f(state.cop); }
/** * 重心位置の計算. 再帰呼び出し用. * * @param ss */ private static void calcCenterOfMassRecursively(SomaticContext ss, Frames id, Vector3f com) { log.trace("calculate CoM recursively"); RobotFrame rf = ss.getRobot().get(id); FrameState fs = ss.get(id); Vector3f frameCom = fs.getBodyCenterOfMass(); Matrix3f mat = fs.getBodyRotation(); mat.transform(rf.getCenterOfMass(), frameCom); frameCom.add(fs.getBodyPosition()); com.scaleAdd(rf.getMass(), frameCom, com); // 子フレームがあれば再帰的に計算する if (rf.getChildren() != null) for (RobotFrame child : rf.getChildren()) calcCenterOfMassRecursively(ss, child.getId(), com); }
/** * ZYXオイラー角(Roll-Pitch-Yaw, ロール-ピッチ-ヨー) pyrから回転行列を求め、rotに格納します. * * ただしzをRoll, yをYaw, xをPitchとします. * * @param ypr * @param rot */ public static void pyr2rot(Vector3f ypr, Matrix3f rot) { float sRoll = (float) Math.sin(ypr.x); // ロール(yaw) float cRoll = (float) Math.cos(ypr.x); float sPitch = (float) Math.sin(ypr.y); float cPitch = (float) Math.cos(ypr.y); float sYaw = (float) Math.sin(ypr.z); float cYaw = (float) Math.cos(ypr.z); rot.m00 = cYaw * cPitch; rot.m01 = -sYaw * cRoll + cYaw * sPitch * sRoll; rot.m02 = sYaw * sRoll + cYaw * sPitch * cRoll; rot.m10 = sYaw * cPitch; rot.m11 = cYaw * cRoll + sYaw * sPitch * sRoll; rot.m12 = -cYaw * sRoll + sYaw * sPitch * cRoll; rot.m20 = -sPitch; rot.m21 = cPitch * sRoll; rot.m22 = cPitch * cRoll; }
